Coal56AB/matlab_23550
4
0
Fork 0
Code Issues 4 Pull Requests Actions Packages Projects 1 Releases Wiki Activity
45beae3740
matlab_23550/Inu/Src/main/v_rotor.c

725 lines
23 KiB
C
Raw Normal View History

попытка запустить 23550_on_ship. такая же фигня, шим красивый но неправильный
2025-01-15 13:39:33 +03:00
#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"
#pragma DATA_SECTION(WRotor,".fast_vars");
WRotorValues WRotor = WRotorValues_DEFAULTS;
#pragma DATA_SECTION(WRotorPBus,".fast_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;
void rotorInit(void)
{
WRotorPBus.ModeAutoDiscret = 1;
}
#pragma CODE_SECTION(update_rot_sensors,".fast_run");
void update_rot_sensors(void)
{
inc_sensor.update_sensors(&inc_sensor);
}
#define MAX_COUNT_OVERFULL_DISCRET 2250
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
#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 *RotorDirection,
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;
static _iq koefW = _IQ(0.05);//0.05
// 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 <20><>.
static _iq time_level_discret_0to1 = 400;//204800; // KoefNorm_discret0/2000 = 0.244140625 <20><>.
static unsigned int discret;
if (valid_sensor_direct == 0)
d1 = 0;
if (valid_sensor_90 == 0)
d2 = 0;
// <20><><EFBFBD> <20><><EFBFBD>-<2D><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD>, <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>!!! <20> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> == 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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>!!! <20> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> == MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS!!!
}
// <20><><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*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)
{
// <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
if (d1 == 0 && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
{
// d1 - <20><><EFBFBD><EFBFBD><EFBFBD>, d2 <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
d1 = d2;
}
else
if (d1 == LEVEL_VALUE_SENSOR_OVERFULL && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
{
// d1 - <20><><EFBFBD><EFBFBD><EFBFBD>, d2 <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
d1 = d2;
}
else
if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0)
{
// d2 - <20><><EFBFBD><EFBFBD><EFBFBD>, d1 <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
d2 = d1;
}
else
if (d2 == 0 && d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0)
{
// d2 - <20><><EFBFBD><EFBFBD><EFBFBD>, d1 <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{
// <20><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>!
iqWRotorCalc = 0;
*prev_iqTimeRotor = 0;
iqTimeRotor = 0;
}
else
{
// <20><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> prev_iqTimeRotor
if (iqTimeRotor==0)
iqTimeRotor = *prev_iqTimeRotor;
}
*prev_iqTimeRotor = iqTimeRotor;
// <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (WRotorPBus.ModeAutoDiscret==1)
{
if ( (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) || (iqTimeRotor==0) )
{
// <20><><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>=0
// <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> discret_out = 0
if (discret_in == 1) // <20><><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> discret?
{
// discret <20><><EFBFBD> =1, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 0.
*discret_out = 0;
*count_overfull_discret = 0; // <20><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>!
}
}
else
{
// <20><><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> discret==0 <20><><EFBFBD><EFBFBD><EFBFBD>...
if (discret==0 && iqTimeRotor<time_level_discret_0to1 && iqTimeRotor!=65535)
*discret_out = 1;
// <20><><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> discret==1 <20><><EFBFBD><EFBFBD><EFBFBD>...
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) )
{
// <20><><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20> 0, <20>.<2E>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>!
*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 <20><>.
static _iq time_level_discret_0to1 = 400;//204800; // KoefNorm_discret0/2000 = 0.244140625 <20><>.
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;
_iq koefW = _IQ(0.05);//0.05
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;
#if(C_cds_in_number>=1)
project.cds_in[0].read_pbus(&project.cds_in[0]);
#endif
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;
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_1==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==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
#if (ENABLE_ROTOR_SENSOR_1==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==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==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,
&WRotorPBus.RotorDirection1,
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==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,
&WRotorPBus.RotorDirection2,
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
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++;
}
}
#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
volatile float MyVar0 = 0;
// volatile unsigned int MyVar1 = 0;
// volatile unsigned int MyVar2 = 0;
unsigned int MyVar3 = 0;
int direction1 = 0, direction2 = 0;
_iq koefW = _IQ(0.05);//0.05
inc_sensor.read_sensors(&inc_sensor);
// rotation_sensor.read_sensors(&rotation_sensor);
// if(rotation_sensor.in_plane.read.regs.comand_reg.bit.update_registers)
// {
// rotor_error_update_count ++;
// }
// WRotor.RotorDirection = (rotation_sensor.in_plane.out.direction1 + rotation_sensor.in_plane.out.direction2) > 0 ? 1 :
// (rotation_sensor.in_plane.out.direction1 + rotation_sensor.in_plane.out.direction2) < 0 ? -1 :
// 0;
//
// direction1 = rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir0 == 2 ? 1 :
// rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir0 == 1 ? -1 :
// 0;
// direction2 = rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir1 == 2 ? 1 :
// rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir1 == 1 ? -1 :
// 0;
// WRotor.RotorDirection = (direction1 + direction2) > 0 ? 1 :
// (direction1 + direction2) < 0 ? -1 :
// 0;
//**************************************************************************************************
// sensor 1
// if((rotation_sensor.in_plane.out.CountOne1 <= 200)
// || 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.CountZero1 == 65535)
// { rotation_sensor.in_plane.out.CountZero1 = 0; }
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))
{
WRotor.iqWRotorRaw0 = MyVar3;
// WRotor.iqWRotorRaw0 = KoefNorm / MyVar3;
// MyVar0 = 9590 / MyVar0;//100000 / MyVar0; // 100000 = 60MHz/Impulses, Balzam: Dents = 600, Impuls per dent = 1; Pr 162 : Dents = 782, Impuls per dent = 8
// WRotor.iqWRotorRaw0 = _IQ(MyVar0/NORMA_WROTOR);
}
else
{
WRotor.iqWRotorRaw0 = 0;
}
// MyVar3 = (unsigned long) rotation_sensor.in_plane.out.CountZero1;
MyVar3 = inc_sensor.data.CountZero1;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotor.iqWRotorRaw1 = MyVar3;
}
else
{
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((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 (inc_sensor.use_sensor2)
{
MyVar3 = inc_sensor.data.CountOne2;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotor.iqWRotorRaw2 = MyVar3;
}
else
{
WRotor.iqWRotorRaw2 = 0;
}
MyVar3 = inc_sensor.data.CountZero2;
if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR)
&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
{
WRotor.iqWRotorRaw3 = MyVar3;
}
else
{
WRotor.iqWRotorRaw3 = 0;
}
// i_led1_on_off(0);
}
else
{
WRotor.iqWRotorRaw2 = 0;
WRotor.iqWRotorRaw3 = 0;
}
WRotor.iqTimeSensor1 = WRotor.iqWRotorRaw0 + WRotor.iqWRotorRaw1;
WRotor.iqTimeSensor2 = WRotor.iqWRotorRaw2 + WRotor.iqWRotorRaw3;
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;
WRotor.iqWRotorCalc1 = exp_regul_iq(koefW, WRotor.iqWRotorCalc1, WRotor.iqWRotorCalcBeforeRegul1);
}
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;
WRotor.iqWRotorCalc2 = exp_regul_iq(koefW, WRotor.iqWRotorCalc2, WRotor.iqWRotorCalcBeforeRegul2);
}
else
{
WRotor.iqWRotorCalc2 = 0;
WRotor.iqWRotorCalcBeforeRegul2 = 0;
}
if (inc_sensor.data.TimeCalcFromImpulses1 && inc_sensor.use_sensor1)
WRotor.iqWRotorImpulses1 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses1 * ROTOR_SENSOR_IMPULSES_PER_ROTATE);
else
WRotor.iqWRotorImpulses1 = 0;
if (inc_sensor.data.TimeCalcFromImpulses2 && inc_sensor.use_sensor2)
WRotor.iqWRotorImpulses2 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses2 * ROTOR_SENSOR_IMPULSES_PER_ROTATE);
else
WRotor.iqWRotorImpulses2 = 0;
// WRotor.iqWRotorCalcBeforeRegul = _IQdiv(WRotor.iqWRotorCalcBeforeRegul,IQ_CONST_3);
// i_led1_on_off(0);
if (WRotor.iqWRotorCalc1 == 0 && inc_sensor.use_sensor1)
WRotor.RotorDirection1 = 0;
if (WRotor.iqWRotorCalc2 == 0 && inc_sensor.use_sensor2)
WRotor.RotorDirection2 = 0;
//wrotor.iq_wrotor_calc = 0;
//i_led2_on_off(0);
}
Reference in New Issue Copy Permalink