#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 #include #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_direction0) 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>1; // max OVERFULL if (flag_overfull_rotor) { if (*count_overfull_discret0) (*count_overfull_discret)--; } // zero? if (flag_not_ready_rotor) { if (*count_zero_discret0) (*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 && iqTimeRotortime_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_discret0) // (*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_discret0) // (*count_overfull_discret)--; // } // // // zero? // if (flag_not_ready_rotor) // { // if (*count_zero_discret0) // (*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); } ///////////////////////////////////////////////////////////////