#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 "edrk_main.h" #include "params_pwm24.h" #include "v_rotor_22220.h" #include "rmp_cntl_v1.h" #include "mathlib.h" //#define _ENABLE_PWM_LED2_PROFILE 1 #if (_ENABLE_PWM_LED2_PROFILE) extern unsigned int profile_pwm[30]; extern unsigned int pos_profile_pwm; #endif // //#pragma DATA_SECTION(buf1,".logs2") //int buf1[SIZE_BUF_W]; //#pragma DATA_SECTION(buf2,".logs2") //int buf2[SIZE_BUF_W]; //#pragma DATA_SECTION(buf3,".logs2") //int buf3[SIZE_BUF_W]; //#pragma DATA_SECTION(buf4,".logs2") //int buf4[SIZE_BUF_W]; //#pragma DATA_SECTION(buf5,".logs2") //int buf5[SIZE_BUF_W]; //#pragma DATA_SECTION(buf6,".logs2") //int buf6[SIZE_BUF_W]; //#pragma DATA_SECTION(buf7,".logs2") //int buf7[SIZE_BUF_W]; //#pragma DATA_SECTION(buf8,".logs2") //int buf8[SIZE_BUF_W]; // /////////////////////////////////////////////// /////////////////////////////////////////////// /////////////////////////////////////////////// RMP_V1 rmp_wrot = RMP_V1_DEFAULTS; #pragma DATA_SECTION(rotor_22220,".fast_vars"); ROTOR_VALUE_22220 rotor_22220 = ROTOR_VALUE_22220_DEFAULTS; void rotorInit_22220(void) { unsigned int i = 0, size = 0, *pint = 0; rmp_wrot.RampLowLimit = 0; rmp_wrot.RampHighLimit = _IQ(1.0); rmp_wrot.RampPlus = _IQ(0.0015/NORMA_FROTOR); rmp_wrot.RampMinus = _IQ(-0.0015/NORMA_FROTOR); rmp_wrot.DesiredInput = 0; rmp_wrot.Out = 0; // pint = (unsigned int*)&rotor; // size = sizeof(rotor) / sizeof(unsigned int); // for(i = 0; i < size; i++) // { // *(pint + i) = 0; // } } _iq koef_Wout_filter = _IQ(0.2); //_IQ(0.15); _iq koef_Wout_filter_long = _IQ(0.001);//_IQ(0.03); #define SIZE_BUF_F1 10 #pragma DATA_SECTION(f1,".slow_vars") static _iq f1[SIZE_BUF_F1]={0,0,0,0,0,0,0,0,0,0}; #pragma DATA_SECTION(f1_int,".slow_vars") static long f1_int[SIZE_BUF_F1]={0,0,0,0,0,0,0,0,0,0}; #pragma CODE_SECTION(clear_iqFsensors,".fast_run"); void clear_iqFsensors(void) { int i; for (i=0;i<SENSORS_NUMBER;i++) rotor_22220.iqFsensors[i] = 0; } //#pragma CODE_SECTION(Rotor_measure_22220,".fast_run"); void Rotor_measure_22220(void) { static long c_s=-1; // static int flag_buf = 0, prev_flag_buf = 0 ; static unsigned long PrevAngle1 = 0, PrevAngle2 = 0, PrevAngle3 = 0, PrevAngle4 = 0; static unsigned int peroid_shim_mks = (unsigned int)(1000000L / FREQ_PWM / 2.0); static _iq prev_wrot = 0; static unsigned int prev_wrot_count = 0; static int direct_accum = 0; static int sens1_err_count = 0; static int sens2_err_count = 0; unsigned int s_number = 0, begin_data = 0, end_data = 0, i; unsigned int s_number2 = 0; unsigned int s_number3 = 0; _iq accumulator = 0, deltaF = 0, deltaAngle = 0; long sum_count=0; int direction1, direction2, sum_direct, sens_err1, sens_err2; #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif clear_iqFsensors(); #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif // rotation_sensor.read_sensors(&rotation_sensor); inc_sensor.read_sensors(&inc_sensor); #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif 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; sens_err1 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == 3; sens_err2 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == 3; if(inc_sensor.pm67regs.read_comand_reg.bit.update_registers) { rotor_22220.error_update_count++; } // logpar.log22 = rotation_sensor.in_plane.out.CountOne1; // logpar.log23 = rotation_sensor.in_plane.out.CountZero1; // if (edrk.logs_rotor==1) // flag_buf = edrk.logs_rotor; // // // if (flag_buf==1 && prev_flag_buf==0) // c_s = -1; // // if (flag_buf) // { // if (c_s>=(SIZE_BUF_W-1)) // { // flag_buf = 0; // c_s = 0; // } // else // c_s++; // // // buf1[c_s] = inc_sensor.data.CountOne1;// rotation_sensor.in_plane.out.CountOne1; // buf2[c_s] = inc_sensor.data.CountZero1;//rotation_sensor.in_plane.out.CountZero1; // buf3[c_s] = inc_sensor.data.CountOne2;//rotation_sensor.in_plane.out.CountOne2; // buf4[c_s] = inc_sensor.data.CountZero2;//rotation_sensor.in_plane.out.CountZero2; // buf5[c_s] = direction1;//inc_sensor.data.;//(rotation_sensor.in_plane.out.direction1); // buf6[c_s] = direction2;//(rotation_sensor.in_plane.out.direction2); // buf7[c_s] = (project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1); // buf8[c_s] = (project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2); // // } // prev_flag_buf = flag_buf; #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif if(inc_sensor.use_sensor1) { if((inc_sensor.data.CountOne1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) || inc_sensor.data.CountOne1 == 65535) { inc_sensor.data.CountOne1 = 0; } if((inc_sensor.data.CountZero1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) || inc_sensor.data.CountZero1 == 65535) { inc_sensor.data.CountZero1 = 0; } //����� ��������� ���� ������� �� ������� if (inc_sensor.data.Impulses1 < 5) { if(inc_sensor.data.CountOne1 && inc_sensor.data.CountZero1 ) { sum_count = (long)inc_sensor.data.CountOne1 + (long)inc_sensor.data.CountZero1; if (s_number3<SIZE_BUF_F1) f1[s_number3++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq1); if (s_number2<SIZE_BUF_F1) { if (inc_sensor.data.counter_freq1) f1_int[s_number2++] = sum_count/6; else f1_int[s_number2++] = sum_count*100/6; } if (s_number<SENSORS_NUMBER) rotor_22220.iqFsensors[s_number++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq1); } // if(rotation_sensor.in_plane.out.CountOne1) { // rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne1, // rotation_sensor.in_plane.out.counter_freq1); // } // if(rotation_sensor.in_plane.out.CountZero1) { // rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero1, // rotation_sensor.in_plane.out.counter_freq1); // } } else { if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = -2; } if(inc_sensor.data.Impulses1 > 2) { if (s_number<SENSORS_NUMBER) rotor_22220.iqFsensors[s_number++] = impulses_To_iqF(inc_sensor.data.Time1, inc_sensor.data.Impulses1); if (s_number3<SIZE_BUF_F1) f1[s_number3++] = impulses_To_iqF(inc_sensor.data.Time1, inc_sensor.data.Impulses1); if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = inc_sensor.data.Time1*10/inc_sensor.data.Impulses1; // logpar.log11 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]); } else { // logpar.log11 = 0; if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = -3; } if (rotor_22220.iqF > 139810L)//10 rpm { if (inc_sensor.data.CountOne1 == 0 && inc_sensor.data.CountZero1 == 0 && inc_sensor.data.Impulses1 == 0) { sens1_err_count += 1; } else { sens1_err_count = 0; } if (sens1_err_count > 50) { sens1_err_count = 50; edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 1; // faults.faults4.bit.Speed_Datchik_1_Off |= 1; } } else { sens1_err_count = 0; edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 0; } } #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif // logpar.log4 = rotation_sensor.in_plane.out.CountOne2; // logpar.log20 = rotation_sensor.in_plane.out.CountZero2; if(inc_sensor.use_sensor2) { if((inc_sensor.data.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) || inc_sensor.data.CountOne2 == 65535) { inc_sensor.data.CountOne2 = 0; } if((inc_sensor.data.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) || inc_sensor.data.CountZero2 == 65535) { inc_sensor.data.CountZero2 = 0; } //����� ��������� ����, ������� �� ������� if (inc_sensor.data.Impulses2 < 5) { if(inc_sensor.data.CountOne2 && inc_sensor.data.CountZero2 ) { sum_count = (long)inc_sensor.data.CountOne2+(long)inc_sensor.data.CountZero2; if (s_number3<SIZE_BUF_F1) f1[s_number3++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq2); //f1_int[s_number2++] = sum_count/60; if (s_number2<SIZE_BUF_F1) { if (inc_sensor.data.counter_freq2) f1_int[s_number2++] = sum_count/6; else f1_int[s_number2++] = sum_count*100/6; } if (s_number<SENSORS_NUMBER) rotor_22220.iqFsensors[s_number++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq2); } // if(rotation_sensor.in_plane.out.CountOne2) { // rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne2, // rotation_sensor.in_plane.out.counter_freq2); // } // if(rotation_sensor.in_plane.out.CountZero2) { // rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero2, // rotation_sensor.in_plane.out.counter_freq2); // } } else { if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = -4; } #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif if(inc_sensor.data.Impulses2 > 2) { if (s_number<SENSORS_NUMBER) rotor_22220.iqFsensors[s_number++] = impulses_To_iqF(inc_sensor.data.Time2, inc_sensor.data.Impulses2); if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = inc_sensor.data.Time2*10/inc_sensor.data.Impulses2; // logpar.log16 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]); } else { if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = -5; } #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif if (rotor_22220.iqF > 139810L)//10 rpm { if (inc_sensor.data.CountOne2 == 0 && inc_sensor.data.CountZero2 == 0 && inc_sensor.data.Impulses2 == 0) { sens2_err_count += 1; } else { sens2_err_count = 0; } if (sens2_err_count > 50) { sens2_err_count = 50; // faults.faults4.bit.Speed_Datchik_2_Off |= 1; edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 1; } } else { sens2_err_count = 0; edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 0; } } if(s_number > SENSORS_NUMBER_ONLY_IN) {s_number = SENSORS_NUMBER_ONLY_IN;} //TODO set SENSORS_NUMBER when tune angle measure if(s_number > 3) { sort_F_array(rotor_22220.iqFsensors, s_number); deltaF = rotor_22220.iqFout >> 2; if(deltaF < 43000) // ~3 ob/min { deltaF = 43000; } i = 0; begin_data = 0; end_data = s_number; //TODO test, as usial while(i < s_number) { if(_IQabs(rotor_22220.iqFout - rotor_22220.iqFsensors[i]) >= deltaF) { i++; } else { break; } } if(i < s_number) { begin_data = i; } else {begin_data = 0;} while((i < s_number) && (_IQabs(rotor_22220.iqFout - rotor_22220.iqFsensors[i]) < deltaF)) { i++; } if(i <= SENSORS_NUMBER) { end_data = i; } else { end_data = SENSORS_NUMBER; } } else { begin_data = 0; end_data = s_number; } if (begin_data >= end_data) { //This part to prevent freeze of speed on some level if signal lost begin_data = 0; end_data = s_number; } // ��������� for(i = begin_data; i < end_data; i++) { accumulator += rotor_22220.iqFsensors[i]; } // ��������� if(end_data != begin_data) { rotor_22220.iqFdirty = accumulator / (end_data - begin_data); prev_wrot_count = 0; } else { rotor_22220.iqFdirty = prev_wrot; prev_wrot_count += 1; } // logpar.log19 = (int16)(_IQtoIQ15(rotor.iqF)); // rotor_22220.iqFdirty = rotor_22220.iqF; if (prev_wrot != rotor_22220.iqFdirty || rotor_22220.iqFdirty==0 ) { rmp_wrot.DesiredInput = rotor_22220.iqFdirty; rmp_wrot.calc(&rmp_wrot); rotor_22220.iqF = rmp_wrot.Out; } else { rotor_22220.iqF = rotor_22220.iqFdirty; } prev_wrot=rotor_22220.iqF; #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif //�� ���������� ������� ���������. if (prev_wrot_count > 10) { prev_wrot = 0; prev_wrot_count = 10; } rotor_22220.iqFout = exp_regul_iq(koef_Wout_filter, rotor_22220.iqFout, rotor_22220.iqF); rotor_22220.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor_22220.iqFlong, rotor_22220.iqF); #if (_ENABLE_PWM_LED2_PROFILE) if (profile_pwm[pos_profile_pwm++]) i_led2_on_off(1); else i_led2_on_off(0); #endif rotor_22220.direct_rotor_in1 = direction1;//rotation_sensor.in_plane.out.direction1; #if (_STEND_40MWT==1) // ������ ����������� �������� ��� ������� ������, �.�. �� ������ ���� ������ ��������� ������, � ������ ��������� ����!!! rotor_22220.direct_rotor_in2 = -rotation_sensor.in_plane.out.direction2; #else rotor_22220.direct_rotor_in2 = direction2;//rotation_sensor.in_plane.out.direction2; #endif // rotor.direct_rotor_angle = rotation_sensor.rotation_plane.out.direction; rotor_22220.error.sens_err1 = sens_err1; rotor_22220.error.sens_err2 = sens_err2; // rotor.direct_rotor = (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0 ? 1 : // + rotor.direct_rotor_angle // (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) < 0 ? -1 : // + rotor.direct_rotor_angle // 0; if(rotor_22220.iqFout >139810L) //10ob/min { if((rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) > 0) { direct_accum++; } else if((rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) < 0) { direct_accum--; } else { if(direct_accum > 0) {direct_accum--;} if(direct_accum < 0) {direct_accum++;} } if(direct_accum > 60) { direct_accum = 60; } if(direct_accum < -60) { direct_accum = -60; } rotor_22220.direct_rotor = direct_accum > 0 ? 1 : direct_accum < 0 ? -1 : 0; // if (f.flag_second_PCH) { // rotor.direct_rotor = - rotor.direct_rotor; // } } else { rotor_22220.direct_rotor = (rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) > 0 ? 1 : // + rotor.direct_rotor_angle (rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) < 0 ? -1 : // + rotor.direct_rotor_angle 0; // if (f.flag_second_PCH) { // rotor.direct_rotor = - rotor.direct_rotor; // } direct_accum = rotor_22220.direct_rotor; } // if(rotation_sensor.in_plane.write.regs.comand_reg.bit.set_sampling_time) //�������� �������� // { // rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0x5C; // } // else //�������� �������, ������ ��������� // { // rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0xA8; // } if (s_number2<SIZE_BUF_F1) f1_int[s_number2++] = -1; } #pragma CODE_SECTION(impulses_To_iqF,".fast_run"); _iq impulses_To_iqF(unsigned int time, unsigned int impulses) //time mks. impulses count { //Flong = (impulses / time / IMPULSES_PER_TURN) * _IQ(1) / NORMA_FROTOR; static unsigned long long koeff_to_F = 16777216LL / IMPULSES_PER_TURN * 1000000LL / NORMA_FROTOR; long long Flong = (long long)impulses * koeff_to_F / time; return (_iq)Flong; } //Frot = Fimp / IMPULSES_PER_TURN / NORMA_FROTOR = // = counter_freq / counter / 2 / IMPULSES_PER_TURN / NORMA_FROTOR //���� �� 2, �.�. ������� ������������ �������� ������� //��������� �� (1LL << 24) ����� �������� � iq24 #pragma CODE_SECTION(counter_To_iqF,".fast_run"); _iq counter_To_iqF(unsigned int count, unsigned int freq_mode) { static long long koeff_to_F = 60000000LL * _IQ(1.0) / IMPULSES_PER_TURN / 2 / NORMA_FROTOR; long long Flong = 0; if(freq_mode == 0) // 60Mhz { Flong = koeff_to_F / count / 100; } else //600KHz { Flong = koeff_to_F / count; } return Flong; } #pragma CODE_SECTION(counter_To_iqF2,".fast_run"); _iq counter_To_iqF2(long count, unsigned int freq_mode) { static long long koeff_to_F = 60000000LL * _IQ(1.0) / IMPULSES_PER_TURN / NORMA_FROTOR; long long Flong = 0; if(freq_mode == 0) // 60Mhz { Flong = koeff_to_F / count / 100; } else //600KHz { Flong = koeff_to_F / count; } return Flong; } //#pragma CODE_SECTION(delta_Angle_To_iqF,".fast_run"); //_iq delta_Angle_To_iqF(unsigned long delta, unsigned int period) //{ // // iqF = (delta / ANGLE_RESOLUTION) / (period * 10^-6) * (1LL << 24) / NORMA_FROTOR; // // iqF = delta * 10^6 * (1LL << 24) / ANGLE_RESOLUTION / period / NORMA_FROTOR; // static long long koeff_to_F = 1000000LL * (1LL << 24) / ANGLE_RESOLUTION / NORMA_FROTOR; // return (_iq)(koeff_to_F * delta / period); //} // ���������� ������� ��������� #pragma CODE_SECTION(sort_F_array,".fast_run2"); void sort_F_array(_iq *array, unsigned int size) { unsigned int i, j; _iq tmp = 0; for(i = size; i > 0; i--) { for(j = 1; j < size; j++) { if(array[j - 1] > array[j]) { tmp = array[j]; array[j] = array[j - 1]; array[j - 1] = tmp; } } } } void select_values_wrotor_22220(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); WRotor.RotorDirectionSlow = rotor_22220.direct_rotor; WRotor.iqWRotorSum = rotor_22220.iqFout;// * rotor_22220.direct_rotor; WRotor.iqWRotorSumFilter = rotor_22220.iqFout * rotor_22220.direct_rotor; WRotor.iqWRotorSumFilter2 = rotor_22220.iqFlong * rotor_22220.direct_rotor; WRotor.iqWRotorSumFilter3 = rotor_22220.iqFlong * rotor_22220.direct_rotor; WRotor.iqWRotorSumRamp = zad_intensiv_q(wrotor_add, wrotor_add, WRotor.iqWRotorSumRamp, WRotor.iqWRotorSumFilter); }