725 lines
23 KiB
C
725 lines
23 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"
|
||
|
||
|
||
|
||
|
||
|
||
|
||
#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);
|
||
|
||
}
|
||
|
||
|