667 lines
21 KiB
C
667 lines
21 KiB
C
|
||
#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; }
|
||
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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; }
|
||
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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;
|
||
}
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
for(i = begin_data; i < end_data; i++)
|
||
{
|
||
accumulator += rotor_22220.iqFsensors[i];
|
||
}
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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
|
||
|
||
|
||
//<2F><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
||
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)
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20>.<2E>. <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>!!!
|
||
|
||
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) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// {
|
||
// rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0x5C;
|
||
// }
|
||
// else //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// {
|
||
// 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
|
||
//<2F><><EFBFBD><EFBFBD> <20><> 2, <20>.<2E>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> (1LL << 24) <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> 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);
|
||
//}
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
#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; // <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?
|
||
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);
|
||
|
||
|
||
}
|