#include "DSP281x_Examples.h" // DSP281x Examples Include File
#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File
#include "DSP281x_Device.h" // DSP281x Headerfile Include File
#include "IQmathLib.h"
#include <params_bsu.h>
#include <v_rotor.h>
#include "filter_v1.h"
#include "xp_cds_in.h"
#include "xp_inc_sensor.h"
#include "xp_project.h"
#include "params.h"
#include "pwm_test_lines.h"
#include "params_norma.h"
#include "mathlib.h"
#include "params_alg.h"
#pragma DATA_SECTION(WRotor,".fast_vars");
WRotorValues WRotor = WRotorValues_DEFAULTS;
#if (SENSOR_ALG==SENSOR_ALG_23550)
#pragma DATA_SECTION(WRotorPBus,".slow_vars");
WRotorValuesAngle WRotorPBus = WRotorValuesAngle_DEFAULTS;
#pragma DATA_SECTION(rotor_error_update_count,".fast_vars");
unsigned int rotor_error_update_count = 0;
#define SIZE_BUF_SENSOR_LOGS 32
#pragma DATA_SECTION(sensor_1_zero,".slow_vars");
unsigned int sensor_1_zero[6+4+8][SIZE_BUF_SENSOR_LOGS], count_sensor_1_zero=0;
#endif
_iq koefW = _IQ(0.05); //0.05
_iq koefW2 = _IQ(0.01); //0.05
_iq koefW3 = _IQ(0.002); //0.05
#if (SENSOR_ALG==SENSOR_ALG_23550)
///////////////////////////////////////////////////////////////
void rotorInit(void)
{
WRotorPBus.ModeAutoDiscret = 1;
}
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
#define MAX_COUNT_OVERFULL_DISCRET 2250
#define MAX_DIRECTION 4000
#define MAX_DIRECTION_2 2000
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
void RotorDirectionFilter(int RotorDirectionIn, int *RotorDirectionOut, int *RotorDirectionOut2, int *count_direction)
{
// static int count_direction = 0;
// static int count_direction_minus = 0;
if (RotorDirectionIn==0)
{
if (*count_direction>0) (*count_direction)--;
if (*count_direction<0) (*count_direction)++;
// if (count_direction_minus>0) count_direction_minus--;
}
else
if (RotorDirectionIn>0)
{
if (*count_direction<MAX_DIRECTION) (*count_direction)++;
// if (count_direction_minus>0) count_direction_minus--;
}
else
{
if (*count_direction>-MAX_DIRECTION) (*count_direction)--;
// if (count_direction_plus>0) count_direction_plus--;
}
if (RotorDirectionIn==0)
*RotorDirectionOut = 0;
else
if (RotorDirectionIn>0)
*RotorDirectionOut = 1;
else
*RotorDirectionOut = -1;
if (*count_direction>MAX_DIRECTION_2)
*RotorDirectionOut2 = 1;
else
if (*count_direction<-MAX_DIRECTION_2)
*RotorDirectionOut2 = -1;
else
*RotorDirectionOut2 = 0;
}
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
#define LEVEL_VALUE_SENSOR_OVERFULL 65535
#define MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS 4000
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
#pragma CODE_SECTION(AnalisatorRotorSensorPBus,".fast_run");
int AnalisatorRotorSensorPBus(_iq d1, _iq d2, unsigned int *count_overfull_discret, unsigned int *count_zero_discret, _iq *prev_iqTimeRotor,
unsigned int *discret_out, unsigned int discret_in, _iq *iqWRotorCalcBeforeRegul, _iq *iqWRotorCalc,
int modeS1, int modeS2,
int valid_sensor_direct, int valid_sensor_90,
unsigned int *error_count )
{
int flag_not_ready_rotor, flag_overfull_rotor;
_iq iqTimeRotor;
// discret0 = 2 mks
// static long long KoefNorm_discret0 = 409600000LL;//((500 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNorm_discret0 = 102400000LL;//((500 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
// discret1 = 20 ns
// static long long KoefNorm_discret1 = 40960000000LL;//((50 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNorm_discret1 = 10240000000LL;//((50 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
// _iq iqWRotorSumm;//,iqWRotorCalc;
static _iq time_level_discret_1to0 = 60000 ;//682666; // KoefNorm_discret1/60000 = 0.813801288604736328125 ��.
static _iq time_level_discret_0to1 = 400;//204800; // KoefNorm_discret0/2000 = 0.244140625 ��.
static unsigned int discret;
if (valid_sensor_direct == 0)
d1 = 0;
if (valid_sensor_90 == 0)
d2 = 0;
// ��� ���-�� ����� �� ���, ���� ����� ������������� �� ����� �������.
if (valid_sensor_direct == 0 && valid_sensor_90 == 0)
{
if (*error_count<MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS)
{
(*error_count)++;
return 0;
}
else
return 1; // ������!!! � �� ����� == MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS!!!
}
if (valid_sensor_direct == 1 && valid_sensor_90 == 0)
{
modeS2 = modeS1;
}
if (valid_sensor_direct == 0 && valid_sensor_90 == 1)
{
modeS1 = modeS2;
}
if (modeS1 == modeS2)
{
discret = modeS1;
*error_count = 0;
}
else
{
discret = 0;
if (*error_count<MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS)
{
(*error_count)++;
return 0;
}
else
return 1; // ������!!! � �� ����� == MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS!!!
}
// ��� ��� ������ ���, ������� ��������
*error_count = 0;
flag_not_ready_rotor = 0;
flag_overfull_rotor = 0;
if (d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0 && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
{
// ��� ����������
}
else
if (d1 == 0 && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
{
// d1 - �����, d2 ���� ����������
d1 = d2;
}
else
if (d1 == LEVEL_VALUE_SENSOR_OVERFULL && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
{
// d1 - �����, d2 ���� ����������
d1 = d2;
}
else
if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0)
{
// d2 - �����, d1 ���� ����������
d2 = d1;
}
else
if (d2 == 0 && d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0)
{
// d2 - �����, d1 ���� ����������
d2 = d1;
}
else
if (d1 == 0 && d2 == 0)
{
flag_not_ready_rotor = 1;
}
else
if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 == LEVEL_VALUE_SENSOR_OVERFULL)
{
flag_overfull_rotor = 1;
d1 = d2 = 0;
}
else
if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 == 0)
{
flag_overfull_rotor = 1;
d1 = d2 = 0;
}
else
if (d1 == LEVEL_VALUE_SENSOR_OVERFULL && d2 == 0)
{
flag_overfull_rotor = 1;
d1 = d2 = 0;
}
iqTimeRotor = (d1+d2)>>1;
// max OVERFULL
if (flag_overfull_rotor)
{
if (*count_overfull_discret<MAX_COUNT_OVERFULL_DISCRET)
(*count_overfull_discret)++;
}
else
{
if (*count_overfull_discret>0)
(*count_overfull_discret)--;
}
// zero?
if (flag_not_ready_rotor)
{
if (*count_zero_discret<MAX_COUNT_OVERFULL_DISCRET)
(*count_zero_discret)++;
}
else
{
if (*count_zero_discret>0)
(*count_zero_discret)--;
}
// real zero?
if (*count_zero_discret==MAX_COUNT_OVERFULL_DISCRET)
{
// ���� ��� ������� �����, ������ ����� ����!
iqWRotorCalc = 0;
*prev_iqTimeRotor = 0;
iqTimeRotor = 0;
}
else
{
// ���� ��� �� ������� �����, ������ ����� ������ �������� prev_iqTimeRotor
if (iqTimeRotor==0)
iqTimeRotor = *prev_iqTimeRotor;
}
*prev_iqTimeRotor = iqTimeRotor;
// ����� ������� ���������
if (WRotorPBus.ModeAutoDiscret==1)
{
if ( (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) || (iqTimeRotor==0) )
{
// ��� ��� ������������ ��������� ��� ����� ������������, �������=0
// ����� �������� discret_out = 0
if (discret_in == 1) // ��� ��� ���� ����������� discret?
{
// discret ��� =1, ����������� �� 0.
*discret_out = 0;
*count_overfull_discret = 0; // ���� ��� ���� ����!
}
}
else
{
// �����. ������� discret==0 �����...
if (discret==0 && iqTimeRotor<time_level_discret_0to1 && iqTimeRotor!=65535)
*discret_out = 1;
// �����. ������� discret==1 �����...
if (discret==1 && iqTimeRotor>time_level_discret_1to0 && iqTimeRotor!=65535)
*discret_out = 0;
}
}
if (WRotorPBus.ModeAutoDiscret==2)
{
*discret_out = 0;
}
if (WRotorPBus.ModeAutoDiscret==3)
{
*discret_out = 1;
}
if ( (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) )
{
// ��� ��� ����� � 0, �.�. ������� �������� ���� ��������!
*prev_iqTimeRotor = iqTimeRotor = 0;
}
if ((iqTimeRotor != 0)) // && (WRotorPBus.iqTimeRotor<65535)
{
if (discret==0)
*iqWRotorCalcBeforeRegul = KoefNorm_discret0 / iqTimeRotor;
if (discret==1)
*iqWRotorCalcBeforeRegul = KoefNorm_discret1 / iqTimeRotor;
*iqWRotorCalc = exp_regul_iq(koefW, *iqWRotorCalc, *iqWRotorCalcBeforeRegul);
}
else
{
*iqWRotorCalc = 0;
*iqWRotorCalcBeforeRegul = 0;
}
// if (*iqWRotorCalc == 0)
// *RotorDirection = 0;
return 0;
}
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
#pragma CODE_SECTION(RotorMeasurePBus,".fast_run");
void RotorMeasurePBus(void)
{
// discret0 = 2 mks
// static long long KoefNorm_discret0 = 409600000LL;//((500 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNorm_discret0 = 102400000LL;//((500 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
// discret1 = 20 ns
// static long long KoefNorm_discret1 = 40960000000LL;//((50 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNorm_discret1 = 10240000000LL;//((50 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static _iq time_level_discret_1to0 = 60000 ;//682666; // KoefNorm_discret1/60000 = 0.813801288604736328125 ��.
static _iq time_level_discret_0to1 = 400;//204800; // KoefNorm_discret0/2000 = 0.244140625 ��.
static long long KoefNorm_angle = 16384LL; //2^24/1024
// volatile float MyVar0 = 0;
unsigned int MyVar3 = 0;
// int direction1 = 0, direction2 = 0;
volatile unsigned int discret;
static unsigned int discret_out1, discret_out2;
static int count_full_oborots = 0;
static unsigned int count_overfull_discret1 = 0;
static unsigned int count_zero_discret1 = 0;
static unsigned int count_overfull_discret2 = 0;
static unsigned int count_zero_discret2 = 0;
static unsigned int count_discret_to_1 = 0;
static unsigned int count_discret_to_0 = 0;
static unsigned int c_error_pbus_1 = 0;
static unsigned int c_error_pbus_2 = 0;
static _iq prev_iqTimeRotor1 = 0, prev_iqTimeRotor2 = 0;
_iq iqWRotorSumm = 0;
int flag_not_ready_rotor1, flag_overfull_rotor1;
int flag_not_ready_rotor2, flag_overfull_rotor2;
//i_led1_on_off(1);
flag_not_ready_rotor1 = 0;
flag_overfull_rotor1 = 0;
flag_not_ready_rotor2 = 0;
flag_overfull_rotor2 = 0;
discret = project.cds_in[0].read.sbus.enabled_channels.bit.discret;
if (project.cds_in[0].read.sbus.enabled_channels.bit.discret != project.cds_in[0].write.sbus.enabled_channels.bit.discret)
discret = 2;
if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6)
{
sensor_1_zero[0][count_sensor_1_zero] = project.cds_in[0].read.pbus.Time_since_zero_point_S1;
sensor_1_zero[1][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S1;
sensor_1_zero[2][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S1;
sensor_1_zero[3][count_sensor_1_zero] = project.cds_in[0].read.pbus.Time_since_zero_point_S2;
sensor_1_zero[4][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S2;
sensor_1_zero[5][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S2;
}
sensor_1_zero[6][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS1_cnt;
sensor_1_zero[7][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS1_cnt90;
sensor_1_zero[8][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS2_cnt;
sensor_1_zero[9][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS2_cnt90;
sensor_1_zero[10][count_sensor_1_zero] = inc_sensor.data.Time1;
sensor_1_zero[11][count_sensor_1_zero] = inc_sensor.data.Impulses1;
sensor_1_zero[12][count_sensor_1_zero] = inc_sensor.data.CountZero1;
sensor_1_zero[13][count_sensor_1_zero] = inc_sensor.data.CountOne1;
sensor_1_zero[14][count_sensor_1_zero] = inc_sensor.data.Time2;
sensor_1_zero[15][count_sensor_1_zero] = inc_sensor.data.Impulses2;
sensor_1_zero[16][count_sensor_1_zero] = inc_sensor.data.CountZero2;
sensor_1_zero[17][count_sensor_1_zero] = inc_sensor.data.CountOne2;
count_sensor_1_zero++;
if (count_sensor_1_zero>=SIZE_BUF_SENSOR_LOGS)
{
count_sensor_1_zero = 0;
count_full_oborots++;
if (count_full_oborots>3)
count_full_oborots = 0;
}
/*
if (count_sensor_1_zero==904)
{
discret = 3;
}
*/
#if (ENABLE_ROTOR_SENSOR_ZERO_SIGNAL==1)
if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6)
{
#if (ENABLE_ROTOR_SENSOR_1_PBUS==1)
WRotorPBus.iqWRotorRawAngle1F = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S1-32768;
WRotorPBus.iqWRotorRawAngle1R = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S1-32768;
WRotorPBus.iqAngle1F = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle1F;
WRotorPBus.iqAngle1R = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle1R;
#else
WRotorPBus.iqWRotorRawAngle1F = 0;
WRotorPBus.iqWRotorRawAngle1R = 0;
WRotorPBus.iqAngle1F = 0;
WRotorPBus.iqAngle1R = 0;
#endif
#if (ENABLE_ROTOR_SENSOR_2_PBUS==1)
WRotorPBus.iqWRotorRawAngle2F = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S2-32768;
WRotorPBus.iqWRotorRawAngle2R = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S2-32768;
WRotorPBus.iqAngle2F = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle2F;
WRotorPBus.iqAngle2R = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle2R;
#else
WRotorPBus.iqWRotorRawAngle2F = 0;
WRotorPBus.iqWRotorRawAngle2R = 0;
WRotorPBus.iqAngle2F = 0;
WRotorPBus.iqAngle2R = 0;
#endif
}
else
{
WRotorPBus.iqWRotorRawAngle1F = 0;
WRotorPBus.iqWRotorRawAngle1R = 0;
WRotorPBus.iqAngle1F = 0;
WRotorPBus.iqAngle1R = 0;
WRotorPBus.iqWRotorRawAngle2F = 0;
WRotorPBus.iqWRotorRawAngle2R = 0;
WRotorPBus.iqAngle2F = 0;
WRotorPBus.iqAngle2R = 0;
}
#endif
#if (ENABLE_ROTOR_SENSOR_1_PBUS==1)
//**************************************************************************************************
MyVar3 = project.cds_in[0].read.pbus.SpeedS1_cnt;
if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotorPBus.iqWRotorRaw0 = MyVar3;
}
else
{
WRotorPBus.iqWRotorRaw0 = 0;
}
MyVar3 = project.cds_in[0].read.pbus.SpeedS1_cnt90;
if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotorPBus.iqWRotorRaw1 = MyVar3;
}
else
{
WRotorPBus.iqWRotorRaw1 = 0;
}
#else
WRotorPBus.iqWRotorRaw0 = 0;
WRotorPBus.iqWRotorRaw1 = 0;
#endif
#if (ENABLE_ROTOR_SENSOR_2_PBUS==1)
//***************************************************************************************************
MyVar3 = project.cds_in[0].read.pbus.SpeedS2_cnt;
if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotorPBus.iqWRotorRaw2 = MyVar3;
}
else
{
WRotorPBus.iqWRotorRaw2 = 0;
}
MyVar3 = project.cds_in[0].read.pbus.SpeedS2_cnt90;
if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotorPBus.iqWRotorRaw3 = MyVar3;
}
else
{
WRotorPBus.iqWRotorRaw3 = 0;
}
#else
WRotorPBus.iqWRotorRaw2 = 0;
WRotorPBus.iqWRotorRaw3 = 0;
#endif
#if (ENABLE_ROTOR_SENSOR_1_PBUS==1)
// if (project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_direct && project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_90 )
AnalisatorRotorSensorPBus(WRotorPBus.iqWRotorRaw0, WRotorPBus.iqWRotorRaw1, &count_overfull_discret1, &count_zero_discret1,
&prev_iqTimeRotor1, &discret_out1, project.cds_in[0].read.sbus.enabled_channels.bit.discret,
&WRotorPBus.iqWRotorCalcBeforeRegul1, &WRotorPBus.iqWRotorCalc1,
project.cds_in[0].read.pbus.direction_in.bit.mode_sensor1_direct, project.cds_in[0].read.pbus.direction_in.bit.mode_sensor1_90,
project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_direct, project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_90,
&c_error_pbus_1 );
#endif
#if (ENABLE_ROTOR_SENSOR_2_PBUS==1)
// if (project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_direct && project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_90 )
AnalisatorRotorSensorPBus(WRotorPBus.iqWRotorRaw2, WRotorPBus.iqWRotorRaw3, &count_overfull_discret2, &count_zero_discret2,
&prev_iqTimeRotor2, &discret_out2, project.cds_in[0].read.sbus.enabled_channels.bit.discret,
&WRotorPBus.iqWRotorCalcBeforeRegul2, &WRotorPBus.iqWRotorCalc2,
project.cds_in[0].read.pbus.direction_in.bit.mode_sensor2_direct, project.cds_in[0].read.pbus.direction_in.bit.mode_sensor2_90,
project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_direct, project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_90,
&c_error_pbus_2);
#endif
// RotorDirectionFilter(WRotorPBus.RotorDirectionInstant, &WRotorPBus.RotorDirectionSlow);
if (discret_out1==1 || discret_out2==1)
{
project.cds_in[0].write.sbus.enabled_channels.bit.discret = 1;
count_discret_to_1++;
}
else
{
project.cds_in[0].write.sbus.enabled_channels.bit.discret = 0;
count_discret_to_0++;
}
}
#define MAX_COUNT_OVERFULL_DISCRET_2 150
#pragma CODE_SECTION(RotorMeasure,".fast_run");
void RotorMeasure(void)
{
// 600 Khz clock on every edge
// static long long KoefNorm = 53635601LL;//((600 000/6256/NORMA_WROTOR/2) * ((long)2 << 24)); //15 - NormaWRotor 782*8 = 6256
// static long long KoefNormMS = 491520000LL;//((600 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
// static long long KoefNormNS = 49152000000LL;//((60 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNormMS = 122880000LL;//((600 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNormNS = 12288000000LL;//((60 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
static long long KoefNormImpulses = 838860800000000LL;// (2^24 * 1000000000 / (Impulses(ns)) / NORMA_WROTOR
static _iq max_value_rotor = _IQ(500.0/60.0/NORMA_FROTOR);
static _iq wrotor_add_ramp = _IQ(0.001/NORMA_FROTOR);
// volatile float MyVar0 = 0;
// volatile unsigned int MyVar1 = 0;
// volatile unsigned int MyVar2 = 0;
unsigned int MyVar3;
inc_sensor.read_sensors(&inc_sensor);
// flag_not_ready_rotor = 0;
//**************************************************************************************************
// sensor 1
if (inc_sensor.use_sensor1)
{
MyVar3 = inc_sensor.data.CountOne1;
// MyVar3 = (unsigned long) rotation_sensor.in_plane.out.CountOne1;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_21_ON;
#endif
WRotor.iqWRotorRaw0 = MyVar3;
}
else
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_21_OFF;
#endif
WRotor.iqWRotorRaw0 = 0;
}
MyVar3 = inc_sensor.data.CountZero1;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_22_ON;
#endif
WRotor.iqWRotorRaw1 = MyVar3;
}
else
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_22_OFF;
#endif
WRotor.iqWRotorRaw1 = 0;
}
}
else
{
WRotor.iqWRotorRaw0 = 0;
WRotor.iqWRotorRaw1 = 0;
}
//logpar.uns_log0 = (Uint16)(my_var1);
//logpar.uns_log1 = (Uint16)(my_var2);
// sensor 2
if (inc_sensor.use_sensor2)
{
MyVar3 = inc_sensor.data.CountOne2;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_18_ON;
#endif
WRotor.iqWRotorRaw2 = MyVar3;
}
else
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_18_OFF;
#endif
WRotor.iqWRotorRaw2 = 0;
}
MyVar3 = inc_sensor.data.CountZero2;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_23_ON;
#endif
WRotor.iqWRotorRaw3 = MyVar3;
}
else
{
#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR)
PWM_LINES_TK_23_OFF;
#endif
WRotor.iqWRotorRaw3 = 0;
}
}
else
{
WRotor.iqWRotorRaw2 = 0;
WRotor.iqWRotorRaw3 = 0;
}
// if (WRotor.iqWRotorRaw0==0 && WRotor.iqWRotorRaw1==0 && WRotor.iqWRotorRaw2==0 && WRotor.iqWRotorRaw3==0)
// flag_not_ready_rotor = 1;
if (WRotor.iqWRotorRaw0==0)
{
if (WRotor.count_zero_discret0==MAX_COUNT_OVERFULL_DISCRET_2)
{
WRotor.prev_iqWRotorRaw0 = WRotor.iqWRotorRaw0 = 0;
}
else
{
WRotor.iqWRotorRaw0 = WRotor.prev_iqWRotorRaw0;
WRotor.count_zero_discret0++;
}
}
else
{
WRotor.count_zero_discret0 = 0;
WRotor.prev_iqWRotorRaw0 = WRotor.iqWRotorRaw0;
}
if (WRotor.iqWRotorRaw1==0)
{
if (WRotor.count_zero_discret1==MAX_COUNT_OVERFULL_DISCRET_2)
{
WRotor.prev_iqWRotorRaw1 = WRotor.iqWRotorRaw1 = 0;
}
else
{
WRotor.iqWRotorRaw1 = WRotor.prev_iqWRotorRaw1;
WRotor.count_zero_discret1++;
}
}
else
{
WRotor.count_zero_discret1 = 0;
WRotor.prev_iqWRotorRaw1 = WRotor.iqWRotorRaw1;
}
if (WRotor.iqWRotorRaw2==0)
{
if (WRotor.count_zero_discret2==MAX_COUNT_OVERFULL_DISCRET_2)
{
WRotor.prev_iqWRotorRaw2 = WRotor.iqWRotorRaw2 = 0;
}
else
{
WRotor.iqWRotorRaw2 = WRotor.prev_iqWRotorRaw2;
WRotor.count_zero_discret2++;
}
}
else
{
WRotor.count_zero_discret2 = 0;
WRotor.prev_iqWRotorRaw2 = WRotor.iqWRotorRaw2;
}
if (WRotor.iqWRotorRaw3==0)
{
if (WRotor.count_zero_discret3==MAX_COUNT_OVERFULL_DISCRET_2)
{
WRotor.prev_iqWRotorRaw3 = WRotor.iqWRotorRaw3 = 0;
}
else
{
WRotor.iqWRotorRaw3 = WRotor.prev_iqWRotorRaw3;
WRotor.count_zero_discret3++;
}
}
else
{
WRotor.count_zero_discret3 = 0;
WRotor.prev_iqWRotorRaw3 = WRotor.iqWRotorRaw3;
}
WRotor.iqTimeSensor1 = WRotor.iqWRotorRaw0 + WRotor.iqWRotorRaw1;
WRotor.iqTimeSensor2 = WRotor.iqWRotorRaw2 + WRotor.iqWRotorRaw3;
//
// // zero?
// if (flag_not_ready_rotor)
// {
// if (*count_zero_discret<MAX_COUNT_OVERFULL_DISCRET)
// (*count_zero_discret)++;
// }
// else
// {
// if (*count_zero_discret>0)
// (*count_zero_discret)--;
// }
//
// // real zero?
// if (count_zero_discret==MAX_COUNT_OVERFULL_DISCRET)
// {
// // ���� ��� ������� �����, ������ ����� ����!
// WRotor.iqTimeSensor1 = 0;
// WRotor.prev_iqTimeSensor1 = 0;
// }
// else
// {
// // ���� ��� �� ������� �����, ������ ����� ������ �������� prev_iqTimeRotor
// if (WRotor.iqTimeSensor1==0)
// WRotor.iqTimeSensor1 = WRotor.prev_iqTimeSensor1;
// }
// WRotor.prev_iqTimeSensor1 = WRotor.iqTimeSensor1;
//
//
// // max OVERFULL
// if (flag_overfull_rotor)
// {
// if (*count_overfull_discret<MAX_COUNT_OVERFULL_DISCRET)
// (*count_overfull_discret)++;
// }
// else
// {
// if (*count_overfull_discret>0)
// (*count_overfull_discret)--;
// }
//
// // zero?
// if (flag_not_ready_rotor)
// {
// if (*count_zero_discret<MAX_COUNT_OVERFULL_DISCRET)
// (*count_zero_discret)++;
// }
// else
// {
// if (*count_zero_discret>0)
// (*count_zero_discret)--;
// }
//
// // real zero?
// if (*count_zero_discret==MAX_COUNT_OVERFULL_DISCRET)
// {
// // ���� ��� ������� �����, ������ ����� ����!
// iqWRotorCalc = 0;
// *prev_iqTimeRotor = 0;
// iqTimeRotor = 0;
// }
// else
// {
// // ���� ��� �� ������� �����, ������ ����� ������ �������� prev_iqTimeRotor
// if (iqTimeRotor==0)
// iqTimeRotor = *prev_iqTimeRotor;
// }
// *prev_iqTimeRotor = iqTimeRotor;
//
//
//
///
if (WRotor.iqTimeSensor1 != 0 && inc_sensor.use_sensor1)
{
if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time1==0)
WRotor.iqWRotorCalcBeforeRegul1 = KoefNormMS / WRotor.iqTimeSensor1;
if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time1==1)
WRotor.iqWRotorCalcBeforeRegul1 = KoefNormNS / WRotor.iqTimeSensor1;
if (WRotor.iqWRotorCalcBeforeRegul1 > max_value_rotor)
{
WRotor.iqWRotorCalc1 = 0;
WRotor.iqWRotorCalcBeforeRegul1 = 0;
}
else
WRotor.iqWRotorCalc1 = exp_regul_iq(koefW, WRotor.iqWRotorCalc1, WRotor.iqWRotorCalcBeforeRegul1);
/////
if (WRotor.iqWRotorCalc1)
{
if (WRotor.iqPrevWRotorCalc1 != WRotor.iqWRotorCalc1)
{
WRotor.iqWRotorCalc1Ramp = zad_intensiv_q(wrotor_add_ramp, wrotor_add_ramp, WRotor.iqWRotorCalc1Ramp, WRotor.iqWRotorCalc1);
WRotor.iqPrevWRotorCalc1 = WRotor.iqWRotorCalc1;
}
}
else
{
WRotor.iqPrevWRotorCalc1 = 0;
WRotor.iqWRotorCalc1Ramp = 0;
}
////
}
else
{
WRotor.iqWRotorCalc1 = 0;
WRotor.iqWRotorCalcBeforeRegul1 = 0;
}
///
if (WRotor.iqTimeSensor2 != 0 && inc_sensor.use_sensor2)
{
if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time2==0)
WRotor.iqWRotorCalcBeforeRegul2 = KoefNormMS / WRotor.iqTimeSensor2;
if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time2==1)
WRotor.iqWRotorCalcBeforeRegul2 = KoefNormNS / WRotor.iqTimeSensor2;
if (WRotor.iqWRotorCalcBeforeRegul2 > max_value_rotor)
{
WRotor.iqWRotorCalc2 = 0;
WRotor.iqWRotorCalcBeforeRegul2 = 0;
}
else
WRotor.iqWRotorCalc2 = exp_regul_iq(koefW, WRotor.iqWRotorCalc2, WRotor.iqWRotorCalcBeforeRegul2);
/////
if (WRotor.iqWRotorCalc2)
{
if (WRotor.iqPrevWRotorCalc2 != WRotor.iqWRotorCalc2)
{
WRotor.iqWRotorCalc2Ramp = zad_intensiv_q(wrotor_add_ramp, wrotor_add_ramp, WRotor.iqWRotorCalc2Ramp, WRotor.iqWRotorCalc2);
WRotor.iqPrevWRotorCalc2 = WRotor.iqWRotorCalc2;
}
}
else
{
WRotor.iqPrevWRotorCalc2 = 0;
WRotor.iqWRotorCalc2Ramp = 0;
}
////
}
else
{
WRotor.iqWRotorCalc2 = 0;
WRotor.iqWRotorCalcBeforeRegul2 = 0;
}
///
if (inc_sensor.data.TimeCalcFromImpulses1 && inc_sensor.use_sensor1)
WRotor.iqWRotorImpulsesBeforeRegul1 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses1 * ROTOR_SENSOR_IMPULSES_PER_ROTATE);
else
WRotor.iqWRotorImpulsesBeforeRegul1 = 0;
WRotor.iqWRotorImpulses1 = exp_regul_iq(koefW, WRotor.iqWRotorImpulses1, WRotor.iqWRotorImpulsesBeforeRegul1);
if (inc_sensor.data.TimeCalcFromImpulses2 && inc_sensor.use_sensor2)
WRotor.iqWRotorImpulsesBeforeRegul2 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses2 * ROTOR_SENSOR_IMPULSES_PER_ROTATE);
else
WRotor.iqWRotorImpulsesBeforeRegul2 = 0;
WRotor.iqWRotorImpulses2 = exp_regul_iq(koefW, WRotor.iqWRotorImpulses2, WRotor.iqWRotorImpulsesBeforeRegul2);
// WRotor.iqWRotorCalcBeforeRegul = _IQdiv(WRotor.iqWRotorCalcBeforeRegul,IQ_CONST_3);
}
#define LEVEL_SWITCH_TO_GET_IMPULSES_OBOROTS 50 // Oborot
void select_values_wrotor(void)
{
static _iq level_switch_to_get_impulses_hz = _IQ(LEVEL_SWITCH_TO_GET_IMPULSES_OBOROTS/60.0/NORMA_FROTOR);
static unsigned int prev_RotorDirectionInstant = 0;
static unsigned int status_RotorRotation = 0; // ���� ��������?
static _iq wrotor_add = _IQ(0.002/NORMA_FROTOR);
if (WRotor.iqWRotorCalc1>level_switch_to_get_impulses_hz
|| WRotor.iqWRotorCalc2>level_switch_to_get_impulses_hz)
{
// ��� ������� �������
if (WRotor.iqWRotorImpulses1 || WRotor.iqWRotorImpulses2)
{
if(WRotor.iqWRotorImpulses1>WRotor.iqWRotorImpulses2)
WRotor.iqWRotorSum = WRotor.iqWRotorImpulsesBeforeRegul1;
else
WRotor.iqWRotorSum = WRotor.iqWRotorImpulsesBeforeRegul2;
}
else
{
if(WRotor.iqWRotorCalc1>WRotor.iqWRotorCalc2)
WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul1;
else
WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul2;
}
}
else
{
if(WRotor.iqWRotorCalc1>WRotor.iqWRotorCalc2)
WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul1;
else
WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul2;
}
// ������� �����������
// if (prev_prev_RotorDirectionInstant && WRotorPBus.RotorDirectionSlow)
// if (WRotor.iqWRotorSum)
// {
// inc_sensor.break_direction = 1;
// }
// prev_prev_RotorDirectionInstant = WRotorPBus.RotorDirectionSlow;
//// ������ �����������!!!
// if (WRotorPBus.RotorDirectionSlow==0)
// {
// if (WRotor.iqWRotorSum)
// inc_sensor.break_direction = 1;
// }
// else
// inc_sensor.break_direction = 0;
// if (WRotorPBus.RotorDirectionSlow==0)
// {
// // ����� � 0 ������� !!! ������ �����������!!!
// WRotor.iqWRotorSumFilter = exp_regul_iq(koefW, WRotor.iqWRotorSumFilter, 0);
// }
// else
WRotor.iqWRotorSumFilter = exp_regul_iq(koefW, WRotor.iqWRotorSumFilter, WRotor.iqWRotorSum*WRotorPBus.RotorDirectionSlow);
WRotor.iqWRotorSumRamp = zad_intensiv_q(wrotor_add, wrotor_add, WRotor.iqWRotorSumRamp, WRotor.iqWRotorSumFilter);
WRotor.iqWRotorSumFilter2 = exp_regul_iq(koefW2, WRotor.iqWRotorSumFilter2, WRotor.iqWRotorSumFilter);
WRotor.iqWRotorSumFilter3 = exp_regul_iq(koefW3, WRotor.iqWRotorSumFilter3, WRotor.iqWRotorSumFilter);
}
#pragma CODE_SECTION(RotorMeasure,".fast_run");
void RotorMeasureDetectDirection(void)
{
int direction1, direction2, sum_direct;
direction1 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == ROTOR_SENSOR_CODE_CLOCKWISE ? 1 :
project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == ROTOR_SENSOR_CODE_COUNTERCLOCKWISE ? -1 :
0;
direction2 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == ROTOR_SENSOR_CODE_COUNTERCLOCKWISE ? 1 :
project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == ROTOR_SENSOR_CODE_CLOCKWISE ? -1 :
0;
sum_direct = (direction1 + direction2) > 0 ? 1 :
(direction1 + direction2) < 0 ? -1 :
0;
WRotorPBus.RotorDirectionInstant = sum_direct;
}
///////////////////////////////////////////////////////////////
#endif
///////////////////////////////////////////////////////////////
#pragma CODE_SECTION(update_rot_sensors,".fast_run");
void update_rot_sensors(void)
{
inc_sensor.update_sensors(&inc_sensor);
}
///////////////////////////////////////////////////////////////