429 lines
15 KiB
C
429 lines
15 KiB
C
#include "rotation_speed.h"
|
|
#include "xp_rotation_sensor.h"
|
|
#include "IQmathLib.h"
|
|
#include "params.h"
|
|
#include "adc_tools.h"
|
|
#include "rmp_cntl_my1.h"
|
|
#include "my_filter.h"
|
|
#include "vector.h"
|
|
#include "TuneUpPlane.h" //ñâåòîäèîäèê
|
|
#include "errors.h"
|
|
#include "log_to_memory.h"
|
|
#include "DSP281x_Device.h" //for int16
|
|
|
|
#pragma DATA_SECTION(rotor,".fast_vars1");
|
|
ROTOR_VALUE rotor = ROTOR_VALUE_DEFAULTS;
|
|
|
|
RMP_MY1 rmp_wrot = RMP_MY1_DEFAULTS;
|
|
|
|
_iq koef_Wout_filter = _IQ(0.2); //_IQ(0.15);
|
|
_iq koef_Wout_filter_long = _IQ(0.12);//_IQ(0.03);
|
|
|
|
|
|
static _iq impulses_To_iqF(unsigned int time, unsigned int impulses);
|
|
static _iq counter_To_iqF(unsigned int count, unsigned int freq_mode);
|
|
static _iq delta_Angle_To_iqF(unsigned long delta, unsigned int period);
|
|
static void sort_F_array(_iq *array, unsigned int size);
|
|
|
|
void rotorInit(void)
|
|
{
|
|
unsigned int i = 0, size = 0, *pint = 0;
|
|
|
|
rmp_wrot.RampLowLimit = 0;
|
|
rmp_wrot.RampHighLimit = _IQ(1);
|
|
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;
|
|
}
|
|
|
|
koef_Wout_filter = _IQ(0.2); //_IQ(0.15);
|
|
koef_Wout_filter_long = _IQ(0.001); //_IQ(0.0005);
|
|
}
|
|
|
|
#pragma CODE_SECTION(update_rot_sensors,".fast_run");
|
|
void update_rot_sensors()
|
|
{
|
|
// rotation_sensor.update_registers(&rotation_sensor);
|
|
}
|
|
|
|
#pragma CODE_SECTION(Rotor_measure,".fast_run");
|
|
void Rotor_measure(void)
|
|
{
|
|
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;
|
|
// _iq accumulator = 0, deltaF = 0, deltaAngle = 0;
|
|
// rotation_sensor.read_sensors(&rotation_sensor);
|
|
// if(rotation_sensor.in_plane.read.regs.comand_reg.bit.update_registers)
|
|
// {
|
|
// rotor.error_update_count ++;
|
|
// }
|
|
// // logpar.log22 = rotation_sensor.in_plane.out.CountOne1;
|
|
// // logpar.log23 = rotation_sensor.in_plane.out.CountZero1;
|
|
// if(rotation_sensor.use_sensor1)
|
|
// {
|
|
// if((rotation_sensor.in_plane.out.CountOne1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1)
|
|
// || rotation_sensor.in_plane.out.CountOne1 == 65535)
|
|
// { rotation_sensor.in_plane.out.CountOne1 = 0; }
|
|
// if((rotation_sensor.in_plane.out.CountZero1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1)
|
|
// || rotation_sensor.in_plane.out.CountZero1 == 65535)
|
|
// { rotation_sensor.in_plane.out.CountZero1 = 0; }
|
|
|
|
// //Êîãäà èìïóëüñîâ ìàëî ñ÷èòàåì ïî ïåðèîäó
|
|
// if (rotation_sensor.in_plane.out.Impulses1 < 5) {
|
|
// 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);
|
|
// }
|
|
// }
|
|
|
|
// if(rotation_sensor.in_plane.out.Impulses1 > 2)
|
|
// {
|
|
// rotor.iqFsensors[s_number++] = impulses_To_iqF(rotation_sensor.in_plane.out.Time1,
|
|
// rotation_sensor.in_plane.out.Impulses1);
|
|
// // logpar.log11 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]);
|
|
// } else {
|
|
// // logpar.log11 = 0;
|
|
// }
|
|
|
|
// if (rotor.iqF > 139810L)//10 rpm
|
|
// {
|
|
// if (rotation_sensor.in_plane.out.CountOne1 == 0 && rotation_sensor.in_plane.out.CountZero1 == 0
|
|
// && rotation_sensor.in_plane.out.Impulses1 == 0) {
|
|
// sens1_err_count += 1;
|
|
// } else {
|
|
// sens1_err_count = 0;
|
|
// }
|
|
// if (sens1_err_count > 50) {
|
|
// sens1_err_count = 50;
|
|
// faults.faults4.bit.Speed_Datchik_1_Off |= 1;
|
|
// }
|
|
// } else {
|
|
// sens1_err_count = 0;
|
|
// }
|
|
// }
|
|
|
|
// // logpar.log4 = rotation_sensor.in_plane.out.CountOne2;
|
|
// // logpar.log20 = rotation_sensor.in_plane.out.CountZero2;
|
|
// if(rotation_sensor.use_sensor2)
|
|
// {
|
|
// if((rotation_sensor.in_plane.out.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2)
|
|
// || rotation_sensor.in_plane.out.CountOne2 == 65535)
|
|
// { rotation_sensor.in_plane.out.CountOne2 = 0; }
|
|
// if((rotation_sensor.in_plane.out.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2)
|
|
// || rotation_sensor.in_plane.out.CountZero2 == 65535)
|
|
// { rotation_sensor.in_plane.out.CountZero2 = 0; }
|
|
|
|
// //Êîääà èìïóëüñîâ ìàëî, ñ÷èòàåì ïî ïåðèîäó
|
|
// if (rotation_sensor.in_plane.out.Impulses2 < 5) {
|
|
// 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);
|
|
// }
|
|
// }
|
|
|
|
// if(rotation_sensor.in_plane.out.Impulses2 > 2)
|
|
// {
|
|
// rotor.iqFsensors[s_number++] = impulses_To_iqF(rotation_sensor.in_plane.out.Time2,
|
|
// rotation_sensor.in_plane.out.Impulses2);
|
|
// // logpar.log16 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]);
|
|
// }
|
|
|
|
// if (rotor.iqF > 139810L)//10 rpm
|
|
// {
|
|
// if (rotation_sensor.in_plane.out.CountOne2 == 0 && rotation_sensor.in_plane.out.CountZero2 == 0
|
|
// && rotation_sensor.in_plane.out.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;
|
|
// }
|
|
// } else {
|
|
// sens2_err_count = 0;
|
|
// }
|
|
|
|
// }
|
|
|
|
// if(rotation_sensor.use_angle_plane && (s_number < SENSORS_NUMBER))
|
|
// {
|
|
// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel1_enable && rotor.iqFsensors[0] && (s_number < (SENSORS_NUMBER - 1)))
|
|
// {
|
|
// if(rotation_sensor.rotation_plane.out.Delta_angle1 < 1500)
|
|
// {
|
|
// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle1,
|
|
// rotation_sensor.rotation_plane.out.survey_time_mks);
|
|
|
|
// }
|
|
|
|
// deltaAngle = _IQabs(rotation_sensor.rotation_plane.out.Current_angle1 - PrevAngle1);
|
|
// if(deltaAngle > 1500)
|
|
// {
|
|
// deltaAngle = 262144 - deltaAngle;
|
|
// }
|
|
// if((deltaAngle < 291) && (deltaAngle >= 0))
|
|
// {
|
|
// rotor.iqFsensors[s_number++] =
|
|
// delta_Angle_To_iqF(deltaAngle, peroid_shim_mks);
|
|
// }
|
|
// PrevAngle1 = rotation_sensor.rotation_plane.out.Current_angle1;
|
|
// }
|
|
// // else
|
|
// // {
|
|
// // rotor.iqFsensors[s_number++] = 0;
|
|
// // rotor.iqFsensors[s_number++] = 0;
|
|
// // }
|
|
// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel2_enable && (s_number < (SENSORS_NUMBER - 1)))
|
|
// {
|
|
// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle2,
|
|
// rotation_sensor.rotation_plane.out.survey_time_mks);
|
|
// rotor.iqFsensors[s_number++] =
|
|
// delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle2 - PrevAngle2),
|
|
// peroid_shim_mks);
|
|
// PrevAngle2 = rotation_sensor.rotation_plane.out.Current_angle2;
|
|
// }
|
|
// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel3_enable && (s_number < (SENSORS_NUMBER - 1)))
|
|
// {
|
|
// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle3,
|
|
// rotation_sensor.rotation_plane.out.survey_time_mks);
|
|
// rotor.iqFsensors[s_number++] =
|
|
// delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle3 - PrevAngle3),
|
|
// peroid_shim_mks);
|
|
// PrevAngle3 = rotation_sensor.rotation_plane.out.Current_angle3;
|
|
// }
|
|
// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel4_enable && (s_number < (SENSORS_NUMBER - 1)))
|
|
// {
|
|
// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle4,
|
|
// rotation_sensor.rotation_plane.out.survey_time_mks);
|
|
// rotor.iqFsensors[s_number++] =
|
|
// delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle4 - PrevAngle4),
|
|
// peroid_shim_mks);
|
|
// PrevAngle2 = rotation_sensor.rotation_plane.out.Current_angle4;
|
|
// }
|
|
// }
|
|
// // logpar.log22 = (int16) _IQtoIQ15(rotor.iqFsensors[0]);
|
|
// // logpar.log23 = (int16) _IQtoIQ15(rotor.iqFsensors[1]);
|
|
// // logpar.log24 = (int16) _IQtoIQ15(rotor.iqFsensors[2]);
|
|
// // logpar.log25 = (int16) _IQtoIQ15(rotor.iqFsensors[3]);
|
|
// // logpar.log29 = (int16) _IQtoIQ15(rotor.iqFsensors[4]);
|
|
// // logpar.log30 = (int16) _IQtoIQ15(rotor.iqFsensors[5]);
|
|
|
|
// 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.iqFsensors, s_number);
|
|
// deltaF = rotor.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.iqFout - rotor.iqFsensors[i]) >= deltaF)
|
|
// {
|
|
// i++;
|
|
// }
|
|
// else
|
|
// {
|
|
// break;
|
|
// }
|
|
// }
|
|
// if(i < s_number) { begin_data = i; }
|
|
// else {begin_data = 0;}
|
|
// while((i < s_number) && (_IQabs(rotor.iqFout - rotor.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.iqFsensors[i];
|
|
// }
|
|
|
|
// if(end_data != begin_data)
|
|
// {
|
|
// rotor.iqF = accumulator / (end_data - begin_data);
|
|
// prev_wrot_count = 0;
|
|
// }
|
|
// else
|
|
// {
|
|
// rotor.iqF = prev_wrot;
|
|
// prev_wrot_count += 1;
|
|
// }
|
|
|
|
// // logpar.log19 = (int16)(_IQtoIQ15(rotor.iqF));
|
|
|
|
// if (prev_wrot != rotor.iqF || rotor.iqF==0 )
|
|
// {
|
|
// rmp_wrot.DesiredInput = rotor.iqF;
|
|
// rmp_wrot.calc(&rmp_wrot);
|
|
// rotor.iqF = rmp_wrot.Out;
|
|
// }
|
|
// prev_wrot=rotor.iqF;
|
|
// //Îò çàâèñàíèÉà ñòàðîãî çíà÷åíèÉà.
|
|
// if (prev_wrot_count > 10) {
|
|
// prev_wrot = 0;
|
|
// prev_wrot_count = 10;
|
|
// }
|
|
|
|
// rotor.iqFout = exp_regul_iq(koef_Wout_filter, rotor.iqFout, rotor.iqF);
|
|
// rotor.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor.iqFlong, rotor.iqF);
|
|
|
|
// rotor.direct_rotor_in1 = rotation_sensor.in_plane.out.direction1;
|
|
// rotor.direct_rotor_in2 = rotation_sensor.in_plane.out.direction2;
|
|
// rotor.direct_rotor_angle = rotation_sensor.rotation_plane.out.direction;
|
|
|
|
// rotor.error.sens_err1 = rotation_sensor.in_plane.read.pbus.direction.bit.sens_err1;
|
|
// rotor.error.sens_err2 = rotation_sensor.in_plane.read.pbus.direction.bit.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.iqFout >139810L) //10ob/min
|
|
// {
|
|
// if((rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0)
|
|
// {
|
|
// direct_accum++;
|
|
// }
|
|
// else if((rotor.direct_rotor_in1 + rotor.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.direct_rotor = direct_accum > 0 ? 1 :
|
|
// direct_accum < 0 ? -1 :
|
|
// 0;
|
|
// // if (f.flag_second_PCH) {
|
|
// // rotor.direct_rotor = - rotor.direct_rotor;
|
|
// // }
|
|
// }
|
|
// else
|
|
// {
|
|
// 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 (f.flag_second_PCH) {
|
|
// // rotor.direct_rotor = - rotor.direct_rotor;
|
|
// // }
|
|
// direct_accum = rotor.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;
|
|
// }
|
|
}
|
|
|
|
#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) / 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(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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|