robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

Project changes

Browse source 
1/ Added Spi that used to read data from AS5045b position sensor
2/ Added Base FOC code to project
3/ Added Standart DSP lib with optimazes sincos cals Need for FOC
This commit is contained in:
on4ip84 2023-08-23 09:02:09 +03:00
parent ee1319b10e
commit 526b689693
43 changed files with 10985 additions and 410 deletions
Download patch file Download diff file Expand all files Collapse all files

4
.cproject
View file

@ -35,7 +35,7 @@
</tool>
<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1151063087" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.872193149" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.605113329" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.605113329" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.o0" valueType="enumerated"/>
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.449668890" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
<listOptionValue builtIn="false" value="DEBUG"/>
<listOptionValue builtIn="false" value="USE_FULL_LL_DRIVER"/>
@ -45,6 +45,8 @@
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.348859802" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
<listOptionValue builtIn="false" value="../Inc"/>
<listOptionValue builtIn="false" value="../App"/>
<listOptionValue builtIn="false" value="../App/FOC"/>
<listOptionValue builtIn="false" value="../Drivers/DSP/inc/"/>
<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>

117
App/ControlFuncs.c Normal file
View file

@ -0,0 +1,117 @@
/*
* ControlFuncs.c
*
* Created on: 20 авг. 2023 г.
* Author: on4ip
*/
#include "ControlFuncs.h"
/*current offsets form ADC channels*/
float offset_Ia=0;
float offset_Ib=0;
float offset_Ic=0;
void FAST_loop(SYSTEMobj_st *SYSTEMobj)
{
/*Get HW protection state*/
if(GET_HWfault_state(SYSTEMobj->DRIVE->drive_ID))
{
SYSTEMobj->DRIVE->STAT.stat.bit_FAULT = 1;
}
if(!SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED)
{
offset_Ia += SYSTEMobj->DRIVE->currentABC.current_Ia * SYSTEMobj->DRIVE->Tsample_fast;
offset_Ib += SYSTEMobj->DRIVE->currentABC.current_Ib * SYSTEMobj->DRIVE->Tsample_fast;
offset_Ic += SYSTEMobj->DRIVE->currentABC.current_Ic * SYSTEMobj->DRIVE->Tsample_fast;
}
SYSTEMobj->DRIVE->currentABC.current_Ia = SYSTEMobj->IO->current_A - offset_Ia;
SYSTEMobj->DRIVE->currentABC.current_Ib = SYSTEMobj->IO->current_B - offset_Ib;
SYSTEMobj->DRIVE->currentABC.current_Ic = SYSTEMobj->IO->current_C - offset_Ic;
SYSTEMobj->DRIVE->voltage_DC = SYSTEMobj->IO->voltage_DC;
/*Process FOC control*/
fast_loop_control(SYSTEMobj->DRIVE);
/*Calculate shaft torque*/
SYSTEMobj->DRIVE->torque_calc = SYSTEMobj->DRIVE->currentPark.current_Iq * SYSTEMobj->DRIVE->motor_data.Kt;
/*Process Electrical Position Observer*/
SYSTEMobj->OBSERVER_El->fullMode_ena = 0; /*Only speed calculation */
speed_observer(SYSTEMobj->OBSERVER_El);
SYSTEMobj->DRIVE->ElSpeed = SYSTEMobj->OBSERVER_El->W;
SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->OBSERVER_El->Teta_rad;// + SYSTEMobj->OBSERVER_El->teta_correction;
//if(SYSTEMobj->DRIVE->ElAngle >=toPI) SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->DRIVE->ElAngle -toPI;
//if(SYSTEMobj->DRIVE->ElAngle <0.f) SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->DRIVE->ElAngle +toPI;
/*Process Mechanical Position Observer */
/*Observer dynamic correction in speed function*/
SYSTEMobj->OBSERVER_M->refresh_ena = 1;
SYSTEMobj->OBSERVER_M->fullMode_ena = 1; /*Full mode observer */
SYSTEMobj->OBSERVER_M->Ml_izm = SYSTEMobj->DRIVE->torque_calc;
SYSTEMobj->OBSERVER_M->L0 = mod(SYSTEMobj->OBSERVER_M->W)*-0.01f+0.93f;
if(SYSTEMobj->OBSERVER_M->L0 < 0.9f ) SYSTEMobj->OBSERVER_M->L0 = 0.90f;
speed_observer(SYSTEMobj->OBSERVER_M);
SYSTEMobj->DRIVE->speed_Fdb = SYSTEMobj->OBSERVER_M->W;
if(SYSTEMobj->DRIVE->CMD.cmd.bit_RST) /*Clear fault control*/
{
CLEAR_HWfault(SYSTEMobj->DRIVE->drive_ID);
SYSTEMobj->DRIVE->ERRORS.errors_word = 0;
SYSTEMobj->DRIVE->CMD.cmd.bit_RST = 0;
SYSTEMobj->DRIVE->STAT.stat.bit_FAULT = 0;
}
if(SYSTEMobj->DRIVE->CMD.cmd.bit_ON && (!SYSTEMobj->DRIVE->STAT.stat.bit_FAULT))
{
SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED = 1; //Enable Drive
}else
{
SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED = 0; //Disable Drive
}
/*Calculate PWM reference values*/
float inv_dc = 1.0f / (SYSTEMobj->DRIVE->voltage_DC * VoltageNormalisation_coeff);
SYSTEMobj->PWM->Ualfa = DRIVE.voltage_Valpha_ref*inv_dc;
SYSTEMobj->PWM->Ubeta = DRIVE.voltage_Vbeta_ref*inv_dc;
SYSTEMobj->PWM->Uo = 0 * inv_dc;
SYSTEMobj->PWM->Trpd_pwm = GET_PWM_Period(SYSTEMobj->DRIVE->drive_ID);
PWM_proc(SYSTEMobj->PWM);
/*PWM module control*/
//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH1N, LL_TIM_OCMODE_FORCED_ACTIVE);
//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH2N, LL_TIM_OCMODE_FORCED_ACTIVE);
//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH3N, LL_TIM_OCMODE_FORCED_ACTIVE);
if(SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED && (!SYSTEMobj->DRIVE->STAT.stat.bit_FAULT))
{
PWM_COMPARE_SET(SYSTEMobj->DRIVE->drive_ID,(uint16_t)SYSTEMobj->PWM->Ta,(uint16_t)SYSTEMobj->PWM->Tb,(uint16_t)SYSTEMobj->PWM->Tc);
PWM_OUT_ENABLE(SYSTEMobj->DRIVE->drive_ID);
}else
{
uint16_t HALF_period = GET_PWM_Period(SYSTEMobj->DRIVE->drive_ID)>>1;
PWM_COMPARE_SET(SYSTEMobj->DRIVE->drive_ID,HALF_period,HALF_period,HALF_period);
PWM_OUT_DISABLE(SYSTEMobj->DRIVE->drive_ID);
}
}
/*Rate limiter function for Theta */
static inline void Rate_limiter(float *Rated_value, float Ref_value, float Up_rate, float Down_rate,float Tsample)
{
const float Down = Tsample*Down_rate;
const float Up = Tsample*Up_rate;
if((Ref_value - *Rated_value) >=PI)
{
*Rated_value = (*Rated_value+toPI);
}
if((Ref_value - *Rated_value)<=-PI)
{
*Rated_value = (*Rated_value-toPI);
}
if (Ref_value >(*Rated_value + Up))
{
*Rated_value += Up;
}
else if (Ref_value <(*Rated_value - Down))
{
*Rated_value -= Down;
}
else *Rated_value = Ref_value;
}
void SLOW_loop(SYSTEMobj_st *SYSTEMobj)
{
slow_loop_control(SYSTEMobj->DRIVE);
}

18
App/ControlFuncs.h Normal file
View file

@ -0,0 +1,18 @@
/*
* ControlFuncs.h
*
* Created on: 20 авг. 2023 г.
* Author: on4ip
*/
#ifndef CONTROLFUNCS_H_
#define CONTROLFUNCS_H_
#include "main.h"
#include "SystemAPI.h"
#include "InitDrive.h"
#include "PositionReg.h"
void FAST_loop(SYSTEMobj_st *SYSTEMobj);
void SLOW_loop(SYSTEMobj_st *SYSTEMobj);
#endif /* CONTROLFUNCS_H_ */

3
App/FOC/Cogdata.h Normal file
View file

@ -0,0 +1,3 @@
float Current_array[]={-0.007574,0.003754,0.015231,0.040438,0.045051,0.043609,0.048348,0.056742,0.063955,0.079244,0.081118,0.088903,0.086810,0.105881,0.112278,0.119832,0.128995,0.129284,0.123388,0.125845,0.116228,0.113242,0.105548,0.098451,0.092553,0.087468,0.081290,0.067852,0.064801,0.069110,0.066134,0.055019,0.039523,0.040079,0.036484,0.020063,0.013423,0.005789,-0.000393,0.008228,0.020559,0.044342,0.048407,0.044271,0.051735,0.064885,0.081854,0.088148,0.100017,0.103639,0.065694,0.122402,0.029911,0.164267,0.174522,0.186502,0.192211,0.196407,0.197971,0.212691,0.217595,0.205331,0.218105,0.223899,0.229920,0.230518,0.232076,0.230831,0.233037,0.236825,0.229424,0.222815,0.215736,0.232758,0.228465,0.244264,0.230630,0.236270,0.242571,0.248782,0.247151,0.243876,0.245256,0.245006,0.242141,0.256758,0.251909,0.246195,0.255093,0.248715,0.246769,0.238611,0.234933,0.241456,0.248160,0.238488,0.235067,0.237699,0.237625,0.243557,0.246715,0.242855,0.238425,0.234536,0.232238,0.230251,0.226972,0.207739,0.193721,0.187547,0.177137,0.175983,0.173685,0.179271,0.182272,0.173638,0.169415,0.126220,0.134509,0.110410,0.099453,0.079246,0.064891,0.053836,0.039861,0.025426,0.022797,0.007746,0.022078,0.029509,0.050831,0.052915,0.064429,0.068002,0.081636,0.108871,0.113873,0.122359,0.133935,0.143590,0.135670,0.156909,0.165116,0.174087,0.181285,0.179406,0.168003,0.178278,0.178872,0.177982,0.183620,0.183068,0.173011,0.158399,0.167473,0.154510,0.146152,0.118701,0.134026,0.138247,0.130356,0.133843,0.116477,0.106034,0.092952,0.078942,0.075582,0.066560,0.051597,0.024423,0.014059,0.013293,0.013710,0.030875,0.030450,0.024217,0.025568,0.027044,0.035378,0.042489,0.053006,0.051027,0.069324,0.065381,0.112495,0.121506,0.142786,0.144825,0.146196,0.161137,0.163018,0.170331,0.182000,0.186608,0.194815,0.194399,0.197921,0.187889,0.186629,0.194487,0.197680,0.201264,0.208015,0.218639,0.225600,0.220423,0.251910,0.231905,0.232651,0.225305,0.218432,0.214373,0.217027,0.219000,0.215919,0.215762,0.209071,0.210412,0.219614,0.212787,0.202476,0.202289,0.187157,0.166421,0.160244,0.154056,0.142918,0.147784,0.201792,0.159241,0.147841,0.140655,0.115003,0.091783,0.074009,0.065654,0.058071,0.042243,0.035442,0.012616};
uint16_t Positio_array[]={25,56,102,132,159,186,213,241,268,308,333,358,383,407,430,453,481,500,517,534,551,569,587,612,631,649,668,687,706,725,741,766,783,800,818,836,853,871,899,918,937,956,972,988,1005,1029,1045,1061,1076,1090,1105,1120,1142,1156,1168,1180,1189,1200,1209,1220,1235,1245,1254,1264,1274,1282,1290,1302,1310,1316,1323,1331,1340,1349,1358,1369,1378,1387,1395,1401,1409,1414,1419,1425,1431,1437,1442,1450,1455,1459,1464,1471,1479,1486,1494,1506,1514,1523,1534,1543,1551,1559,1571,1580,1590,1600,1608,1618,1629,1651,1666,1681,1697,1713,1727,1741,1765,1781,1798,1818,1840,1863,1888,1914,1954,1981,2011,2041,2074,2107,2136,2178,2207,2235,2262,2290,2315,2342,2377,2399,2422,2444,2462,2479,2495,2516,2530,2544,2558,2573,2587,2601,2621,2636,2651,2666,2682,2698,2714,2730,2752,2765,2779,2792,2807,2823,2839,2862,2877,2894,2911,2930,2949,2975,2993,3008,3026,3042,3059,3075,3093,3117,3134,3152,3167,3183,3197,3210,3223,3242,3254,3267,3280,3292,3303,3315,3331,3341,3353,3365,3375,3387,3399,3414,3423,3432,3441,3449,3463,3470,3479,3489,3499,3510,3520,3532,3550,3561,3572,3584,3595,3606,3619,3638,3654,3671,3688,3708,3724,3742,3767,3784,3803,3823,3845,3869,3896,3935,3962,3989};

427
App/FOC/FOC_Control.c Normal file
View file

@ -0,0 +1,427 @@
/*
* FOC_Control.c
*
* Created on: 3 июн. 2021 г.
* Author: a.ermakov
*/
#include "PlatformMath.h"
#include "pid_reg.h"
#include "FOC_Control.h"
#include "FuncRestrict.h"
//const float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1] __attribute__((section(".RamFunc")));
/*Constants*/
const float lag_correction=1.5f;
float dtime_volt = 0; /*Анпряжение мертвого времени*/
/*Struct definition*/
PIDREG3 Pid_Iq = PIDREG3_DEFAULTS;
PIDREG3 Pid_Id = PIDREG3_DEFAULTS;
PIDREG3 Pid_Idc = PIDREG3_DEFAULTS;
PIDREG3 Pid_FW = PIDREG3_DEFAULTS;
PIDREG3 Pid_W = PIDREG3_DEFAULTS;
PARAM_st PARAMETERS = {0};
FuncRestricts_st RESTRICTS = {0};
DRIVE_st DRIVE={.drive_parameters = &PARAMETERS,
.ppid_Iq = &Pid_Iq,
.ppid_Id = &Pid_Id,
.ppid_Idc = &Pid_Idc,
.ppid_FW = &Pid_FW,
.ppid_Speed = &Pid_W,
.prestricts = &RESTRICTS};
union options_un actual_options = {0};
extern float Icog_compenstion;
/*Special mode used during HAND debug*/
#define HAND_MODE 0
float Teta_norm = 0;
volatile float Teta_shift=0;
enum ANGLE_REF
{
ZERO_ANGLE=0,
PLUS_2PIdiV3,
MINUS_2PIdiV3
};
enum ANGLE_REF WORK_ANGLE=ZERO_ANGLE;
float Teta_ref[3]={0,2.094395f,-2.094395f};
float ElTheta_Hand=0; /*Угол говорота в ручном режиме*/
float ElSpeed_Hand=0; /*Скорость поворота в ручном режиме*/
float voltage_Vref_hand = 0; /*Задание понапряжению в вольтах в ручном режиме*/
float voltage_Vref_hand_d = 0; /*Задание понапряжению в вольтах в ручном режиме*/
/****************************************************************************************************************/
/***********Attantion********************************************************************************************/
/* @detail For control Motor in DQ reference frame we should use R S values scales from 3-Phase to 2 Phase model basically
* it is 1.5 value. SO if we have R-phase in QD model we divide it to 1.5 for Real Phase resistance.
* During PI current loop turning we should use DQ model values of L and R/
* Kp it is Ldq*Bandwidth ( Rads????)
* Ki it is L/R*Tsample for discrete current controller
*/
/**
* @brief This function transforms stator values a and b (which are
* directed along axes each displaced by 120 degrees) into values
* alpha and beta in a stationary qd reference frame.
* alpha = a
* beta = -(2*b+a)/sqrt(3)
*
*
*
**/
static inline void Clark(DRIVE_st *drive)
{
//* alpha = a
//* beta = -(2*b+a)/sqrt(3)
drive->currentClark.current_Ialpha = twoDIVthree*(drive->currentABC.current_Ia - 0.5f*drive->currentABC.current_Ib-0.5f*drive->currentABC.current_Ic);
drive->currentClark.current_Ibeta = invSQRT_3*(drive->currentABC.current_Ib-drive->currentABC.current_Ic);
}
/**
* @brief This function transforms stator values alpha and beta, which
* belong to a stationary qd reference frame, to a rotor flux
* synchronous reference frame (properly oriented), so as q and d.
* d= alpha *cos(theta)+ beta *sin(Theta)
* q= -alpha *sin(Theta)+ beta *cos(Theta)
**/
static inline void Park(DRIVE_st *drive)
{
static float sin_El=0,cos_El=0;
platform_sincos(drive->ElAngle, &sin_El, &cos_El);
drive->currentPark.current_Id = drive->currentClark.current_Ialpha*cos_El+drive->currentClark.current_Ibeta*sin_El;
drive->currentPark.current_Iq = -drive->currentClark.current_Ialpha*sin_El+drive->currentClark.current_Ibeta*cos_El;
}
/**
* @brief invers Park transform function
* @detail inverse transform
* alpha = d*cos(theta)-beta*sin(theta)
* beta = d*sin(theta)+beta*cos(theta)
*/
static inline void InversePark(DRIVE_st *drive)
{
static float sin_El=0,cos_El=0;
platform_sincos(drive->ElAngle,&sin_El,&cos_El);
drive->voltage_Valpha_ref =( drive->voltage_Vd_ref)*cos_El-( drive->voltage_Vq_ref)*sin_El;
drive->voltage_Vbeta_ref =( drive->voltage_Vd_ref)*sin_El+( drive->voltage_Vq_ref)*cos_El;
}
static inline void PowerCalc(DRIVE_st* drive)
{
drive->power_Pdc = (drive->currentPark.current_Iq * (drive->voltage_Vq_ref - sign(drive->voltage_Vq_ref) * dtime_volt)+
drive->currentPark.current_Id * (drive->voltage_Vd_ref - sign(drive->voltage_Vd_ref) * dtime_volt));
drive->current_Idc = drive->power_Pdc/(drive->voltage_DC+0.001f);
}
static inline void Rate_limiter(float *Rated_value, float Ref_value, float Up_rate, float Down_rate,float Tsample)
{
const float Down = Tsample*Down_rate;
const float Up = Tsample*Up_rate;
if (Ref_value >(*Rated_value + Up))
{
*Rated_value += Up;
}
else if (Ref_value <(*Rated_value - Down))
{
*Rated_value -= Down;
}
else *Rated_value = Ref_value;
}
float K1_voltage = 0;
float K2_voltage = 0;
float K1_speed = 0;
float K2_speed = 0;
void filters_init(float voltage_tp, float elspeed_tp)
{
K1_voltage = platform_exp(-DRIVE.Tsample_fast / voltage_tp);
K2_voltage = 1.f - K1_voltage;
K1_speed = platform_exp(-DRIVE.Tsample_slow /elspeed_tp);
K2_speed = 1.f - K1_speed;
}
/**
* .brief Zero cancellation function
* \details Use IIR presentation of zero cancel block with equation y(k)=Ki*Ts/(Kp+Ki*Ts)*x(k)+kp/(kp+ki*Ts)*y(k-1)
* \param pid PIDREG3_handle reference
* \param ref Ref value to regulator
* \return filtered value
*/
static float ZeroCancel(PIDREG3_handle pid, float ref)
{
float out_val = 0;
if((pid->Ki>0) || (pid->Kp > 0))
out_val = pid->Ki / (pid->Kp + pid->Ki) * ref + pid->Kp / (pid->Kp + pid->Ki) * pid->Ref;
return(out_val);
}
FAST_RAM void fast_loop_control(DRIVE_st* drive) ;
void fast_loop_control( DRIVE_st* drive)
{
PIDREG3 *const FOC_Id = drive->ppid_Id;
PIDREG3 *const FOC_Iq = drive->ppid_Iq;
PIDREG3 *const FOC_Idc = drive->ppid_Idc;
PIDREG3 *const FOC_FW = drive->ppid_FW;
/*преобразование Кларк*/
Clark(drive);
/*преобразование Парка*/
if(drive->ElAngle>=toPI) drive->ElAngle = drive->ElAngle-toPI;
if(drive->ElAngle<0.f) drive->ElAngle = drive->ElAngle+toPI;
Park(drive);
/*Расчет параметров мощности по DC*/
PowerCalc(drive);
/*Сброс игтеграторов если не работаем*/
if(!drive->STAT.stat.bit_ENABLED)
{
FOC_FW->dft_st_U = 1u; /*Сброс интгеральной составляющей*/
FOC_Id->dft_st_U = 1u; /*Сброс интгеральной составляющей*/
FOC_Iq->dft_st_U = 1u; /*Сброс интгеральной составляющей*/
FOC_Idc->dft_st_U = 1u; /*Сброс интгеральной составляющей*/
}
drive->voltage_VrefFilt = K1_voltage * drive->voltage_VrefFilt + K2_voltage * drive->voltage_Vref;
if(drive->options.options.bit_FluxWeak1) /*Алгоритм ослабления поля 1*/
{
FOC_FW->Ref = drive->voltage_DC * FiledWeaking_coeff * VoltageLimitation_coeff;
FOC_FW->Fdb = drive->voltage_VrefFilt;
FOC_FW->OutMin = drive->drive_parameters->Idz_min;
FOC_FW->OutMax = drive->drive_parameters->Idz_max;
FOC_FW->calc(FOC_FW);
drive->current_Id_ref = FOC_FW->Out;
}
if(drive->options.options.bit_FluxWeak2) /*Алгоритм ослабления поля 2*/
{
FOC_FW->Ref = drive->voltage_DC * FiledWeaking_coeff * VoltageLimitation_coeff;
FOC_FW->Fdb = drive->voltage_VrefFilt;
float KI_BUFF = FOC_FW->Ki;
FOC_FW->Ki = FOC_FW->Ki * 0.001f;
FOC_FW->OutMin = -PI * 0.5f;
FOC_FW->OutMax = 0;
FOC_FW->calc(FOC_FW);
FOC_FW->Ki = KI_BUFF;
static float sin_fw = 0, cos_fw = 0;
platform_sincos(FOC_FW->Out, &sin_fw, &cos_fw);
drive->current_Id_ref = sin_fw * mod(drive->drive_parameters->Idz_min);
if (drive->current_Id_ref < drive->drive_parameters->Idz_min) drive->current_Id_ref = drive->drive_parameters->Idz_min;
}
/*Вычисление задания по напряжению оси D*/
FOC_Id->Ref = ZeroCancel(FOC_Id, drive->current_Id_ref);
FOC_Id->Fdb = drive->currentPark.current_Id;
FOC_Id->OutMax = drive->voltage_DC;
FOC_Id->OutMin = -FOC_Id->OutMax;
FOC_Id->calc(FOC_Id);
/*Формирование здания по току*/
/*Вычисление максимального тока оси q в функции тока Idc*/
if (drive->STAT.stat.bit_DRIVE_MODE == MOTOR)
{
FOC_Idc->Ref = drive->drive_parameters->Ibattery_dischargeMax;
FOC_Idc->OutMax = mod(drive->drive_parameters->Torque_current_max);
}
else
{
FOC_Idc->Ref = drive->drive_parameters->Ibattery_chargeMax;
FOC_Idc->OutMax = mod(drive->drive_parameters->Brake_current_max);
}
FOC_Idc->Fdb = mod(drive->current_Idc);
FOC_Idc->OutMax = mod(drive->torque_Mz_limited);
FOC_Idc->OutMin = 0;
FOC_Idc->calc(FOC_Idc);
//drive->torque_Mz_avaible = (FOC_Idc->Out);
/*Ограничение по доступному току*/
if (drive->torque_Mz_limited > (FOC_Idc->Out)) drive->current_Iq_ref = (FOC_Idc->Out);
else if (drive->torque_Mz_limited < (-(FOC_Idc->Out))) drive->current_Iq_ref = (-(FOC_Idc->Out));
else drive->current_Iq_ref = drive->torque_Mz_limited;
/*Вычисление задания по напряжению оси Q*/
FOC_Iq->Ref = ZeroCancel(FOC_Iq, drive->current_Iq_ref);
FOC_Iq->Fdb = drive->currentPark.current_Iq;
FOC_Iq->OutMax = drive->voltage_DC;
FOC_Iq->OutMin = -FOC_Iq->OutMax;
FOC_Iq->calc(FOC_Iq);
/*Обработка алгоритма feedforward*/
if(drive->options.options.bit_Decouple){
drive->voltage_Vd_decoupl=drive->motor_data.Rs*FOC_Id->Fdb - drive->ElSpeed*drive->motor_data.Lq *FOC_Iq->Fdb;
drive->voltage_Vq_decoupl=drive->motor_data.Rs*FOC_Iq->Fdb + drive->ElSpeed*drive->motor_data.Ld *FOC_Id->Fdb + drive->ElSpeed*drive->motor_data.Emf;
}
else{
drive->voltage_Vd_decoupl =0;
drive->voltage_Vq_decoupl =0;
}
#if 1
drive->voltage_Vq_ref = FOC_Iq->Out + drive->voltage_Vq_decoupl;
drive->voltage_Vd_ref = FOC_Id->Out + drive->voltage_Vd_decoupl;
#else
drive->voltage_Vq_ref = voltage_Vref_hand + drive->torque_Mz;
drive->voltage_Vd_ref = voltage_Vref_hand_d;
#endif
#if 1
/*Обработка лимитирования выходного напряжения*/
drive->voltage_Vref = platform_sqrt(drive->voltage_Vd_ref * drive->voltage_Vd_ref + drive->voltage_Vq_ref * drive->voltage_Vq_ref);
float max_amp = (drive->voltage_DC-dtime_volt) * VoltageNormalisation_coeff;
float klim = max_amp / (drive->voltage_Vref + 0.1f);
if (klim < 1.0f){
drive->voltage_Vq_ref = drive->voltage_Vq_ref * klim;
drive->voltage_Vd_ref = drive->voltage_Vd_ref * klim;
}
#else
drive->voltage_Vref = platform_sqrt(drive->voltage_Vd_ref * drive->voltage_Vd_ref + drive->voltage_Vq_ref * drive->voltage_Vq_ref);
float max_vd = (drive->voltage_DC - dtime_volt) * VoltageNormalisation_coeff * 0.9f;
if (drive->voltage_Vd_ref > max_vd) drive->voltage_Vd_ref = max_vd;
if (drive->voltage_Vd_ref <-max_vd) drive->voltage_Vd_ref = -max_vd;
float max_vq = platform_sqrt(drive->voltage_DC * drive->voltage_DC * VoltageNormalisation_coeff * VoltageNormalisation_coeff - drive->voltage_Vd_ref * drive->voltage_Vd_ref);
if (drive->voltage_Vq_ref > max_vd) drive->voltage_Vq_ref = max_vq;
if (drive->voltage_Vq_ref < -max_vd) drive->voltage_Vq_ref = -max_vq;
#endif
#if !HAND_MODE
/*Обратное преобразование Парка */
if(mod(drive->ElSpeed) > 50.f) drive->ElAngle = drive->ElAngle + drive->Tsample_fast* drive->ElSpeed * lag_correction;
if(drive->ElAngle>=toPI) drive->ElAngle = drive->ElAngle-toPI;
if(drive->ElAngle<0.f) drive->ElAngle = drive->ElAngle+toPI;
InversePark(drive);
#else
static float cos_teta_el=0,sin_teta_el=0;
ElTheta_Hand=ElTheta_Hand+drive->Tsample_fast*ElSpeed_Hand;
if(ElTheta_Hand>=toPI) ElTheta_Hand=ElTheta_Hand-toPI;
if(ElTheta_Hand<0) ElTheta_Hand=ElTheta_Hand+toPI;
platform_sincos((ElTheta_Hand+Teta_ref[WORK_ANGLE]),&sin_teta_el,&cos_teta_el);
drive->voltage_Valpha_ref =voltage_Vref_hand*cos_teta_el;
drive->voltage_Vbeta_ref =voltage_Vref_hand*sin_teta_el;
#endif
}
#define THRESHOLD 0.0000005f
_inline float Speed_Control(DRIVE_st* drive)
{
PIDREG3 *const FOC_Speed = drive->ppid_Speed;
/*Ограничение задания по скорости*/
if(drive->speed_Ref>drive->drive_parameters->Max_speed) drive->speed_Ref = drive->drive_parameters->Max_speed;
if(drive->speed_Ref<drive->drive_parameters->Min_speed) drive->speed_Ref = drive->drive_parameters->Min_speed;
/*Сохраненеие знака скорости*/
int sign_Fz=sign(drive->speed_Fdb);
float slope_up = drive->Tsample_slow*drive->drive_parameters->Speed_ramp_Up;
float slope_down = drive->Tsample_slow*drive->drive_parameters->Speed_ramp_Down;
if(sign_Fz>0)
{
if(drive->speed_Ref > drive->speed_Ref_rated+slope_up)
{
drive->speed_Ref_rated+=slope_up;
}
else if (drive->speed_Ref < drive->speed_Ref_rated-slope_down)
{
drive->speed_Ref_rated-=slope_down;
}
else drive->speed_Ref_rated = drive->speed_Ref;
}else
{
if(drive->speed_Ref > drive->speed_Ref_rated+slope_down)
{
drive->speed_Ref_rated+=slope_down;
}
else if (drive->speed_Ref <drive->speed_Ref_rated-slope_up)
{
drive->speed_Ref_rated-=slope_up;
}
else drive->speed_Ref_rated = drive->speed_Ref;
}
/*Сброс интегральной составляющей скорости*/
if(!drive->STAT.stat.bit_ENABLED)
{
FOC_Speed->dft_st_U = 1;
}
FOC_Speed->Ref = drive->speed_Ref_rated;
FOC_Speed->Fdb = drive->speed_Fdb;
FOC_Speed->OutMax = drive->drive_parameters->Torque_current_max;
FOC_Speed->OutMin = -drive->drive_parameters->Brake_current_max;
FOC_Speed->calc(FOC_Speed);
if(mod(FOC_Speed->Out)<THRESHOLD) FOC_Speed->Out = 0;
return (FOC_Speed->Out);
}
float test_t = 0;
FAST_RAM void slow_loop_control(DRIVE_st* drive) ;
void slow_loop_control(DRIVE_st* drive)
{
register FuncRestricts_st *const RESTRICTS = drive->prestricts;
register float Mz_buffer = 0;
register float Work_ramp_Up=0;
register float Work_ramp_Down=0;
/*Машина состояний режимов*/
drive->STAT.stat.bit_SPEED_MODE = 0;
drive->STAT.stat.bit_REVERSE_MODE = 0;
switch (drive->CMD.cmd.bit_CTRL_MODE)
{
case SPEED:
{
drive->torque_Mz = Speed_Control(drive);
Mz_buffer = drive->torque_Mz;
Work_ramp_Up = 10000;
Work_ramp_Down = 10000;
drive->STAT.stat.bit_SPEED_MODE = 1;
break;
}
case TORQUE:
{
/*teoretical FULL torque region speed*/
float a = drive->motor_data.Emf * drive->motor_data.Emf + drive->drive_parameters->Torque_current_max * drive->drive_parameters->Torque_current_max * drive->motor_data.Ld * 1.5f * drive->motor_data.Ld * 1.5f;
float b = 2.f * drive->drive_parameters->Torque_current_max * drive->motor_data.Rs * 1.5f * drive->motor_data.Emf;
float c = drive->drive_parameters->Torque_current_max * drive->drive_parameters->Torque_current_max * drive->motor_data.Rs * 1.5f * drive->motor_data.Rs * 1.5f - drive->voltage_DC * drive->voltage_DC * VoltageNormalisation_coeff * VoltageNormalisation_coeff;
float speed_to_TRQlimit = (-b + platform_sqrt(b * b - 4.f * a * c)) / (2.f * a);
float torq2power_coef = speed_to_TRQlimit * 0.9f / (mod(drive->ElSpeed) + 0.1f);
if (torq2power_coef < 1.f) drive->torque_Mz_avaible = drive->drive_parameters->Torque_current_max * torq2power_coef;
else
{
drive->torque_Mz_avaible = drive->drive_parameters->Torque_current_max;
}
register float abs_Mz = mod(drive->torque_Mz_avaible);
if (drive->torque_Mz > abs_Mz) Mz_buffer = abs_Mz;
else if (drive->torque_Mz < -abs_Mz) Mz_buffer = -abs_Mz;
else Mz_buffer = drive->torque_Mz; //Buffer variable
Work_ramp_Up = drive->drive_parameters->Torque_ramp_Up;
Work_ramp_Down = drive->drive_parameters->Torque_ramp_Down;
break;
}
case REGEN_BRAKING:
{
Mz_buffer = -drive->torque_Mz * sat_sign(drive->ElSpeed , 0.005f);
Work_ramp_Up = drive->drive_parameters->Brake_ramp_Up;
Work_ramp_Down = drive->drive_parameters->Brake_ramp_Down;
break;
}
case SWITCH_BRAKING:
{
Mz_buffer = -drive->torque_Mz * sat_sign(drive->ElSpeed, 0.005f);
Work_ramp_Up = drive->drive_parameters->Brake_ramp_Up;
Work_ramp_Down = drive->drive_parameters->Brake_ramp_Down;
break;
}
case POSTION:
{
break;
}
}//end if switch
/*Calculate motor torque*/
drive->torque_calc = 1.5f * 0.5774f * drive->currentPark.current_Iq * drive->motor_data.Emf*drive->motor_data.P;
/*Calculate motor power*/
drive->power_Mech = drive->torque_calc * drive->ElSpeed / (drive->motor_data.P + 0.01f);
/*Calculate Flux Shift*/
drive->flux_shift_calc = drive->flux_shift_calc * K1_speed+ platform_atan2(mod(drive->voltage_Vd_ref),mod(drive->voltage_Vq_ref)) * K2_speed;
if(mod(drive->ElSpeed)>50.f)
{
drive->flux_calc = K1_speed * drive->flux_calc + (drive->voltage_Vq_ref/drive->ElSpeed) * K2_speed;
}
/*Rate limiter*/
Rate_limiter(&drive->torque_Mz_rated, Mz_buffer, Work_ramp_Up, Work_ramp_Down, drive->Tsample_slow);
/*Индикация режиа работы привода по квадранту*/
if(sign(drive->ElSpeed * drive->torque_Mz_rated) >= 0) drive->STAT.stat.bit_DRIVE_MODE = MOTOR;
else drive->STAT.stat.bit_DRIVE_MODE = GENERATOR;
/*Блок функциональных ограничений*/
float m_max = 0;
m_max = drive->torque_Mz_rated * ProcessRestricts(drive->ElSpeed,RESTRICTS,PositiveSpeed_constraint);
m_max = m_max * ProcessRestricts(drive->ElSpeed,RESTRICTS,NegativeSpeed_constraint);
if(drive->STAT.stat.bit_DRIVE_MODE == GENERATOR)
m_max = m_max * ProcessRestricts(drive->voltage_DC,RESTRICTS,HighVolt_constraint);
else
m_max = m_max * ProcessRestricts(drive->voltage_DC,RESTRICTS,LowVolt_constraint);
m_max = m_max * ProcessRestricts(test_t,RESTRICTS,HighTemperature1_constraint);
m_max = m_max * ProcessRestricts(test_t,RESTRICTS,LowTemparature1_constraint);
drive->torque_Mz_limited = m_max;
}

249
App/FOC/FOC_Control.h Normal file
View file

@ -0,0 +1,249 @@
/*
* FOC_Control.h
*
* Created on: 3 июн. 2021 г.
* Author: a.ermakov
*/
#ifndef APPLICATION_USER_FOC_CONTROL_H_
#define APPLICATION_USER_FOC_CONTROL_H_
#include <stdint.h>
#define FiledWeaking_coeff 0.75f
#define VoltageLimitation_coeff 0.866f
#define VoltageNormalisation_coeff 0.866f
/*structs*/
enum ControlMode
{
TORQUE = 0,
SPEED,
REGEN_BRAKING,
SWITCH_BRAKING,
PARK_BRAKING,
POSTION,
DCREG
};
enum AlgMode
{
EMPTY=0,
FOC,
SENSORSLESS,
BLDC,
SINE,
ACIM
};
enum DetectMode
{
STOP_DETECT=0,
motorRS,
motorLS,
HALL_STEP,
HALL_ROTARY,
EMF
};
/*!
* \brief Stucture for advanced options of control
*/
typedef struct
{
uint16_t bit_ALG_MODE:3; /*COntrol algorithm mode */
uint16_t bit_FluxWeak1:1; /*Flux weaking mode with voltage lim reg*/
uint16_t bit_FluxWeak2:1; /*Flux weaking mode with voltage angle reg*/
uint16_t bit_Decouple:1; /*Decoupling mode */
uint16_t bit_PWM_OneLeg:1; /*Discontinuous PWM mode*/
}options_st;
union options_un
{
options_st options;
uint16_t options_word;
};
extern union options_un actual_options;
/*!
*
* \brief Stucture with COmmand bit defination
*/
typedef struct
{
uint16_t bit_ON:1; /*Modulation ON command bit*/
uint16_t bit_RST:1; /*Reset Bit*/
uint16_t bit_BRAKE:1; /*Braking ON command bit*/
uint16_t bit_CTRL_MODE:3; /*Control mode*/
uint16_t bit_DETECT_MODE:5; /*Detect mode*/
uint16_t bit_REF_INVERSE:1; /*Reference inversion mode command bit*/
uint16_t bit_PHASE_INVERS:1; /*Phase inversion mode command bit*/
uint16_t bit_SPEED_MODE:1; /*Sine control mode command bit*/
//uint16_t bit_FOC:1; /*FOC control mode command bit*/
//uint16_t bit_SENSORLESS:1; /*SensorlEss mode command bit*/
uint16_t bit_ALG_MODE:3; /*COntrol algorithm mode */
}command_st;
enum DRIVE_MODE{
MOTOR=0,
GENERATOR
};
/*!
*
* \brief Stucture with Status bit defination
*/
typedef struct
{
uint16_t bit_READY1:1; /*1st ready bit NO error*/
uint16_t bit_READY2:1; /*2nd rady bit NO saturation*/
uint16_t bit_ENABLED:1; /*Modulation enabled Bit*/
uint16_t bit_FAULT:1; /*Global fault bit*/
uint16_t bit_DRIVE_MODE:2; /*Work in motor or gen mode bit*/
uint16_t bit_IDENT_MODE:1; /*Identification but*/
uint16_t bit_SPEED_MODE:1; /*Work in SPeed reg mode*/
uint16_t bit_REVERSE_MODE:1; /*WOrk in reverse*/
uint16_t bit_FW_MODE:1; /*Work in Fieldweaking*/
uint16_t bit_SAT_MODE:1; /*WOkr in saturation*/
uint16_t bit_PROFILE:2; /*Active profile*/
uint16_t bit_HALL_DISABLE:1; /*Hall disabledBit*/
}status_st;
/*!
* \brief Structure for Error Bits definitions
*/
typedef struct
{
uint16_t bit_FREE1:1; /*Empty bit*/
uint16_t bit_FREE2:1; /*Empty bit*/
uint16_t bit_AmpCurr_err:1; /*Amplitude current error bit*/
uint16_t bit_Hpower:1; /*Hardware error bit*/
uint16_t bit_Hcurr_err:1; /*Hardware ivercurrent bit*/
uint16_t bit_Hvolt_err:1; /*Hardware overvoltage error bit*/
uint16_t bit_GasFault:1; /*Throttle input error bit*/
uint16_t bit_FREE3:1; /*Empty bit*/
uint16_t bit_OverVolt:1; /*Software overvoltage bit*/
uint16_t bit_OverCurr:1; /*Software overcurrent error error bit*/
uint16_t bit_OverTempBoard:1; /*Software over tempr PowerBoard error bit*/
uint16_t bit_OverTempMotor:1; /*Software over tempr Motor error bit*/
uint16_t bit_OverTempCntr:1; /*Software over tempr COntrol Board error bit*/
uint16_t bit_ISRoverTime:1; /*main ISR over load error bit*/
uint16_t bit_PosSens:1; /*Position sensor error bit*/
}errors_st;
/*!
*
* \brief Structure with limitation parameters used in algorithm
*/
typedef struct
{
float Torque_current_max; ///< Óñòàâêà îãðàíè÷åíèÿ çàäíèÿ òîêà
float Brake_current_max; ///< Óñòàâêà ìàêñèìàëüíîãî òîêà òîðìîæåíèÿ
float Current_Attention; ///< Óñòàâêà äëÿ àâàðèè ïî òîêó Ñðåäíåå çíà÷åíèå òîêà
float Current_Error; ///< Óñòàâêà äëÿ àâàðèè ïî òîêó ÀÌÏËÈÒÓÄÀ ôàçíîãî òîêà
float Udc_max; ///< Óñòàâêà äëÿ àâàðèè ïî ìàêñèìàëüíîìó íàïðÿæåíèþ Udc
float Udc_min; ///< Óñòàâêà äëÿ àâàðèè ïî ìèíèìàëüíîìó íàïðÿæåíèþ Udc
float Wmax_el_err; ///< Óñòàâêà äëÿ àâàðèè ïî ïðåâûøåíèþ ñêîðîñòè
float Tmin_Board; ///< Ìèíèìàëüíàÿ òåìïåðàòóðà ñèëîàîé ïëàòû
float Tmin_Cpu; ///< Ìèíèìàëüíàÿ òåìïåðàòóðà ïðîöåññîðíîé ïëàòû
float Tmin_Wind; ///< Ìèíèìàëüíàÿ òåìïåðàòóðà Îáìîòêè ìîòîðà
float Tmax_Board; ///< Ìàêñèìàëüíàÿ òåìïåðàòóðà ñèëîâîé ïëàòû
float Tmax_Cpu; ///< Ìàêñèìàëüíàÿ òåìïåðàòóðà ïðîöåññîðíîé ïëàòû
float Tmax_Wind; ///< Ìàêñèìàëüíàÿ òåìïåðàòóðà îáìîòêè
float Mz_ogr; ///< Óñòàâêà îãðàíè÷åíèÿ çàäàíèÿ ìîìåíòà
float Uz_ogr; ///< Óñòàâêà îãðàíè÷åíèÿ çàäàíèÿ íàïðÿæåíèÿ
float P_ogr; ///< Óñòàâêà îãðàíè÷íèåÿ ìîùíîñòè
float M_TADgenOgr; ///< Óñòàâêà îãðàíè÷åíèå ãåíåðàòîðíîãî ìîìåíòà ( èñïîëüçóåòñÿ â ôóíêöèîíàëüíûõ îðàíè÷åíèÿõ)
float P_TADgenOgr; ///< Óñòàâêà îãðàíè÷åíèå ãåíåðàòîðíîé ìîùíîñòè ( èñïîëüçóåòñÿ â ôóíêöèîíàëüíûõ îðàíè÷åíèÿõ)
float Ibattery_dischargeMax; ///< Óñòàâêà ìàêñèìàëüíîãî òîêà ðàçðÿäà áàòàðåè
float Ibattery_chargeMax; ///< Óñòàâêà ìàêñèìàëüíî òîêà çàðÿäà áàòàðåè
float Idz_min; ///< Óñòàâêà îãðàíè÷åíèå òîêà íàìàãíè÷èâàíèÿ ñíèçó ïðè ðàáîòå ïðèâîäà..
float Idz_max; ///< Óñòàâêà îãðàíè÷åíèå òîêà íàìàãíè÷èâàíèÿ ñâåðõó ïðè ðàáîòå ïðèâîäà.
float Max_ref_position; ///< Óñòàâêà îãðàíè÷åíèÿ çàäàíèÿ óãëà äëÿ ðåãóëÿòîðà ïîëîæåíèÿ ñâåðõó
float Min_ref_position; ///< Óñòàâêà îãðàíè÷åíèÿ çàäàíèÿ óãëà äëÿ ðåãóëÿòîðà ïîëîæåíèÿ ñíèçó
float Max_speed; ///< Óñòàâêà îãðàíè÷åíèÿ ìàêñèìàëüíîé ñêîðîñòè ïî âûõîäó ðåãóîÿòîðà ïîëîæåíèÿ
float Min_speed; ///< Óñòàâêà îãðàíè÷åíèÿ ìàêñèìàëüíîé ñêîðîñòè ïî âûõîäó ðåãóîÿòîðà ïîëîæåíèÿ
float Speed_ramp_Up; ///< Ðàìïà íàðàñòàíèÿ çàäàíèÿ ñêîðîñòè rad/s
float Speed_ramp_Down; ///< Ðàìïà ñïàäà çàäàíèÿ ñêîðîñòè rad/s0
float Torque_ramp_Up; ///< Ðàìïà íàðàñòàíèÿ çàäàíèÿ ìîìîåíòà ( òîêà) A/s
float Torque_ramp_Down; ///< Ðàìïà ñïàäà çàäàíèÿ ìîìåíòà (òîêà) A/s
float Brake_ramp_Up; ///< Ðàìïà íàðàñòàíèÿ çàäàíèÿ òîðìîçíîãî ìîìîåíòà ( òîêà) A/s
float Brake_ramp_Down; ///< Ðàìïà ñïàäà çàäàíèÿ òîðìîçíîãî ìîìåíòà (òîêà) A/s
} PARAM_st;
/*!
* \brief Structure for Drive definition
*/
typedef struct{
uint16_t drive_ID;
union{
command_st cmd; /*Command word Union*/
uint16_t cmd_word;
}CMD;
union{
errors_st errors;
uint16_t errors_word; /*Errors word union*/
}ERRORS;
union{
status_st stat;
uint16_t stat_word; /*Status word union*/
}STAT;
struct currentABC_st{
float current_Ia;
float current_Ib;
float current_Ic;
}currentABC;
struct currentClark_st{
float current_Ialpha;
float current_Ibeta;
}currentClark;
struct currentPark_st{
float current_Iq;
float current_Id;
}currentPark;
float torque_Mz;
float torque_Mz_rated;
float torque_Mz_limited;
float torque_Mz_avaible;
float speed_Ref;
float speed_Fdb;
float speed_Ref_rated;
float current_Iq_ref;
float current_Id_ref;
float voltage_DC;
float voltage_Vq_ref;
float voltage_Vd_ref;
float voltage_Vd_decoupl;
float voltage_Vq_decoupl;
float voltage_Valpha_ref;
float voltage_Vbeta_ref;
float voltage_Vref;
float voltage_VrefFilt;
float ElAngle;
float ElSpeed;
float ElSpeedFilt;
float temperature_Winding;
float temperature_Power;
float temperature_Control;
float power_Pdc;
float current_Idc;
float torque_calc;
float flux_calc;
float flux_shift_calc;
float power_Mech;
struct motor_st
{
float Rs;
float Ld;
float Lq;
float Emf;
float Kt;
uint16_t P;
float J;
float B;
}motor_data;
float Tsample_fast;
float Tsample_slow;
PARAM_st *drive_parameters;
union options_un options;
void *ppid_Iq;
void *ppid_Id;
void *ppid_Idc;
void *ppid_FW;
void *ppid_Speed;
void *prestricts;
}DRIVE_st;
extern DRIVE_st DRIVE;
extern PARAM_st PARAMETERS;
void filters_init(float, float);
void fast_loop_control(DRIVE_st* );
void slow_loop_control(DRIVE_st* );
#endif /* APPLICATION_USER_FOC_CONTROL_H_ */

22
App/FOC/Filters.c Normal file
View file

@ -0,0 +1,22 @@
/*
* Filters.c
*
* Created on: 16 авг. 2021 г.
* Author: v.timokhin
*/
#include "Filters.h"
lowPass_st GyroLowPass = {0};
void initFilter(lowPass_st *filter,float Tsample,float Tp)
{
filter->k1 = platform_exp(-Tsample/Tp);
filter->k2 = 1.f-filter->k1;
filter->filtVal = 0;
}
void filterProc(lowPass_st* filter,float rawVal)
{
filter->filtVal = filter->k1 * filter->filtVal+filter->k2 * rawVal;
}

22
App/FOC/Filters.h Normal file
View file

@ -0,0 +1,22 @@
/*
* Filters.h
*
* Created on: 16 авг. 2021 г.
* Author: v.timokhin
*/
#ifndef APP_FOC_FILTERS_H_
#define APP_FOC_FILTERS_H_
#include "PlatformMath.h"
#include "stdint.h"
#include "math.h"
typedef struct
{
float k1;
float k2;
float filtVal;
}lowPass_st;
extern lowPass_st GyroLowPass;
void initFilter(lowPass_st *filter,float Tsample,float Tp);
void filterProc(lowPass_st* filter,float rawVal);
#endif /* APP_FOC_FILTERS_H_ */

110
App/FOC/FuncRestrict.c Normal file
View file

@ -0,0 +1,110 @@
#include "PlatformMath.h"
#include "FOC_Control.h"
#include "FuncRestrict.h"
//-------------------------------------------------------
uint16_t InitRestricts(FuncRestricts_st *restricts)
{
uint16_t OgrName=0;
uint16_t Error = 0;
for(OgrName=0;OgrName<Constraints_count;OgrName++)
{
// Ïðîâåðêà ðàçðøåííûõ äèàïàçîíîâ
if( restricts->ActualLimits[OgrName].Value > restricts->AbsoluteLimits[OgrName].ValueMaximum )
{ restricts->ActualLimits[OgrName].Value = restricts->AbsoluteLimits[OgrName].ValueMaximum ;
Error |= ogrErrLimMax;
}
else if(restricts->ActualLimits[OgrName].Value < restricts->AbsoluteLimits[OgrName].ValueMinimum)
{ restricts->ActualLimits[OgrName].Value = restricts->AbsoluteLimits[OgrName].ValueMinimum;
Error |= ogrErrLimMin;
};
if( restricts->ActualLimits[OgrName].Span > restricts->AbsoluteLimits[OgrName].SpanMaximum )
{ restricts->ActualLimits[OgrName].Span = restricts->AbsoluteLimits[OgrName].SpanMaximum;
Error |= ogrErrDiapMax;
}
else if(restricts->ActualLimits[OgrName].Span < restricts->AbsoluteLimits[OgrName].SpanMinumum)
{ restricts->ActualLimits[OgrName].Span = restricts->AbsoluteLimits[OgrName].SpanMinumum;
Error |= ogrErrDiapMin;
}
restricts->Koef[OgrName] = 1.0f/restricts->ActualLimits[OgrName].Span;
}
return Error;
}
float ProcessRestricts(float value, FuncRestricts_st *restricts,enum Constraint_en OgrName)
{
float temp = 1;
switch(OgrName)
{
/*Îãðàíè÷åíèÿ ðàáîòàþò åñëè âåëè÷èíà Val > Limit*/
case HighVolt_constraint:
case PositiveSpeed_constraint:
case HighTemperature1_constraint:
temp = 1.0f - restricts->Koef[OgrName]*(value-restricts->ActualLimits[OgrName].Value);
break;
/*Îãðàíè÷åíèÿ ðàáîòàþò åñëè âåëè÷íà Val < Limit*/
case LowVolt_constraint:
case NegativeSpeed_constraint:
case LowTemparature1_constraint:
temp = 1.0f + restricts->Koef[OgrName]*(value-restricts->ActualLimits[OgrName].Value);
break;
default:
restricts->ContraintActive.ConstraintActive.Error = ogrErrProgErr;
}
if (temp > 1.0f){
restricts->ContraintActive.ConstratinActive_word&=~(1<<OgrName);
return 1.0f;
}
else if (temp < 0.f) { restricts->ContraintActive.ConstratinActive_word |= 1<<OgrName; return 0.f;}
else { restricts->ContraintActive.ConstratinActive_word |= 1<<OgrName; return temp; }
return temp;
}
/*!
*
* @param I_reference
* @param W_real
* @param I_trig
* @return I_control
*/
float Low_speed_Current_derating(float I_reference, float W_real, float I_trig)
{
//Åñëè òîê çàäàíèÿ áîëüøå íåêîé óñòàâêè, ïðåäïîëîæèòåëüíî 30% ìàêñèìàëüíîãî òîêà èíâåðòîðà
//òî íà÷íåì îòñò÷åò âðåìåíè äëÿ ñíèæåíèÿ òîêà.
static float Time_to_derate = 0;
float I_control=1;
//Åñëè ñêîðîñòü äâèãàòåëÿ ìåíüøå ïîðîãà òî îáñëóæèâàåì îãðàíè÷íèå
if(W_real<200)
{
if(I_reference>=(I_trig*1))
{
Time_to_derate+=1e-3;//
//Ã<>à÷íåì îãðàíè÷åíèå åñëè òîê áîëüøå óñòàâêè â òå÷åíèè 5 ñåêóíä.
if(Time_to_derate >=5)
{
I_control = (7-Time_to_derate)*0.5;//Îãðàíè÷íèå ñïîëçåò äî íóëÿ çà 2 ñåêóíäû
if(I_control<=0) I_control = 0;
}//end of if
}
}else{
if(I_reference<=(I_trig*0.95))
{
Time_to_derate-=0.1e-3;
I_control = (7-Time_to_derate)*0.25;//Îãðàíè÷íèå ñïîëçåò äî íóëÿ çà 2 ñåêóíäû
if(I_control>=1) I_control = 1;
}
}//end of else
if(Time_to_derate <=0) Time_to_derate = 0; //÷òîáû âðåìÿ íåáûëî îòðèöàòåëüíûì
if(Time_to_derate >=7) Time_to_derate = 7;
float res_current = (I_control*(I_reference-I_trig)+I_trig);
if(I_control >=1) return (I_reference);
if(I_control <=0) return (res_current);
if(res_current < I_trig) return (I_trig);
else return (res_current);
}

87
App/FOC/FuncRestrict.h Normal file
View file

@ -0,0 +1,87 @@
/*
* FuncRestrict.h
*
* Created on: 13 <EFBFBD><EFBFBD><EFBFBD>. 2015 <EFBFBD>.
* Author: Andrey Ermakov
*/
#ifndef BLDC_FOC_HALL_INCLUDE_FUNCRESTRICT_H_
#define BLDC_FOC_HALL_INCLUDE_FUNCRESTRICT_H_
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> OgrManage.Err
#define ogrErrLimMin 0x0100 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
#define ogrErrLimMax 0x0200 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
#define ogrErrDiapMin 0x0400 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
#define ogrErrDiapMax 0x0800 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
#define ogrErrProgErr 0x0001 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>) ogrErrAbsWrong
#define ogrErrAbsWrong 0x0002 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>
enum Constraint_en{
HighVolt_constraint=0,
LowVolt_constraint,
PositiveSpeed_constraint,
NegativeSpeed_constraint,
HighTemperature1_constraint,
HighTemperature2_constraint,
HighTemperature3_constarint,
LowTemparature1_constraint,
LowTemparature2_constraint,
LowTemparature3_constraint,
Constraints_count
};
struct OgrManageStruct {
unsigned int OgrStat; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
unsigned int ZeroStat; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
unsigned int Err; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>. bitdefinition.h
};
struct OgrLimitsStruct {
float ValueMinimum; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float ValueMaximum; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float SpanMinumum; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float SpanMaximum; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
};
struct OgrStruct
{ float Value; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Span; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
};
typedef struct
{
union{
uint16_t ConstratinActive_word;
struct ContraintActive_st
{
uint16_t HighVolt_constraint:1;
uint16_t LowVolt_constraint:1;
uint16_t PositiveSpeed_constraint:1;
uint16_t NegativeSpeed_constraint:1;
uint16_t HighTemperature1_constraint:1;
uint16_t HighTemperature2_constraint:1;
uint16_t HighTemperature3_constarint:1;
uint16_t LowTemparature1_constraint:1;
uint16_t LowTemparature2_constraint:1;
uint16_t LowTemparature3_constraint:1;
uint16_t Error:6;
}ConstraintActive;
}ContraintActive;
struct OgrLimitsStruct AbsoluteLimits[Constraints_count];
struct OgrStruct ActualLimits[Constraints_count];
float Koef[Constraints_count];
}FuncRestricts_st;
extern FuncRestricts_st RESTRICTS;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>!)
struct TOgrType
{ float TwinOgr; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Twin_Zero; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Tboard; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Tboard_Zero;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Tboard_min; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Tboard_min_Zero;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
};
uint16_t InitRestricts(FuncRestricts_st *restricts);
float ProcessRestricts(float value, FuncRestricts_st *restricts,enum Constraint_en OgrName);
float Low_speed_Current_derating(float I_reference, float W_real, float I_trig);
#endif /* BLDC_FOC_HALL_INCLUDE_FUNCRESTRICT_H_ */

49
App/FOC/IOdata.c Normal file
View file

@ -0,0 +1,49 @@
/*
* IOdata.c
*
* Created on: 3 июн. 2021 г.
* Author: a.ermakov
*/
#include "IOdata.h"
IO_settings_st IO_settings_ext={0};
IO_settings_st IO_settings_int={0};
IO_st IO={.IO_coefs=&IO_settings_int};
void set_IOSettings(IO_st *io,IO_settings_st* settings)
{
io->IO_coefs->ADC_A_Ku = settings->ADC_A_Ku;
io->IO_coefs->ADC_A_shift = settings->ADC_A_shift;
io->IO_coefs->ADC_B_Ku = settings->ADC_B_Ku;
io->IO_coefs->ADC_B_shift = settings->ADC_B_shift;
io->IO_coefs->ADC_C_Ku = settings->ADC_C_Ku;
io->IO_coefs->ADC_C_shift = settings->ADC_C_shift;
io->IO_coefs->ADC_Udc_Ku = settings->ADC_Udc_Ku;
io->IO_coefs->ADC_Udc_shift = settings->ADC_Udc_shift;
}
static void convert_IOdata(IO_st* io,const uint16_t mode)
{
io->current_A = io->ADC_A*io->IO_coefs->ADC_A_Ku + io->IO_coefs->ADC_A_shift;
io->current_B = io->ADC_B*io->IO_coefs->ADC_B_Ku + io->IO_coefs->ADC_B_shift;
if(io->ADC_C)
{
io->current_C = io->ADC_C*io->IO_coefs->ADC_C_Ku + io->IO_coefs->ADC_C_shift;
}else io->current_C = -io->current_B-io->current_A;
io->voltage_DC = io->ADC_Udc*io->IO_coefs->ADC_Udc_Ku + io->IO_coefs->ADC_Udc_shift;
}
void update_IO_ADC(IO_st* io,uint16_t *ADC_data,const uint16_t mode)
{
io->ADC_A = (ADC_data[0] + ADC_data[1]) >> 1 ;
io->ADC_B = (ADC_data[2] + ADC_data[3]) >> 1 ;
io->ADC_C = 0;
io->ADC_Udc = ADC_data[5];
io->ADC_VrefInt = ADC_data[4];
convert_IOdata(io,mode);
}
void update_IO_Sensor(IO_st* io,uint16_t pos_cnt)
{
io->ENC_CNT_AB = pos_cnt;
}

49
App/FOC/IOdata.h Normal file
View file

@ -0,0 +1,49 @@
/*
* IOdata.h
*
* Created on: 3 июн. 2021 г.
* Author: a.ermakov
*/
#ifndef APPLICATION_USER_IODATA_H_
#define APPLICATION_USER_IODATA_H_
#include "stdint-gcc.h"
typedef struct
{
float ADC_A_Ku;
float ADC_A_shift;
float ADC_B_Ku;
float ADC_B_shift;
float ADC_C_Ku;
float ADC_C_shift;
float ADC_Udc_Ku;
float ADC_Udc_shift;
}IO_settings_st;
typedef struct
{
IO_settings_st *IO_coefs;
int16_t ADC_A;
float current_A;
int16_t ADC_B;
float current_B;
int16_t ADC_C;
float current_C;
uint16_t ENC_CNT_AB;
uint16_t ENC_CNT_SPI;
uint16_t ENC_CNT_PWM;
uint16_t ADC_Udc;
float voltage_DC;
uint16_t ADC_Tpower;
float temparature_power;
uint16_t ADC_VrefInt;
float voltage_VrefInt;
}IO_st;
extern IO_st IO;
extern IO_settings_st IO_settings_ext;
/*!
* @
*/
void update_IO_ADC(IO_st* io,uint16_t *ADC_data,const uint16_t mode);
void update_IO_Sensor(IO_st* io,uint16_t pos_cnt);
void set_IOSettings(IO_st *io,IO_settings_st* settings);
#endif /* APPLICATION_USER_IODATA_H_ */

118
App/FOC/InitDrive.c Normal file
View file

@ -0,0 +1,118 @@
/*
* InitDrive.c
*
* Created on: 9 июн. 2021 г.
* Author: a.ermakov
*/
#include "InitDrive.h"
/*Указание сопротивления мотора*/
#define RS 7.2f
/*Указание индуктивности мотора*/
#define LS 3.1e-3f
/*Указание полосы для регулятора в rad/sec*/
#define BAND 3250
SYSTEMobj_st SYSTEMobj_1={.DRIVE=&DRIVE,.IO=&IO,.OBSERVER_El=&Observer,.OBSERVER_M=&ObserverM,.PWM=&MY_PWM,.CurrentRegBand = BAND};
SYSTEMobj_st SYSTEMobj_2={.DRIVE=&DRIVE,.IO=&IO,.OBSERVER_El=&Observer,.OBSERVER_M=&ObserverM,.PWM=&MY_PWM};
void DriveInit(float Tsample_fast,float Tsample_slow)
{
//y = 1,1287E-03x - 2,1845E+00
IO_settings_ext.ADC_A_Ku = 1.1287E-03f;
IO_settings_ext.ADC_A_shift = -2.1845E+00;
IO_settings_ext.ADC_B_Ku = 1.1287E-03f;
IO_settings_ext.ADC_B_shift = -2.1845E+00;
IO_settings_ext.ADC_C_Ku = 1.1287E-03f;
IO_settings_ext.ADC_C_shift = -2.1845E+00;
IO_settings_ext.ADC_Udc_Ku = 0.009242578f;
IO_settings_ext.ADC_Udc_shift = 0.0f;
set_IOSettings(&IO,&IO_settings_ext);
DRIVE.Tsample_fast = Tsample_fast;
DRIVE.Tsample_slow = Tsample_slow;
//DRIVE.Tsample_slow = Tsample_slow;
DRIVE.motor_data.Rs = RS;
DRIVE.motor_data.Lq = LS;
DRIVE.motor_data.Ld = LS;
DRIVE.motor_data.Emf = 9.f/330;
set_PI_coefs(DRIVE.ppid_Id, DRIVE.motor_data.Ld*SYSTEMobj_1.CurrentRegBand, DRIVE.motor_data.Rs*SYSTEMobj_1.CurrentRegBand*DRIVE.Tsample_fast, 0.9f);
set_PI_coefs(DRIVE.ppid_Iq, DRIVE.motor_data.Lq*SYSTEMobj_1.CurrentRegBand, DRIVE.motor_data.Rs*SYSTEMobj_1.CurrentRegBand*DRIVE.Tsample_fast, 0.9f);
set_PI_coefs(DRIVE.ppid_Idc,0.5f, 0.0195f, 0.9f);
set_PI_coefs(DRIVE.ppid_FW, 0, 0.016f, 0.9f);
/*Настройка регулятора скорости*/
set_PI_coefs(DRIVE.ppid_Speed, 0.06f, 0.0005f, 0.9f);
/*установка параметров*/
PARAMETERS.Torque_current_max = 0.75f;
PARAMETERS.Torque_ramp_Up = 40000;
PARAMETERS.Torque_ramp_Down = 40000;
PARAMETERS.Brake_current_max = 0.75f;
PARAMETERS.Brake_ramp_Up = 40000;
PARAMETERS.Brake_ramp_Down = 40000;
PARAMETERS.Ibattery_chargeMax = 1.0f;
PARAMETERS.Ibattery_dischargeMax = 1.0f;
PARAMETERS.Idz_max = 0;
PARAMETERS.Idz_min = -0;
PARAMETERS.Max_speed =300;
PARAMETERS.Min_speed =-300;
PARAMETERS.Speed_ramp_Down = 200000;
PARAMETERS.Speed_ramp_Up = 200000;
/*Установка опций*/
actual_options.options.bit_FluxWeak1 = 1;
actual_options.options.bit_FluxWeak2 = 0;
actual_options.options.bit_Decouple = 0;
DRIVE.options.options_word = actual_options.options_word;
/*Filter vars init*/
filters_init(1/25.f,1/5.f);
initFilter(&GyroLowPass,DRIVE.Tsample_slow,1/100.f);
/*Инициалиазция ограничений*/
///Инициализация абсолютных значений
RESTRICTS.AbsoluteLimits[LowVolt_constraint].ValueMaximum = 100;
RESTRICTS.AbsoluteLimits[LowVolt_constraint].ValueMinimum = 7;
RESTRICTS.AbsoluteLimits[LowVolt_constraint].SpanMaximum = 6;
RESTRICTS.AbsoluteLimits[LowVolt_constraint].SpanMinumum = 6;
RESTRICTS.AbsoluteLimits[HighVolt_constraint].ValueMaximum = 100;
RESTRICTS.AbsoluteLimits[HighVolt_constraint].ValueMinimum = 2;
RESTRICTS.AbsoluteLimits[HighVolt_constraint].SpanMaximum = 3;
RESTRICTS.AbsoluteLimits[HighVolt_constraint].SpanMinumum = 0;
RESTRICTS.AbsoluteLimits[PositiveSpeed_constraint].ValueMaximum = 9000;
RESTRICTS.AbsoluteLimits[PositiveSpeed_constraint].ValueMinimum = 0;
RESTRICTS.AbsoluteLimits[PositiveSpeed_constraint].SpanMaximum = 1000;
RESTRICTS.AbsoluteLimits[PositiveSpeed_constraint].SpanMinumum = 0;
RESTRICTS.AbsoluteLimits[NegativeSpeed_constraint].ValueMaximum = 0;
RESTRICTS.AbsoluteLimits[NegativeSpeed_constraint].ValueMinimum = -9000;
RESTRICTS.AbsoluteLimits[NegativeSpeed_constraint].SpanMaximum = 1000;
RESTRICTS.AbsoluteLimits[NegativeSpeed_constraint].SpanMinumum = 0;
RESTRICTS.AbsoluteLimits[HighTemperature1_constraint].ValueMaximum = 80;
RESTRICTS.AbsoluteLimits[HighTemperature1_constraint].ValueMinimum = 0;
RESTRICTS.AbsoluteLimits[HighTemperature1_constraint].SpanMaximum = 5;
RESTRICTS.AbsoluteLimits[HighTemperature1_constraint].SpanMinumum = 0;
RESTRICTS.AbsoluteLimits[LowTemparature1_constraint].ValueMaximum = 0;
RESTRICTS.AbsoluteLimits[LowTemparature1_constraint].ValueMinimum = -20;
RESTRICTS.AbsoluteLimits[LowTemparature1_constraint].SpanMaximum = 5;
RESTRICTS.AbsoluteLimits[LowTemparature1_constraint].SpanMinumum =0;
///инициализация настраиваемых значений
RESTRICTS.ActualLimits[LowVolt_constraint].Span = 3;
RESTRICTS.ActualLimits[LowVolt_constraint].Value = 9;
RESTRICTS.ActualLimits[HighVolt_constraint].Span = 1;
RESTRICTS.ActualLimits[HighVolt_constraint].Value = 65;
RESTRICTS.ActualLimits[NegativeSpeed_constraint].Span = 50;
RESTRICTS.ActualLimits[NegativeSpeed_constraint].Value = -200 + RESTRICTS.ActualLimits[NegativeSpeed_constraint].Span;
RESTRICTS.ActualLimits[PositiveSpeed_constraint].Span = 50;
RESTRICTS.ActualLimits[PositiveSpeed_constraint].Value = 200 - RESTRICTS.ActualLimits[PositiveSpeed_constraint].Span;
RESTRICTS.ActualLimits[HighTemperature1_constraint].Value = 75;
RESTRICTS.ActualLimits[HighTemperature1_constraint].Span = 5;
RESTRICTS.ActualLimits[LowTemparature1_constraint].Value = -20;
RESTRICTS.ActualLimits[LowTemparature1_constraint].Span = 5;
InitRestricts(&RESTRICTS);
/*Для работы по механике */
ObserverM.Tsample = Tsample_fast;
ObserverM.Teta_diskr = toPI/4000;
ObserverM.L0 = 0.96f;
init_observer(&ObserverM);
/*Для работы по электрике*/
Observer.Tsample = Tsample_fast;
Observer.Teta_diskr = toPI/571.f;
Observer.L0 = 0.986f;
init_observer(&Observer);
/*Инициализация регулятора положения*/
Position_reg_init();
}

34
App/FOC/InitDrive.h Normal file
View file

@ -0,0 +1,34 @@
/*
* InitDrive.h
*
* Created on: 9 июн. 2021 г.
* Author: a.ermakov
*/
#ifndef APPLICATION_USER_INITDRIVE_H_
#define APPLICATION_USER_INITDRIVE_H_
#include "PlatformMath.h"
#include "IOdata.h"
#include "Observer.h"
#include "FOC_Control.h"
#include "pid_reg.h"
#include "FuncRestrict.h"
#include "PWM_algorithms.h"
#include "Filters.h"
#include "PositionReg.h"
/*SYStem object defination for Drive*/
typedef struct
{
IO_st *IO;
DRIVE_st *DRIVE;
Observer_struct *OBSERVER_El;
Observer_struct *OBSERVER_M;
PWM_struct *PWM;
uint16_t CurrentRegBand;
}SYSTEMobj_st;
/*Make System Objects external*/
extern SYSTEMobj_st SYSTEMobj_1;
extern uint32_t CPU_ID;
void DriveInit(const float, const float);
#endif /* APPLICATION_USER_INITDRIVE_H_ */

111
App/FOC/Observer.c Normal file
View file

@ -0,0 +1,111 @@
//Observer
#include "PlatformMath.h"
#include "observer.h"
Observer_struct Observer = {0};
Observer_struct ObserverM = {0};
float hall_corr[6]={1.2285765,1.82447,2.7726,3.80408,4.99,5.93};
void init_observer(Observer_struct *Observer)
{
//Observer->Teta_diskr=1.047;
Observer->POS_rad=0;
Observer->last_CNT=0;
Observer->POS_dobavka_sum=0;
Observer->fullMode_ena=0;
Observer->teta_correction=0;
Observer->refresh_ena=1;
refresh_koef(Observer);
Observer->CNT=0;
Observer->Time_CNT=0;
Observer->W = 0;
Observer->Teta_rad = 0;
Observer->J = 0.00046;
Observer->Ml = 0;
Observer->Ml_izm=0;
}
FAST_RAM void speed_observer(Observer_struct *Observer);
void speed_observer(Observer_struct *Observer)
{
static float teta_dif=0;
static float imp_time=0;
static float w_last = 0;
Observer->POS_rad=/*Observer->direction*/Observer->CNT *Observer->Teta_diskr;
if(Observer->CNT!=Observer->last_CNT) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
Observer->POS_dobavka_sum = 0;//* 0.0f *sign(Observer->W);
}
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>*/
Observer->POS_rad+=Observer->teta_correction;
if(Observer->POS_rad >=toPI) Observer->POS_rad-=toPI;
if(Observer->POS_rad < 0) Observer->POS_rad+=toPI;
Observer->last_CNT=Observer->CNT;
//--------------------------------------------------------------------------
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 2 <20><>
teta_dif=(Observer->Teta_rad - Observer->POS_rad);
if(teta_dif>PI)
{
teta_dif=teta_dif-toPI;
}
if(teta_dif<-PI)
{
teta_dif=teta_dif+toPI;
}
if(teta_dif>PI)
{
teta_dif=teta_dif-toPI;
}
if(teta_dif<-PI)
{
teta_dif=teta_dif+toPI;
}
//-----------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//------------------------------------------------------------
if(!Observer->fullMode_ena){
Observer->Teta_rad=Observer->Teta_rad+Observer->W*Observer->Tsample+Observer->L1_w*(teta_dif);//<2F> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Observer->Teta_deg=Observer->Teta_rad*rad2Deg;
Observer->W=Observer->W+Observer->L2_w*(teta_dif);//<2F> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}else{
Observer->Teta_rad=Observer->Teta_rad + Observer->W*Observer->Tsample + Observer->K_teta0*(Observer->Ml_izm-Observer->Ml) + Observer->L1_w*(teta_dif);//<2F> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Observer->W=Observer->W + Observer->K_teta1*(Observer->Ml_izm-Observer->Ml)+Observer->L2_w*(teta_dif);//<2F> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Observer->Ml=Observer->Ml+Observer->L3_w*teta_dif;
}
if(Observer->Teta_rad>toPI) Observer->Teta_rad-=toPI;
if(Observer->Teta_rad<0.f) Observer->Teta_rad+=toPI;
if(Observer->refresh_ena==1)
{
refresh_koef(Observer);
Observer->refresh_ena=0;
}
}
//#pragma CODE_SECTION(refresh_koef,"ramfuncs");
inline void refresh_koef(Observer_struct *Observer)
{
if(!Observer->fullMode_ena){
Observer->L1_w=2.f*Observer->L0-2.f;
Observer->L2_w=(-1.f+2.f*Observer->L0-Observer->L0*Observer->L0)/(Observer->Tsample);
}else{
Observer->L1_w=3*(Observer->L0-1);
Observer->L2_w=(9*Observer->L0 - 3*Observer->L0*Observer->L0 - Observer->L0*Observer->L0*Observer->L0 - 5)/(2*Observer->Tsample);
Observer->L3_w=Observer->J*(-3*Observer->L0 + 3*Observer->L0*Observer->L0 - Observer->L0*Observer->L0*Observer->L0 + 1)/(Observer->Tsample*Observer->Tsample );
Observer->K_teta0 = Observer->Tsample * Observer->Tsample/(2*Observer->J);
Observer->K_teta1 = Observer->Tsample / Observer->J;
}
}
///------------------------------------------------------------------------

37
App/FOC/Observer.h Normal file
View file

@ -0,0 +1,37 @@
#ifndef INCLUDE_OBSERVER_H_
#define INCLUDE_OBSERVER_H_
#include "math.h"
typedef struct
{
float Teta_diskr;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float POS_rad;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>
float last_CNT;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float POS_dobavka_sum;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> 1 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
unsigned int fullMode_ena;// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
unsigned int refresh_ena;//<2F><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
float teta_correction;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Teta_rad;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Teta_deg;// <20><><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float W; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Interpolate_W_Start;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD> <20><> teta+dt*w
float K_teta0; //<2F><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float K_teta1; // <20><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float CNT;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Ml_izm;
float Ml;
unsigned int Time_CNT;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float J;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float L0;//<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Tsample;//<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float L1_w;// <20><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float L2_w;
float L3_w;
} Observer_struct;
extern Observer_struct Observer;
extern Observer_struct ObserverM;
void refresh_koef(Observer_struct *Observer);
void init_observer(Observer_struct *Observer);
void speed_observer(Observer_struct *Observer);
#endif /*INCLUDE_OBSERVER_H_*/

221
App/FOC/PWM_algorithms.c Normal file
View file

@ -0,0 +1,221 @@
/*
* PWM_algorithms.c
*
* Created on: 07 <EFBFBD><EFBFBD><EFBFBD>. 2017 <EFBFBD>.
* Author: Andrey
*/
#include "PlatformMath.h"
#include "PWM_algorithms.h"
#include "math.h"
#define GENERIC_MAX(x,y) ((x)>(y) ?(x) : (y))
#define GENERIC_MIN(x,y) ((x)<(y) ?(x) : (y))
typedef enum
{
SV_PWM = 0x1,
DPWMMAX,
DPWMMIN,
DPWM30,
DPWM60
}PWM_MODE;
PWM_struct MY_PWM = {0};
FAST_RAM unsigned int PWM_proc(PWM_struct * myPWM);
unsigned int PWM_proc(PWM_struct * myPWM)
{
float VrefA=0,VrefB=0,VrefC=0;
//double Va=0,Vb=0,Vc=0;
float X=0,Y=0,Z=0;
unsigned int A=0,B=0,C=0;
float t1=0,t2=0;
float Ta=0,Tb=0,Tc=0;
float Ualfa_norm=0,Ubeta_norm=0;//modulation index
//Vdc_pu=(sqrt(myPWM->Ualfa*myPWM->Ualfa+my_PWM.Ubeta*my_PWM.Ubeta))*sqrt(3.)/myPWM->Vm_max;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//Vdc_T=myPWM->Trpd_pwm;///myPWM->Vdc;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> Udc
Ualfa_norm=myPWM->Ualfa;//*1.73205/myPWM->Vdc;
if(Ualfa_norm>1) Ualfa_norm=1.f;
if(Ualfa_norm<-1) Ualfa_norm=-1.f;
Ubeta_norm=myPWM->Ubeta;//*1.73205/myPWM->Vdc;
if(Ubeta_norm>1) Ubeta_norm=1.f;
if(Ubeta_norm<-1) Ubeta_norm=-1.f;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
VrefA=Ubeta_norm;
VrefB=-0.5f*Ubeta_norm+0.8665025f*Ualfa_norm;
VrefC=-0.5f*Ubeta_norm-0.8665025f*Ualfa_norm;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
if(VrefA>0) A=1;
else A=0;
if(VrefB>0) B=1;
else B=0;
if(VrefC>0) C=1;
else C=0;
myPWM->sector=A+2*B+4*C;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// X=Ubeta_norm;
// Y=0.5f*(1.732f*Ualfa_norm+Ubeta_norm);
// Z=0.5f*(-1.732f*Ualfa_norm+Ubeta_norm);
X=VrefA;
Y=-VrefC;
Z=-VrefB;
//
//X=2*0.57735*Ubeta_norm;
//Y=(0.57735*Ualfa_norm+Ubeta_norm);
//Z=(-0.57735*Ualfa_norm+Ubeta_norm);
// <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
switch(myPWM->sector)
{
case 1:
{
t1=Z;
t2=Y;
Tb =(1.f-t1-t2)*0.5f; // tbon = (1-t1-t2)/2
Ta = Tb+t1; // taon = tbon+t1
Tc = Ta+t2; // tcon = taon+t2
break;
}
case 2:
{
t1=Y;
t2=-X;
Ta =0.5f*(1.f-t1-t2); // taon = (1-t1-t2)/2
Tc = Ta+t1; // tcon = taon+t1
Tb = Tc+t2; // tbon = tcon+t2
break;
}
case 3:
{
t1=-Z;
t2=X;
Ta = 0.5f*(1.f-t1-t2); // taon = (1-t1-t2)/2
Tb = Ta+t1; // tbon = taon+t1
Tc = Tb+t2; // tcon = tbon+t2
break;
}
case 4:
{
t1=-X;
t2=Z;
Tc = 0.5f*(1.f-t1-t2); // tcon = (1-t1-t2)/2
Tb = Tc+t1; // tbon = tcon+t1
Ta = Tb+t2; // taon = tbon+t2
break;
}
case 5:
{
t1=X;
t2=-Y;
Tb = 0.5f*(1.f-t1-t2); // tbon = (1-t1-t2)/2
Tc = Tb+t1; // tcon = tbon+t1
Ta = Tc+t2; // taon = tcon+t2
break;
}
case 6:
{
t1=-Y;
t2=-Z;
Tc = 0.5*(1.f-t1-t2); // tcon = (1-t1-t2)/2
Ta = Tc+t1; // taon = tcon+t1
Tb = Ta+t2; // tbon = taon+t2
break;
}
case 0:
{
t1=0.5f;
t2=0.5f;
Ta=0.5f;
Tb=0.5f;
Tc=0.5f;
break;
}
default:
{
break;
}
}//end of switch
static float Tas = 0,Tbs=0,Tcs=0;
static float Toffset = 0;
static float Tmin = 0, Tmax = 0;
Tmax = GENERIC_MAX(Ta, Tb);
Tmax = GENERIC_MAX(Tmax, Tc);
Tmin = GENERIC_MIN(Ta, Tb);
Tmin = GENERIC_MIN(Tmin, Tc);
PWM_MODE pwm_mod = SV_PWM;
switch (pwm_mod)
{
case SV_PWM:
{
Toffset = (1.f-(Tmax-Tmin)) / 2.f - Tmin;
break;
}
case DPWMMAX:
{
Toffset = 1.f - Tmax;
break;
}
case DPWMMIN:
{
Toffset = -Tmin;
break;
}
case DPWM30:
{
if ((2.f*(Tmin + Tmax)-2.f) >= 0)
Toffset = -(Tmin*2.f-1.f);
else
{
Toffset = 1.0f - (Tmax*2.f-1.f);
}
break;
}
case DPWM60:
{
break;
}
default:
break;
}
////D<>+
//Toffset = 1 - Tmax;
////DC-
//Toffset = -Tmin;
////Generic
//static float sigma = toPI/8.f;
//float mu = 1 - 0.5f * (1 + my_sign(cos(BLDC_drive.Teta_rot_el+sigma),100000));
//mu = 0.5f;
//Toffset = (1 - mu) + (mu - 1) * Tmax - mu * Tmin;
Tas = Ta + Toffset;
Tbs = Tb + Toffset;
Tcs = Tc + Toffset;
//
if (Tas > 1.0f) Tas = 1.0f;
if (Tbs > 1.0f) Tbs = 1.0f;
if (Tcs > 1.0f) Tcs = 1.0f;
if (Tas < 0.0f) Tas = 0.0f;
if (Tbs < 0.0f) Tbs = 0.0f;
if (Tcs < 0.0f) Tcs = 0.0f;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>
myPWM->Ta=Tas*myPWM->Trpd_pwm;
myPWM->Tb=Tbs*myPWM->Trpd_pwm;
myPWM->Tc=Tcs*myPWM->Trpd_pwm;
return 1;
}

33
App/FOC/PWM_algorithms.h Normal file
View file

@ -0,0 +1,33 @@
/*
* PWM_algorithms.h
*
* Created on: 07 <EFBFBD><EFBFBD><EFBFBD>. 2017 <EFBFBD>.
* Author: Andrey
*/
#ifndef FOC_CODE_PWM_ALGORITHMS_H_
#define FOC_CODE_PWM_ALGORITHMS_H_
/*!
* \brief Struct describe pwm module and used in all PWM algorithms
*/
typedef struct
{
float Trpd_pwm; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> Tpwm=1/Fpc*2
float Ualfa; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
float Ubeta; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
float Uo; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
float Ta; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20>
float Tb; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20>
float Tc; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20>
unsigned int sector; ///< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
} PWM_struct;
#define PWM_DEFAULTS {1,0,0,0,0.5,0.5,0.5,0}
extern PWM_struct MY_PWM;
unsigned int PWM_proc(PWM_struct * myPWM);
unsigned int Sine_PWM_proc(PWM_struct * myPWM);
unsigned int PWM_SIX_step(PWM_struct * myPWM);
void PWM_deadTime(float *Currents,float Kdt,PWM_struct * myPWM);
#endif /* FOC_CODE_PWM_ALGORITHMS_H_ */

112
App/FOC/PlatformMath.h Normal file
View file

@ -0,0 +1,112 @@
/*
* PlatformMath.h
*
* Created on: 7 июн. 2021 г.
* Author: a.ermakov
*/
#ifndef APPLICATION_USER_PLATFORMMATH_H_
#define APPLICATION_USER_PLATFORMMATH_H_
#include "arm_math.h"
#include "arm_common_tables.h"
#define _inline static inline
#define PLATFORM_STM32 0
#define toPI 6.28318530f
#ifndef PI
#define PI 3.141592f
#endif
#define rad2Deg 57.295779513f
#define mod(x) ( ((x)>0)? (x): (-(x)) )
#define sign(x) ( ((x)>=0)? (1): (-(1)) )
#define min(x,y) ( ((x)>(y))? (y): (x) )
#define invSQRT_3 0.5773502691896f
#define twoDIVthree 2.f/3.f
#define rad2Deg 57.295779513f
/*Using FAST RAM*/
#define FAST_RAM __attribute__((section(".ccmram")))
//#define FAST_RAM
/**
* Fast atan2
*
* See http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
*
* @param y
* y
*
* @param x
* x
*
* @return
* The angle in radians
*/
#define PI_DIV_4 toPI/8.f
/*
static float utils_fast_atan2(float y, float x) {
float abs_y = fabsf(y) + 1e-20f; // kludge to prevent 0/0 condition
float angle;
if (x >= 0.f) {
float r = (x - abs_y) / (x + abs_y);
float rsq = r * r;
angle = ((0.1963f * rsq) - 0.9817f) * r + (PI_DIV_4);
} else {
float r = (x + abs_y) / (abs_y - x);
float rsq = r * r;
angle = ((0.1963f * rsq) - 0.9817f) * r + (PI_DIV_4);
}
if (y < 0.f) {
return(-angle);
} else {
return(angle);
}
}
*/
_inline float platform_sqrt(float ref)
{
float out=0.f;
if(ref > 0) out = sqrtf(ref);
return (out);
}
_inline float platform_isqrt(float val)
{
if(val>0) return (1/platform_sqrt(val));
return 0;
}
FAST_RAM void arm_sin_cos_f32(
float32_t theta,
float32_t * pSinVal,
float32_t * pCosVal);
_inline float platform_sin(float theta)
{
return(arm_sin_f32(theta));
}
_inline void platform_sincos(float theta,float *pSinVal,float *pCosVal)
{
arm_sin_cos_f32((theta*rad2Deg), pSinVal, pCosVal);
}
_inline float platform_atan2(float X,float Y )
{
return atan2f(X,Y);
//return utils_fast_atan2(X,Y);
}
_inline float platform_exp(float x)
{
return expf(x);
}
/*!
* \brief Функция Знака в виде усилитель с насыщением.
* @param I Текущее значение
* @param k Коэффициент нечувствительности
* @return
*/
_inline float sat_sign(float I, float k)
{
float out;
out = k * I;
if (out >= 1) return (1.);
else if (out <= -1) return (-1.);
return (0.f);
}
#endif /* APPLICATION_USER_PLATFORMMATH_H_ */

138
App/FOC/PositionReg.c Normal file
View file

@ -0,0 +1,138 @@
/*
* PositionReg.c
*
* Created on: 9 июн. 2021 г.
* Author: a.ermakov
*/
#include "PositionReg.h"
#include "SystemAPI.h"
enum REG_status Reg_state=DISABLED;
PIDREG3 Pid_Position = PIDREG3_DEFAULTS;
PIDREG3 Pid_Speed = PIDREG3_DEFAULTS;
float current_reference = 0;
#define DIRECT_POSITION 1
#define GRAVITY_COMPENSTATION 0
/**
* .brief Zero cancellation function
* \details Use IIR presentation of zero cancel block with equation y(k)=Ki*Ts/(Kp+Ki*Ts)*x(k)+kp/(kp+ki*Ts)*y(k-1)
* \param pid PIDREG3_handle reference
* \param ref Ref value to regulator
* \return filtered value
*/
static float ZeroCancel(PIDREG3_handle pid, float ref)
{
float out_val = 0;
if((pid->Ki>0) || (pid->Kp > 0))
out_val = pid->Ki / (pid->Kp + pid->Ki) * ref + pid->Kp / (pid->Kp + pid->Ki) * pid->Ref;
return(out_val);
}
void Position_reg_init(void)
{
Pid_Speed.calc = &pid_reg3_calc;
Pid_Speed.Kp = 0.61;
Pid_Speed.Ki = 0.0003233;
Pid_Speed.Tfilt_d = 0.1f;
Pid_Speed.Kd = 0;
Pid_Speed.OutMax = 1;
Pid_Speed.OutMin = -1;
Pid_Position.calc = &pid_reg3_calc_position;
Pid_Position.Kp = 2.56;
Pid_Position.Ki = 0.009233;
Pid_Position.Tfilt_d = 0.01f;
Pid_Position.Kd = 540;
Pid_Position.OutMax = DEF_MAX_CURRENT;
Pid_Position.OutMin = -DEF_MAX_CURRENT;
}
//#define radToImp 4000/toPI
#define radToImp toPI/16383
extern int16_t raw_angular_rate_X;
extern int16_t raw_angular_rate_Y;
extern int16_t raw_angular_rate_Z;
float angular_speed_coef = 0;
int16_t *p_angular_speed[2]={&raw_angular_rate_X,&raw_angular_rate_Y};
enum {
X=0,
Y
}axie ={X};
FAST_RAM void Position_reg_process(float ref,float fdb) ;
void Position_reg_process(float ref,float fdb)
{
Pid_Position.Ref = ZeroCancel(&Pid_Position,ref);//ref;
Pid_Position.Fdb = fdb;
Pid_Position.Err = Pid_Position.Ref - Pid_Position.Fdb;
/*исключение сброса на 2 пи*/
Pid_Position.Err = rollPI(Pid_Position.Err);
Pid_Position.Err = rollPI(Pid_Position.Err);
/*Расчет пи регулятора*/
if(Reg_state != ENABLED) Pid_Position.dft_st_U = 1u;
if(mod(Pid_Position.Err) < (radToImp*2.f) ) Pid_Position.Err = 0;
/*Gain control */
Pid_Position.calc(&Pid_Position);
#if GRAVITY_COMPENSTATION
current_reference = Pid_Position.Out + platform_sin(fdb)*0.345f+*p_angular_speed[axie]*angular_speed_coef;
#else
current_reference = Pid_Position.Out;// + platform_sin(fdb)*0.345f+*p_angular_speed[axie]*angular_speed_coef;
#endif
}
void SetSpeedReg_IC(float ui)
{
Pid_Speed.Ui = ui;
}
void Speed_reg_process(float ref , float fdb)
{
Pid_Speed.Ref =ref;
Pid_Speed.Fdb =fdb;
Pid_Speed.calc(&Pid_Speed);
current_reference = Pid_Speed.Out;
}
void Position_reg_processV2(float ref,float fdb,float speed_fdb)
{
static float position_error;
position_error = ref - fdb;
/*исключение сброса на 2 пи*/
position_error = rollPI(position_error);
position_error = rollPI(position_error);
if(mod(position_error) < radToImp ) position_error = 0;
Pid_Position.Ui+=Pid_Position.Ki*position_error;
/*Расчет пи регулятора*/
if(Reg_state != ENABLED) Pid_Position.Ui = 0;
Pid_Position.Up = Pid_Position.Kp*fdb;
Pid_Position.Ud = -Pid_Position.Kd*speed_fdb;
#if GRAVITY_COMPENSTATION
current_reference = Pid_Position.Out + platform_sin(fdb)*0.345f+*p_angular_speed[axie]*angular_speed_coef;
#else
current_reference = Pid_Position.Ui-Pid_Position.Kp*fdb-Pid_Position.Kd*speed_fdb;// + platform_sin(fdb)*0.345f+*p_angular_speed[axie]*angular_speed_coef;
if(current_reference > Pid_Position.OutMax)
{
current_reference = Pid_Position.OutMax;
//Pid_Position.Ui = current_reference ;
}
if(current_reference < Pid_Position.OutMin){
current_reference = Pid_Position.OutMin;
//Pid_Position.Ui = current_reference ;
}
#endif
}
float get_reference(void)
{
return current_reference;
}
enum REG_status get_pos_reg_state(void)
{
return Reg_state;
}
void set_pos_reg_state(enum REG_status state)
{
Reg_state = state;
}

55
App/FOC/PositionReg.h Normal file
View file

@ -0,0 +1,55 @@
/*
* PositionReg.h
*
* Created on: 9 июн. 2021 г.
* Author: a.ermakov
*/
#ifndef APPLICATION_USER_POSITIONREG_H_
#define APPLICATION_USER_POSITIONREG_H_
#include "PlatformMath.h"
#include "pid_reg.h"
enum REG_status{
DISABLED = 0,
ENABLED,
WORK,
};
extern PIDREG3 Pid_Position;
/*Rall halper function*/
static inline float rollPI(const float position_error){
if(position_error>=PI)
{
return (position_error-toPI);
}
if(position_error<=-PI)
{
return (position_error+toPI);
}
return position_error;
}
static inline float roll2PI(const float position_error){
if(position_error>=toPI)
{
return (position_error-toPI);
}
if(position_error<0)
{
return (position_error+toPI);
}
return position_error;
}
static float limitVal(float const ref,float max,float min)
{
if(ref > max) return max;
if(ref < min) return min;
return ref;
}
void Position_reg_init(void);
float get_reference(void);
enum REG_status get_pos_reg_state(void);
void set_pos_reg_state(enum REG_status state);
void Position_reg_process(float ref,float fdb);
void Position_reg_processV2(float ref,float fdb,float speed_fdb);
void Speed_reg_process(float ref , float fdb);
void SetSpeedReg_IC(float Ui);
#endif /* APPLICATION_USER_POSITIONREG_H_ */

165
App/FOC/pid_reg.c Normal file
View file

@ -0,0 +1,165 @@
/*
* pid_reg.c
*
* Created on: 4 июн. 2021 г.
* Author: a.ermakov
*/
#include "pid_reg.h"
void pid_reg3_calc(PIDREG3 *v)
{
// Compute the error
v->Err = v->Ref - v->Fdb;
// Compute the proportional output
v->Up = v->Kp*v->Err;
// Compute the integral output
//v->Ui = v->Ui + v->Ki*v->Up + v->Kc*v->SatErr;
v->Ui = v->Ui + v->Ki*v->Err + v->Kc*v->SatErr;
if(v->Ui > v->OutMax ) v->Ui = v->OutMax;
if(v->Ui < v->OutMin ) v->Ui = v->OutMin;
// Compute the derivative output
//v->Ud = v->Kd*(v->Up - v->Up1);
float Ud_tmp = v->Kd*(v->Up - v->Up1);
// Filter for Ud part
v->Ud = v->Ud+v->Tfilt_d*(Ud_tmp-v->Ud);
// Compute the pre-saturated output
v->OutPreSat = v->Up + v->Ui + v->Ud;
// Saturate the output
if (v->OutPreSat > v->OutMax)
v->Out = v->OutMax;
else if (v->OutPreSat < v->OutMin)
v->Out = v->OutMin;
else
v->Out = v->OutPreSat;
if(v->dft_st_U==1)
{
//v->Out=0;
v->Ui=0;
}
v->dft_st_U = 0;
// Compute the saturate difference
v->SatErr = v->Out - v->OutPreSat;
// Update the previous proportional output
v->Up1 = v->Up;
//v->Up1 = v->Err;
}
void pid_reg3_calc_position(PIDREG3 *v)
{
// Compute the proportional output
v->Up = v->Kp*v->Err;
// Compute the integral output
//v->Ui = v->Ui + v->Ki*v->Up + v->Kc*v->SatErr;
v->Ui = v->Ui + v->Ki*v->Err + v->Kc*v->SatErr;
if(v->Ui > v->OutMax ) v->Ui = v->OutMax;
if(v->Ui < v->OutMin ) v->Ui = v->OutMin;
// Compute the derivative output
//v->Ud = v->Kd*(v->Up - v->Up1);
float Ud_tmp = v->Kd*(v->Up - v->Up1);
// Filter for Ud part
v->Ud = v->Ud+v->Tfilt_d*(Ud_tmp-v->Ud);
// Compute the pre-saturated output
v->OutPreSat = v->Up + v->Ui + v->Ud;
// Saturate the output
if (v->OutPreSat > v->OutMax)
v->Out = v->OutMax;
else if (v->OutPreSat < v->OutMin)
v->Out = v->OutMin;
else
v->Out = v->OutPreSat;
if(v->dft_st_U==1)
{
//v->Out=0;
v->Ui=0;
}
v->dft_st_U = 0;
// Compute the saturate difference
v->SatErr = v->Out - v->OutPreSat;
// Update the previous proportional output
v->Up1 = v->Up;
//v->Up1 = v->Err;
}
void pid_reg3_calc_clamp(PIDREG3 *v)
{
// Compute the error
v->Err = v->Ref - v->Fdb;
// Compute the proportional output
v->Up = v->Kp*v->Err;
// Compute the integral output
//v->Ui = v->Ui + v->Ki*v->Up + v->Kc*v->SatErr;
v->Ui = v->Ui + v->Ki*v->Err *v->Up1;
// Compute the pre-saturated output
v->OutPreSat = v->Up + v->Ui;
v->Up1 = 1.f;//No saturation
// Saturate the output
if (v->OutPreSat > v->OutMax)
{
v->Out = v->OutMax;
v->Up1 = 0;
}
else if (v->OutPreSat < v->OutMin)
{
v->Out = v->OutMin;
v->Up1 = 0;
}
else
v->Out = v->OutPreSat;
if(v->dft_st_U==1)
{
//v->Out=0;
v->Ui=0;
}
v->dft_st_U = 0;
// Compute the saturate difference
v->SatErr = v->Out - v->OutPreSat;
}
void pid_reg3_calc_ErrLim(PIDREG3 *v)
{
// Compute the error
v->Err = v->Ref - v->Fdb;
// Compute the proportional output
v->Up = v->Kp*v->Err;
// Compute the pre-saturated output
v->OutPreSat = v->Up + v->Ui;
// Saturate the output
v->Up1 = 0;
if (v->OutPreSat > v->OutMax)
{
v->Out = v->OutMax;
v->Up1 = v->OutMax - v->OutPreSat ;
}
else if (v->OutPreSat < v->OutMin)
{
v->Out = v->OutMin;
v->Up1 = v->OutMin - v->OutPreSat ;
}else v->Out = v->OutPreSat;
//Intergral err limit
v->Ui=v->Ui+v->Ki*(v->Err + v->Up1/v->Kp);
if(v->dft_st_U==1)
{
//v->Out=0;
v->Ui=0;
}
v->dft_st_U = 0;
}
void set_PI_coefs(PIDREG3 *v,float Kp,float Ki,float Kc)
{
v->Kp = Kp;
v->Ki = Ki;
v->Kc = Kc;
}

91
App/FOC/pid_reg.h Normal file
View file

@ -0,0 +1,91 @@
/*
* pid_reg.h
*
* Created on: 4 июн. 2021 г.
* Author: a.ermakov
*/
#ifndef APPLICATION_USER_PID_REG_H_
#define APPLICATION_USER_PID_REG_H_
typedef struct { float Ref; // Input: Reference input
float Fdb; // Input: Feedback input
float Err; // Variable: Error
float Kp; // Parameter: Proportional gain
float Up; // Variable: Proportional output
float Ui; // Variable: Integral output
float Ud; // Variable: Derivative output
float Tfilt_d; // Filter time constant for D in 1\Tf
float Ui_max; //Max integral out
float Ui_min; //Min int out
float OutPreSat; // Variable: Pre-saturated output
float OutMax; // Parameter: Maximum output
float OutMin; // Parameter: Minimum output
float Out; // Output: PID output
float SatErr; // Variable: Saturated difference
float Ki; // Parameter: Integral gain
float Kc; // Parameter: Integral correction gain
float Kd; // Parameter: Derivative gain
float Up1; // History: Previous proportional output
unsigned int dft_st_U;// default bit for pid out
void (*calc)(); // Pointer to calculation function
} PIDREG3;
typedef PIDREG3 *PIDREG3_handle;
/*-----------------------------------------------------------------------------
Default initalizer for the PIDREG3 object.
-----------------------------------------------------------------------------*/
#define PIDREG3_DEFAULTS { 0, \
0, \
0, \
0.05, \
0, \
0,\
0, \
0,\
0,\
0, \
0, \
0.83, \
0, \
0, \
0, \
0.1, \
0.7, \
0, \
0, \
0,\
(void (*)(unsigned int))pid_reg3_calc }
#define PIDREG3_DEFAULTS_CLAMP { 0, \
0, \
0, \
0.05, \
0, \
0,\
0, \
0,\
0,\
0, \
0, \
0.83, \
0, \
0, \
0, \
0.1, \
0.9, \
0, \
0, \
0,\
(void (*)(unsigned int))pid_reg3_calc_clamp }
/*------------------------------------------------------------------------------
Prototypes for the functions in PIDREG3.C
------------------------------------------------------------------------------*/
void pid_reg3_calc(PIDREG3_handle);
void pid_reg3_calc_position(PIDREG3_handle);
void pid_reg3_calc_clamp(PIDREG3_handle);
void pid_reg3_calc_ErrLim(PIDREG3_handle);
void set_PI_coefs(PIDREG3_handle ,float Kp,float ,float );
#endif /* APPLICATION_USER_PID_REG_H_ */

147
App/SystemAPI.h Normal file
View file

@ -0,0 +1,147 @@
#include "main.h"
#pragma once
#define DEF_MAX_CURRENT (1.00f)
typedef enum {
MOTOR_STATE_IDLE = 0,
MOTOR_STATE_GOTO_ZERO = 1,
MOTOR_STATE_GOTO_ZERO_FINISH = 2,
MOTOR_STATE_STABILIZATION = 3,
MOTOR_STATE_ERROR = 4,
MOTOR_STATE_TURN_OFF = 5,
MOTOR_STATE_TURN_OFF_FINISH = 6,
MOTOR_STATE_EMERGENCY_BRAKE = 7,
MOTOR_STATE_FLUX_ALIGN =10,
MOTOR_STATE_TEST_PID =20,
MOTOR_STATE_TEST_MOTION =30,
} ENUM_MOTOR_STATES;
#pragma pack(push,1)
typedef struct {
uint32_t u32CpuID;
float f32TetaCorrection;
float f32MinAngle_deg;
float f32MaxAngle_deg;
float f32IcogCompCoef;
float f32IaccelCompFactor;
float f32PositionZero_deg;
float f32PidPositionKd;
float f32PidPositionKi;
float f32PidPositionKp;
float f32PidPositionTfilt_d;
float f32UpRateZero;
float f32DownRateZero;
float f32UpRateStab;
float f32DownRateStab;
} TPARAMS_USED_BY_SERVO;
#pragma pack(pop)
extern TPARAMS_USED_BY_SERVO stUsedParameters;
extern uint32_t CPU_ID;
/*TEST pins define section*/
#define TESTPIN_ENABLE 0
#define TESTPIN_UART_ISR_ENABLE (1)
//#define TEST_UART 0
#define FSAMPLE_FAST (10000)
#define TSAMPLE_FAST (1.0f/FSAMPLE_FAST)
#define FSAMPLE_SLOW (1000)
#define TSAMPLE_SLOW (1.0f/FSAMPLE_SLOW)
static inline float ISR_TIME_CALC(float start,float stop)
{
static float systemTimeStep = -1.f;
if(systemTimeStep <0.f){
systemTimeStep=1.f/SystemCoreClock;
return 0;
}
if (start > stop)
{
return ((start- stop)*(systemTimeStep));
}
else
{
uint32_t counter = ((0xFFFFFF - stop) + start) + 1;
return ( (counter)*(systemTimeStep));
}
}
static inline uint16_t GET_PWM_Period(uint16_t ID)
{
return((uint16_t)LL_TIM_GetAutoReload(TIM1));
}
static inline void PWM_OUT_ENABLE(uint16_t ID)
{
LL_GPIO_SetOutputPin(EN_U_GPIO_Port,EN_U_Pin);
LL_GPIO_SetOutputPin(EN_W_GPIO_Port,EN_W_Pin);
LL_GPIO_SetOutputPin(EN_V_GPIO_Port,EN_V_Pin);
LL_TIM_EnableAllOutputs(TIM1);
}
static inline void PWM_OUT_DISABLE(uint16_t ID)
{
LL_GPIO_ResetOutputPin(EN_U_GPIO_Port,EN_U_Pin);
LL_GPIO_ResetOutputPin(EN_W_GPIO_Port,EN_W_Pin);
LL_GPIO_ResetOutputPin(EN_V_GPIO_Port,EN_V_Pin);
LL_TIM_DisableAllOutputs(TIM1);
}
static inline void PWM_COMPARE_SET(uint16_t ID,uint16_t A,uint16_t B,uint16_t C)
{
LL_TIM_OC_SetCompareCH1(TIM1, A);
LL_TIM_OC_SetCompareCH2(TIM1, B);
LL_TIM_OC_SetCompareCH3(TIM1, C);
}
static uint16_t hw_err_state = 0;
static inline uint16_t GET_HWfault_state(uint16_t ID)
{
if(hw_err_state) LL_TIM_DisableIT_BRK(TIM1);
return hw_err_state;
}
static inline void CLEAR_HWfault(uint16_t ID)
{
hw_err_state = 0;
LL_TIM_EnableIT_BRK(TIM1);
}
static inline uint32_t GET_ADC_data(uint16_t ID,uint16_t *ADC_array,uint16_t *error_code)
{
*error_code = 0;
uint32_t adc_wait1 = 0;
while((!LL_ADC_IsActiveFlag_JEOS(ADC1))){
if(adc_wait1++ > 1000) return (*error_code = 0x1);
};
/*Channel 1 ADC 1- Current A*/
ADC_array[0] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_1);
/*Channel 2 ADC 1- Current B*/
ADC_array[2] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_2);
/*Channel 4 ADC 1 - Vdc bs*/
ADC_array[5] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_4);
/*Clear flags*/
LL_ADC_ClearFlag_JEOS(ADC1);
return (0x0);
}
static inline uint16_t GET_ENCODER_counter(uint16_t ID)
{
return ( LL_TIM_GetCounter(TIM3));
}
static inline void SET_ENCODER_counter(uint16_t ID,uint16_t value)
{
LL_TIM_SetCounter(TIM3, value);
}

52
App/appIsrCallBacks.c
View file

@ -5,28 +5,50 @@
* Author: on4ip
*/
#include "main.h"
uint32_t adcTick=0;
uint16_t adc1Data=0;
uint16_t adc2Data=0;
void adcIsrCallBack(void){
adcTick++;
adc1Data = LL_ADC_INJ_ReadConversionData12(ADC1, LL_ADC_INJ_RANK_1);
adc2Data = LL_ADC_INJ_ReadConversionData12(ADC1, LL_ADC_INJ_RANK_2);
LL_ADC_ClearFlag_JEOS(ADC1);
#include "ControlFuncs.h"
uint32_t adcTick = 0;
uint16_t adc1Data = 0;
uint16_t adc2Data = 0;
float ADC_ISR_time = 0; /*ISR load time*/
static uint16_t ADC_array[16];
void adcIsrCallBack(void) {
static uint32_t start_ADC_isr = 0;
static uint32_t stop_ADC_isr = 0;
LL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
/*Time test of ADC isr*/
start_ADC_isr = SysTick->VAL;
adcTick++;
/*Reading and processing ADC data*/
uint16_t error_code = 0;
if(GET_ADC_data(SYSTEMobj_1.DRIVE->drive_ID,ADC_array,&error_code))
{
SYSTEMobj_1.DRIVE->ERRORS.errors.bit_FREE1 = 1;
}
update_IO_ADC(SYSTEMobj_1.IO, ADC_array,SYSTEMobj_1.DRIVE->STAT.stat.bit_ENABLED);
/*Calling of Application control function with Fast update rate*/
FAST_loop(&SYSTEMobj_1);
stop_ADC_isr = SysTick->VAL;
ADC_ISR_time = ISR_TIME_CALC(start_ADC_isr, stop_ADC_isr);
LL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
LL_ADC_ClearFlag_JEOS(ADC1);
}
uint32_t tim1msTick=0;
void tim1msCallBack(void){
uint32_t tim1msTick = 0;
float TIM1ms_ISR_time = 0; /*ISR load time*/
void tim1msCallBack(void) {
tim1msTick++;
LL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
/*Colling of Application control function with Slow update rate*/
SLOW_loop(&SYSTEMobj_1);
//LL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
LL_TIM_ClearFlag_UPDATE(TIM2);
}
uint32_t timPwmTick =0;
void timPwmCallBack(void){
uint32_t timPwmTick = 0;
void timPwmCallBack(void) {
timPwmTick++;
LL_GPIO_TogglePin(LED2_GPIO_Port, LED2_Pin);
//LL_GPIO_TogglePin(LED2_GPIO_Port, LED2_Pin);
LL_TIM_ClearFlag_CC4(TIM1);
}

BIN
Drivers/DSP/Lib/arm_cortexM4lf_math.lib Normal file
View file

Binary file not shown.

BIN
Drivers/DSP/Lib/libarm_cortexM4lf_math.a Normal file
View file

Binary file not shown.

379
Drivers/DSP/inc/arm_common_tables.h Normal file
View file

@ -0,0 +1,379 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_common_tables.h
* Description: Extern declaration for common tables
*
* $Date: 27. January 2017
* $Revision: V.1.5.1
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef _ARM_COMMON_TABLES_H
#define _ARM_COMMON_TABLES_H
#include "arm_math.h"
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FFT_ALLOW_TABLES)
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREV_1024)
extern const uint16_t armBitRevTable[1024];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_16)
extern const float32_t twiddleCoef_16[32];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_32)
extern const float32_t twiddleCoef_32[64];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_64)
extern const float32_t twiddleCoef_64[128];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_128)
extern const float32_t twiddleCoef_128[256];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_256)
extern const float32_t twiddleCoef_256[512];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_512)
extern const float32_t twiddleCoef_512[1024];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_1024)
extern const float32_t twiddleCoef_1024[2048];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_2048)
extern const float32_t twiddleCoef_2048[4096];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_F32_4096)
extern const float32_t twiddleCoef_4096[8192];
#define twiddleCoef twiddleCoef_4096
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_16)
extern const q31_t twiddleCoef_16_q31[24];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_32)
extern const q31_t twiddleCoef_32_q31[48];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_64)
extern const q31_t twiddleCoef_64_q31[96];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_128)
extern const q31_t twiddleCoef_128_q31[192];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_256)
extern const q31_t twiddleCoef_256_q31[384];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_512)
extern const q31_t twiddleCoef_512_q31[768];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_1024)
extern const q31_t twiddleCoef_1024_q31[1536];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_2048)
extern const q31_t twiddleCoef_2048_q31[3072];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q31_4096)
extern const q31_t twiddleCoef_4096_q31[6144];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_16)
extern const q15_t twiddleCoef_16_q15[24];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_32)
extern const q15_t twiddleCoef_32_q15[48];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_64)
extern const q15_t twiddleCoef_64_q15[96];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_128)
extern const q15_t twiddleCoef_128_q15[192];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_256)
extern const q15_t twiddleCoef_256_q15[384];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_512)
extern const q15_t twiddleCoef_512_q15[768];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_1024)
extern const q15_t twiddleCoef_1024_q15[1536];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_2048)
extern const q15_t twiddleCoef_2048_q15[3072];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_Q15_4096)
extern const q15_t twiddleCoef_4096_q15[6144];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_32)
extern const float32_t twiddleCoef_rfft_32[32];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_64)
extern const float32_t twiddleCoef_rfft_64[64];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_128)
extern const float32_t twiddleCoef_rfft_128[128];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_256)
extern const float32_t twiddleCoef_rfft_256[256];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_512)
extern const float32_t twiddleCoef_rfft_512[512];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_1024)
extern const float32_t twiddleCoef_rfft_1024[1024];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_2048)
extern const float32_t twiddleCoef_rfft_2048[2048];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_TWIDDLECOEF_RFFT_F32_4096)
extern const float32_t twiddleCoef_rfft_4096[4096];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
/* floating-point bit reversal tables */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_16)
#define ARMBITREVINDEXTABLE_16_TABLE_LENGTH ((uint16_t)20)
extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE_16_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_32)
#define ARMBITREVINDEXTABLE_32_TABLE_LENGTH ((uint16_t)48)
extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE_32_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_64)
#define ARMBITREVINDEXTABLE_64_TABLE_LENGTH ((uint16_t)56)
extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE_64_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_128)
#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208)
extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_256)
#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440)
extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_512)
#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448)
extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_1024)
#define ARMBITREVINDEXTABLE_1024_TABLE_LENGTH ((uint16_t)1800)
extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE_1024_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_2048)
#define ARMBITREVINDEXTABLE_2048_TABLE_LENGTH ((uint16_t)3808)
extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE_2048_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FLT_4096)
#define ARMBITREVINDEXTABLE_4096_TABLE_LENGTH ((uint16_t)4032)
extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE_4096_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
/* fixed-point bit reversal tables */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_16)
#define ARMBITREVINDEXTABLE_FIXED_16_TABLE_LENGTH ((uint16_t)12)
extern const uint16_t armBitRevIndexTable_fixed_16[ARMBITREVINDEXTABLE_FIXED_16_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_32)
#define ARMBITREVINDEXTABLE_FIXED_32_TABLE_LENGTH ((uint16_t)24)
extern const uint16_t armBitRevIndexTable_fixed_32[ARMBITREVINDEXTABLE_FIXED_32_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_64)
#define ARMBITREVINDEXTABLE_FIXED_64_TABLE_LENGTH ((uint16_t)56)
extern const uint16_t armBitRevIndexTable_fixed_64[ARMBITREVINDEXTABLE_FIXED_64_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_128)
#define ARMBITREVINDEXTABLE_FIXED_128_TABLE_LENGTH ((uint16_t)112)
extern const uint16_t armBitRevIndexTable_fixed_128[ARMBITREVINDEXTABLE_FIXED_128_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_256)
#define ARMBITREVINDEXTABLE_FIXED_256_TABLE_LENGTH ((uint16_t)240)
extern const uint16_t armBitRevIndexTable_fixed_256[ARMBITREVINDEXTABLE_FIXED_256_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_512)
#define ARMBITREVINDEXTABLE_FIXED_512_TABLE_LENGTH ((uint16_t)480)
extern const uint16_t armBitRevIndexTable_fixed_512[ARMBITREVINDEXTABLE_FIXED_512_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_1024)
#define ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH ((uint16_t)992)
extern const uint16_t armBitRevIndexTable_fixed_1024[ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_2048)
#define ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH ((uint16_t)1984)
extern const uint16_t armBitRevIndexTable_fixed_2048[ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREVIDX_FXT_4096)
#define ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH ((uint16_t)4032)
extern const uint16_t armBitRevIndexTable_fixed_4096[ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_REALCOEF_F32)
extern const float32_t realCoefA[8192];
extern const float32_t realCoefB[8192];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_REALCOEF_Q31)
extern const q31_t realCoefAQ31[8192];
extern const q31_t realCoefBQ31[8192];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_REALCOEF_Q15)
extern const q15_t realCoefAQ15[8192];
extern const q15_t realCoefBQ15[8192];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_F32_128)
extern const float32_t Weights_128[256];
extern const float32_t cos_factors_128[128];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_F32_512)
extern const float32_t Weights_512[1024];
extern const float32_t cos_factors_512[512];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_F32_2048)
extern const float32_t Weights_2048[4096];
extern const float32_t cos_factors_2048[2048];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_F32_8192)
extern const float32_t Weights_8192[16384];
extern const float32_t cos_factors_8192[8192];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q15_128)
extern const q15_t WeightsQ15_128[256];
extern const q15_t cos_factorsQ15_128[128];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q15_512)
extern const q15_t WeightsQ15_512[1024];
extern const q15_t cos_factorsQ15_512[512];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q15_2048)
extern const q15_t WeightsQ15_2048[4096];
extern const q15_t cos_factorsQ15_2048[2048];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q15_8192)
extern const q15_t WeightsQ15_8192[16384];
extern const q15_t cos_factorsQ15_8192[8192];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q31_128)
extern const q31_t WeightsQ31_128[256];
extern const q31_t cos_factorsQ31_128[128];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q31_512)
extern const q31_t WeightsQ31_512[1024];
extern const q31_t cos_factorsQ31_512[512];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q31_2048)
extern const q31_t WeightsQ31_2048[4096];
extern const q31_t cos_factorsQ31_2048[2048];
#endif
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_DCT4_Q31_8192)
extern const q31_t WeightsQ31_8192[16384];
extern const q31_t cos_factorsQ31_8192[8192];
#endif
#endif /* if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FFT_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FAST_ALLOW_TABLES)
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_RECIP_Q15)
extern const q15_t armRecipTableQ15[64];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_RECIP_Q31)
extern const q31_t armRecipTableQ31[64];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
/* Tables for Fast Math Sine and Cosine */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SIN_F32)
extern const float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SIN_Q31)
extern const q31_t sinTable_q31[FAST_MATH_TABLE_SIZE + 1];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SIN_Q15)
extern const q15_t sinTable_q15[FAST_MATH_TABLE_SIZE + 1];
#endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
#endif /* if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FAST_TABLES) */
#endif /* ARM_COMMON_TABLES_H */

66
Drivers/DSP/inc/arm_const_structs.h Normal file
View file

@ -0,0 +1,66 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_const_structs.h
* Description: Constant structs that are initialized for user convenience.
* For example, some can be given as arguments to the arm_cfft_f32() function.
*
* $Date: 27. January 2017
* $Revision: V.1.5.1
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef _ARM_CONST_STRUCTS_H
#define _ARM_CONST_STRUCTS_H
#include "arm_math.h"
#include "arm_common_tables.h"
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len16;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len32;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len64;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len128;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len256;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len512;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048;
extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len16;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len32;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len64;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len128;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len256;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len512;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len1024;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len2048;
extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len4096;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len16;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len32;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len64;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len128;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len256;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len512;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len1024;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len2048;
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len4096;
#endif

7361
Drivers/DSP/inc/arm_math.h Normal file
View file

File diff suppressed because it is too large Load diff

122
Drivers/DSP/src/arm_cos_f32.c Normal file
View file

@ -0,0 +1,122 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_cos_f32.c
* Description: Fast cosine calculation for floating-point values
*
* $Date: 18. March 2019
* $Revision: V1.6.0
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "arm_math.h"
#include "arm_common_tables.h"
/**
@ingroup groupFastMath
*/
/**
@defgroup cos Cosine
Computes the trigonometric cosine function using a combination of table lookup
and linear interpolation. There are separate functions for
Q15, Q31, and floating-point data types.
The input to the floating-point version is in radians while the
fixed-point Q15 and Q31 have a scaled input with the range
[0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a
value of 2*pi wraps around to 0.
The implementation is based on table lookup using 256 values together with linear interpolation.
The steps used are:
-# Calculation of the nearest integer table index
-# Compute the fractional portion (fract) of the table index.
-# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
where
<pre>
b = Table[index];
c = Table[index+1];
</pre>
*/
/**
@addtogroup cos
@{
*/
/**
@brief Fast approximation to the trigonometric cosine function for floating-point data.
@param[in] x input value in radians
@return cos(x)
*/
float32_t arm_cos_f32(
float32_t x)
{
float32_t cosVal, fract, in; /* Temporary input, output variables */
uint16_t index; /* Index variable */
float32_t a, b; /* Two nearest output values */
int32_t n;
float32_t findex;
/* input x is in radians */
/* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */
in = x * 0.159154943092f + 0.25f;
/* Calculation of floor value of input */
n = (int32_t) in;
/* Make negative values towards -infinity */
if (in < 0.0f)
{
n--;
}
/* Map input value to [0 1] */
in = in - (float32_t) n;
/* Calculation of index of the table */
findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
index = (uint16_t)findex;
/* when "in" is exactly 1, we need to rotate the index down to 0 */
if (index >= FAST_MATH_TABLE_SIZE) {
index = 0;
findex -= (float32_t)FAST_MATH_TABLE_SIZE;
}
/* fractional value calculation */
fract = findex - (float32_t) index;
/* Read two nearest values of input value from the cos table */
a = sinTable_f32[index];
b = sinTable_f32[index+1];
/* Linear interpolation process */
cosVal = (1.0f - fract) * a + fract * b;
/* Return output value */
return (cosVal);
}
/**
@} end of cos group
*/

149
Drivers/DSP/src/arm_sin_cos_f32.c Normal file
View file

@ -0,0 +1,149 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_sin_cos_f32.c
* Description: Sine and Cosine calculation for floating-point values
*
* $Date: 18. March 2019
* $Revision: V1.6.0
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "arm_math.h"
#include "arm_common_tables.h"
const float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1] __attribute__((section(".RamFunc")));
/**
@ingroup groupController
*/
/**
@defgroup SinCos Sine Cosine
Computes the trigonometric sine and cosine values using a combination of table lookup
and linear interpolation.
There are separate functions for Q31 and floating-point data types.
The input to the floating-point version is in degrees while the
fixed-point Q31 have a scaled input with the range
[-1 0.9999] mapping to [-180 +180] degrees.
The floating point function also allows values that are out of the usual range. When this happens, the function will
take extra time to adjust the input value to the range of [-180 180].
The result is accurate to 5 digits after the decimal point.
The implementation is based on table lookup using 360 values together with linear interpolation.
The steps used are:
-# Calculation of the nearest integer table index.
-# Compute the fractional portion (fract) of the input.
-# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1.
-# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>.
-# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1.
-# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>.
*/
/**
@addtogroup SinCos
@{
*/
/**
@brief Floating-point sin_cos function.
@param[in] theta input value in degrees
@param[out] pSinVal points to processed sine output
@param[out] pCosVal points to processed cosine output
@return none
*/
/*__attribute__((section(".ccmram"))) */void arm_sin_cos_f32(
float32_t theta,
float32_t * pSinVal,
float32_t * pCosVal) ;
void arm_sin_cos_f32(
float32_t theta,
float32_t * pSinVal,
float32_t * pCosVal)
{
float32_t fract, in; /* Temporary input, output variables */
uint16_t indexS, indexC; /* Index variable */
float32_t f1, f2, d1, d2; /* Two nearest output values */
float32_t Dn, Df;
float32_t temp, findex;
/* input x is in degrees */
/* Scale input, divide input by 360, for cosine add 0.25 (pi/2) to read sine table */
in = theta * 0.00277777777778f;
if (in < 0.0f)
{
in = -in;
}
in = in - (int32_t)in;
/* Calculate the nearest index */
findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
indexS = ((uint16_t)findex) & 0x1ff;
indexC = (indexS + (FAST_MATH_TABLE_SIZE / 4)) & 0x1ff;
/* Calculation of fractional value */
fract = findex - (float32_t) indexS;
/* Read two nearest values of input value from the cos & sin tables */
f1 = sinTable_f32[indexC ];
f2 = sinTable_f32[indexC+1];
d1 = -sinTable_f32[indexS ];
d2 = -sinTable_f32[indexS+1];
temp = (1.0f - fract) * f1 + fract * f2;
Dn = 0.0122718463030f; /* delta between the two points (fixed), in this case 2*pi/FAST_MATH_TABLE_SIZE */
Df = f2 - f1; /* delta between the values of the functions */
temp = Dn * (d1 + d2) - 2 * Df;
temp = fract * temp + (3 * Df - (d2 + 2 * d1) * Dn);
temp = fract * temp + d1 * Dn;
/* Calculation of cosine value */
*pCosVal = fract * temp + f1;
/* Read two nearest values of input value from the cos & sin tables */
f1 = sinTable_f32[indexS ];
f2 = sinTable_f32[indexS+1];
d1 = sinTable_f32[indexC ];
d2 = sinTable_f32[indexC+1];
temp = (1.0f - fract) * f1 + fract * f2;
Df = f2 - f1; // delta between the values of the functions
temp = Dn * (d1 + d2) - 2 * Df;
temp = fract * temp + (3 * Df - (d2 + 2 * d1) * Dn);
temp = fract * temp + d1 * Dn;
/* Calculation of sine value */
*pSinVal = fract * temp + f1;
if (theta < 0.0f)
{
*pSinVal = -*pSinVal;
}
}
/**
@} end of SinCos group
*/

122
Drivers/DSP/src/arm_sin_f32.c Normal file
View file

@ -0,0 +1,122 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_sin_f32.c
* Description: Fast sine calculation for floating-point values
*
* $Date: 18. March 2019
* $Revision: V1.6.0
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "arm_math.h"
#include "arm_common_tables.h"
/**
@ingroup groupFastMath
*/
/**
@defgroup sin Sine
Computes the trigonometric sine function using a combination of table lookup
and linear interpolation. There are separate functions for
Q15, Q31, and floating-point data types.
The input to the floating-point version is in radians while the
fixed-point Q15 and Q31 have a scaled input with the range
[0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a
value of 2*pi wraps around to 0.
The implementation is based on table lookup using 256 values together with linear interpolation.
The steps used are:
-# Calculation of the nearest integer table index
-# Compute the fractional portion (fract) of the table index.
-# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
where
<pre>
b = Table[index];
c = Table[index+1];
</pre>
*/
/**
@addtogroup sin
@{
*/
/**
@brief Fast approximation to the trigonometric sine function for floating-point data.
@param[in] x input value in radians.
@return sin(x)
*/
float32_t arm_sin_f32(
float32_t x)
{
float32_t sinVal, fract, in; /* Temporary input, output variables */
uint16_t index; /* Index variable */
float32_t a, b; /* Two nearest output values */
int32_t n;
float32_t findex;
/* input x is in radians */
/* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi */
in = x * 0.159154943092f;
/* Calculation of floor value of input */
n = (int32_t) in;
/* Make negative values towards -infinity */
if (in < 0.0f)
{
n--;
}
/* Map input value to [0 1] */
in = in - (float32_t) n;
/* Calculation of index of the table */
findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
index = (uint16_t)findex;
/* when "in" is exactly 1, we need to rotate the index down to 0 */
if (index >= FAST_MATH_TABLE_SIZE) {
index = 0;
findex -= (float32_t)FAST_MATH_TABLE_SIZE;
}
/* fractional value calculation */
fract = findex - (float32_t) index;
/* Read two nearest values of input value from the sin table */
a = sinTable_f32[index];
b = sinTable_f32[index+1];
/* Linear interpolation process */
sinVal = (1.0f - fract) * a + fract * b;
/* Return output value */
return (sinVal);
}
/**
@} end of sin group
*/

2
Inc/adc.h
View file

@ -37,8 +37,6 @@ extern "C" {
/* USER CODE END Private defines */
void MX_ADC1_Init(void);
void MX_ADC2_Init(void);
void MX_ADC3_Init(void);
/* USER CODE BEGIN Prototypes */

10
Inc/main.h
View file

@ -71,8 +71,14 @@ void Error_Handler(void);
/* USER CODE END EFP */
/* Private defines -----------------------------------------------------------*/
#define SD1_Pin LL_GPIO_PIN_11
#define SD1_GPIO_Port GPIOA
#define AS5045_CS_Pin LL_GPIO_PIN_15
#define AS5045_CS_GPIO_Port GPIOB
#define EN_W_Pin LL_GPIO_PIN_6
#define EN_W_GPIO_Port GPIOC
#define EN_U_Pin LL_GPIO_PIN_11
#define EN_U_GPIO_Port GPIOA
#define EN_V_Pin LL_GPIO_PIN_12
#define EN_V_GPIO_Port GPIOA
#define LED1_Pin LL_GPIO_PIN_10
#define LED1_GPIO_Port GPIOC
#define LED2_Pin LL_GPIO_PIN_11

2
Inc/spi.h
View file

@ -36,7 +36,7 @@ extern "C" {
/* USER CODE END Private defines */
void MX_SPI1_Init(void);
void MX_SPI2_Init(void);
/* USER CODE BEGIN Prototypes */

107
Src/adc.c
View file

@ -110,113 +110,6 @@ void MX_ADC1_Init(void)
/* USER CODE END ADC1_Init 2 */
}
/* ADC2 init function */
void MX_ADC2_Init(void)
{
/* USER CODE BEGIN ADC2_Init 0 */
/* USER CODE END ADC2_Init 0 */
LL_ADC_InitTypeDef ADC_InitStruct = {0};
LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = {0};
LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
/* Peripheral clock enable */
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC2);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC);
/**ADC2 GPIO Configuration
PC3 ------> ADC2_IN13
PC4 ------> ADC2_IN14
PC5 ------> ADC2_IN15
*/
GPIO_InitStruct.Pin = LL_GPIO_PIN_3|LL_GPIO_PIN_4|LL_GPIO_PIN_5;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
LL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* USER CODE BEGIN ADC2_Init 1 */
/* USER CODE END ADC2_Init 1 */
/** Common config
*/
ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B;
ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT;
ADC_InitStruct.SequencersScanMode = LL_ADC_SEQ_SCAN_DISABLE;
LL_ADC_Init(ADC2, &ADC_InitStruct);
ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE;
ADC_REG_InitStruct.SequencerLength = LL_ADC_REG_SEQ_SCAN_DISABLE;
ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE;
ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE;
ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_NONE;
LL_ADC_REG_Init(ADC2, &ADC_REG_InitStruct);
LL_ADC_REG_SetFlagEndOfConversion(ADC2, LL_ADC_REG_FLAG_EOC_UNITARY_CONV);
/** Configure Regular Channel
*/
LL_ADC_REG_SetSequencerRanks(ADC2, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_15);
LL_ADC_SetChannelSamplingTime(ADC2, LL_ADC_CHANNEL_15, LL_ADC_SAMPLINGTIME_3CYCLES);
/* USER CODE BEGIN ADC2_Init 2 */
/* USER CODE END ADC2_Init 2 */
}
/* ADC3 init function */
void MX_ADC3_Init(void)
{
/* USER CODE BEGIN ADC3_Init 0 */
/* USER CODE END ADC3_Init 0 */
LL_ADC_InitTypeDef ADC_InitStruct = {0};
LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = {0};
LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
/* Peripheral clock enable */
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC3);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC);
/**ADC3 GPIO Configuration
PC0 ------> ADC3_IN10
PC1 ------> ADC3_IN11
*/
GPIO_InitStruct.Pin = LL_GPIO_PIN_0|LL_GPIO_PIN_1;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
LL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* USER CODE BEGIN ADC3_Init 1 */
/* USER CODE END ADC3_Init 1 */
/** Common config
*/
ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B;
ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT;
ADC_InitStruct.SequencersScanMode = LL_ADC_SEQ_SCAN_DISABLE;
LL_ADC_Init(ADC3, &ADC_InitStruct);
ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE;
ADC_REG_InitStruct.SequencerLength = LL_ADC_REG_SEQ_SCAN_DISABLE;
ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE;
ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE;
ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_NONE;
LL_ADC_REG_Init(ADC3, &ADC_REG_InitStruct);
LL_ADC_REG_SetFlagEndOfConversion(ADC3, LL_ADC_REG_FLAG_EOC_UNITARY_CONV);
/** Configure Regular Channel
*/
LL_ADC_REG_SetSequencerRanks(ADC3, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_10);
LL_ADC_SetChannelSamplingTime(ADC3, LL_ADC_CHANNEL_10, LL_ADC_SAMPLINGTIME_3CYCLES);
/* USER CODE BEGIN ADC3_Init 2 */
/* USER CODE END ADC3_Init 2 */
}
/* USER CODE BEGIN 1 */

35
Src/gpio.c
View file

@ -52,21 +52,40 @@ void MX_GPIO_Init(void)
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOD);
/**/
LL_GPIO_ResetOutputPin(SD1_GPIO_Port, SD1_Pin);
LL_GPIO_SetOutputPin(AS5045_CS_GPIO_Port, AS5045_CS_Pin);
/**/
LL_GPIO_ResetOutputPin(GPIOC, LED1_Pin|LED2_Pin|LED3_Pin);
LL_GPIO_ResetOutputPin(GPIOC, EN_W_Pin|LED1_Pin|LED2_Pin|LED3_Pin);
/**/
LL_GPIO_ResetOutputPin(GPIOA, EN_U_Pin|EN_V_Pin);
/**/
LL_GPIO_ResetOutputPin(spi1_cs_GPIO_Port, spi1_cs_Pin);
/**/
GPIO_InitStruct.Pin = SD1_Pin;
GPIO_InitStruct.Pin = AS5045_CS_Pin;
GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
LL_GPIO_Init(SD1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
LL_GPIO_Init(AS5045_CS_GPIO_Port, &GPIO_InitStruct);
/**/
GPIO_InitStruct.Pin = EN_W_Pin;
GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
LL_GPIO_Init(EN_W_GPIO_Port, &GPIO_InitStruct);
/**/
GPIO_InitStruct.Pin = EN_U_Pin|EN_V_Pin;
GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/**/
GPIO_InitStruct.Pin = LED1_Pin|LED2_Pin|LED3_Pin;
@ -79,9 +98,9 @@ void MX_GPIO_Init(void)
/**/
GPIO_InitStruct.Pin = spi1_cs_Pin;
GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
LL_GPIO_Init(spi1_cs_GPIO_Port, &GPIO_InitStruct);
}

308
Src/main.c
View file

@ -1,20 +1,20 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
@ -27,7 +27,7 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "InitDrive.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -47,7 +47,12 @@
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
volatile uint16_t asSpiWordLow = 0;
volatile uint16_t asSpiWordHigh = 0;
volatile uint32_t asResultWord = 0;
typedef struct AS5045_data {
};
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
@ -64,144 +69,169 @@ static void MX_NVIC_Init(void);
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
__disable_irq();
/* USER CODE END 1 */
* @brief The application entry point.
* @retval int
*/
int main(void) {
/* USER CODE BEGIN 1 */
__disable_irq();
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE BEGIN Init */
SysTick_Config(0xFFFFFF);
LL_DBGMCU_APB1_GRP1_FreezePeriph( LL_DBGMCU_APB1_GRP1_TIM3_STOP);
LL_DBGMCU_APB1_GRP1_FreezePeriph( LL_DBGMCU_APB1_GRP1_TIM2_STOP);
LL_DBGMCU_APB2_GRP1_FreezePeriph( LL_DBGMCU_APB2_GRP1_TIM1_STOP);
LL_DBGMCU_APB2_GRP1_FreezePeriph( LL_DBGMCU_APB2_GRP1_TIM8_STOP);
/* USER CODE END Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
SystemCoreClockUpdate();
/* USER CODE END SysInit */
/* USER CODE BEGIN SysInit */
SystemCoreClockUpdate();
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_ADC1_Init();
MX_TIM1_Init();
MX_USART1_UART_Init();
MX_USART2_UART_Init();
MX_CAN2_Init();
MX_TIM2_Init();
MX_SPI2_Init();
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_ADC1_Init();
MX_ADC2_Init();
MX_ADC3_Init();
MX_SPI1_Init();
MX_TIM1_Init();
MX_USART1_UART_Init();
MX_USART2_UART_Init();
MX_CAN2_Init();
MX_TIM2_Init();
/* Initialize interrupts */
MX_NVIC_Init();
/* USER CODE BEGIN 2 */
/* Initialize interrupts */
MX_NVIC_Init();
/* USER CODE BEGIN 2 */
LL_GPIO_ResetOutputPin(LED1_GPIO_Port, LED1_Pin);
LL_GPIO_ResetOutputPin(LED2_GPIO_Port, LED2_Pin);
LL_GPIO_ResetOutputPin(LED1_GPIO_Port, LED1_Pin);
LL_GPIO_ResetOutputPin(LED2_GPIO_Port, LED2_Pin);
LL_ADC_EnableIT_JEOS(ADC1);
LL_ADC_Enable(ADC1);
LL_ADC_EnableIT_JEOS(ADC1);
LL_ADC_Enable(ADC1);
LL_TIM_SetAutoReload(TIM1, SystemCoreClock/PWM_FRQ_HZ);
LL_TIM_OC_SetCompareCH4(TIM1, (SystemCoreClock/PWM_FRQ_HZ)-1);
LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH4);
/*Used center alogned mode so set ARR to Half of Calculated period*/
uint32_t TIM_ARR_value = (SystemCoreClock / PWM_FRQ_HZ) >> 1;
LL_TIM_SetAutoReload(TIM1, TIM_ARR_value - 1);
LL_TIM_OC_SetCompareCH4(TIM1, LL_TIM_GetAutoReload(TIM1) - 1);
LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH4);
LL_TIM_EnableIT_CC4(TIM1);
LL_TIM_EnableCounter(TIM1);
/*TIM2 on Half clock bus so divide SystemClock by 2 */
LL_TIM_SetAutoReload(TIM2, ((SystemCoreClock/ALG_TIMER_HZ)>>1)-1);
LL_TIM_EnableIT_UPDATE(TIM2);
LL_TIM_EnableCounter(TIM2);
__enable_irq();
/* USER CODE END 2 */
LL_TIM_EnableIT_CC4(TIM1);
LL_TIM_EnableCounter(TIM1);
/*TIM2 on Half clock bus so divide SystemClock by 2 */
LL_TIM_SetAutoReload(TIM2, ((SystemCoreClock / ALG_TIMER_HZ) >> 1) - 1);
LL_TIM_EnableIT_UPDATE(TIM2);
LL_TIM_EnableCounter(TIM2);
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
LL_GPIO_SetOutputPin(AS5045_CS_GPIO_Port, AS5045_CS_Pin);
LL_SPI_SetBaudRatePrescaler(SPI2,LL_SPI_BAUDRATEPRESCALER_DIV64 );
LL_SPI_Enable(SPI2);
/*Init Control structure*/
float TSAMPLE_FAST = 1.f / PWM_FRQ_HZ;
float TSAMPLE_SLOW = 1.f / ALG_TIMER_HZ;
DriveInit(TSAMPLE_FAST, TSAMPLE_SLOW);
__enable_irq();
/* USER CODE END 2 */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1) {
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
if (!LL_SPI_IsActiveFlag_OVR(SPI2)) {
LL_GPIO_ResetOutputPin(AS5045_CS_GPIO_Port, AS5045_CS_Pin);
while (!LL_SPI_IsActiveFlag_TXE(SPI2)){};
LL_SPI_TransmitData16(SPI2, 0xFFFF);
while (!LL_SPI_IsActiveFlag_RXNE(SPI2)) {
}
uint16_t buffer = LL_SPI_ReceiveData16(SPI2);
asSpiWordLow = (buffer >> 4) & 0xFFF;
LL_GPIO_SetOutputPin(AS5045_CS_GPIO_Port, AS5045_CS_Pin);
} else {
LL_SPI_ClearFlag_OVR(SPI2);
}
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
LL_FLASH_SetLatency(LL_FLASH_LATENCY_5);
while(LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_5)
{
}
LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1);
LL_PWR_EnableOverDriveMode();
LL_RCC_HSE_Enable();
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void) {
LL_FLASH_SetLatency(LL_FLASH_LATENCY_5);
while (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_5) {
}
LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1);
LL_PWR_EnableOverDriveMode();
LL_RCC_HSE_Enable();
/* Wait till HSE is ready */
while(LL_RCC_HSE_IsReady() != 1)
{
/* Wait till HSE is ready */
while (LL_RCC_HSE_IsReady() != 1) {
}
LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSE, LL_RCC_PLLM_DIV_4, 180, LL_RCC_PLLP_DIV_2);
LL_RCC_PLL_Enable();
}
LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSE, LL_RCC_PLLM_DIV_4, 180,
LL_RCC_PLLP_DIV_2);
LL_RCC_PLL_Enable();
/* Wait till PLL is ready */
while(LL_RCC_PLL_IsReady() != 1)
{
/* Wait till PLL is ready */
while (LL_RCC_PLL_IsReady() != 1) {
}
LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1);
LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_4);
LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_2);
LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);
}
LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1);
LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_4);
LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_2);
LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);
/* Wait till System clock is ready */
while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL)
{
/* Wait till System clock is ready */
while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) {
}
LL_SetSystemCoreClock(180000000);
}
LL_SetSystemCoreClock(180000000);
/* Update the time base */
if (HAL_InitTick (TICK_INT_PRIORITY) != HAL_OK)
{
Error_Handler();
}
LL_RCC_SetTIMPrescaler(LL_RCC_TIM_PRESCALER_TWICE);
/* Update the time base */
if (HAL_InitTick(TICK_INT_PRIORITY) != HAL_OK) {
Error_Handler();
}
LL_RCC_SetTIMPrescaler(LL_RCC_TIM_PRESCALER_TWICE);
}
/**
* @brief NVIC Configuration.
* @retval None
*/
static void MX_NVIC_Init(void)
{
/* TIM1_BRK_TIM9_IRQn interrupt configuration */
NVIC_SetPriority(TIM1_BRK_TIM9_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),1, 0));
NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
/* CAN2_RX0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
/* CAN2_RX1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
/* ADC_IRQn interrupt configuration */
NVIC_SetPriority(ADC_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
NVIC_EnableIRQ(ADC_IRQn);
/* TIM1_CC_IRQn interrupt configuration */
NVIC_SetPriority(TIM1_CC_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
NVIC_EnableIRQ(TIM1_CC_IRQn);
* @brief NVIC Configuration.
* @retval None
*/
static void MX_NVIC_Init(void) {
/* TIM1_BRK_TIM9_IRQn interrupt configuration */
NVIC_SetPriority(TIM1_BRK_TIM9_IRQn,
NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 1, 0));
NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
/* TIM1_CC_IRQn interrupt configuration */
NVIC_SetPriority(TIM1_CC_IRQn,
NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 3, 0));
NVIC_EnableIRQ(TIM1_CC_IRQn);
/* TIM2_IRQn interrupt configuration */
NVIC_SetPriority(TIM2_IRQn,
NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 4, 0));
NVIC_EnableIRQ(TIM2_IRQn);
/* ADC_IRQn interrupt configuration */
NVIC_SetPriority(ADC_IRQn,
NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 2, 0));
NVIC_EnableIRQ(ADC_IRQn);
/* CAN2_RX0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
/* CAN2_RX1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
}
/* USER CODE BEGIN 4 */
@ -209,18 +239,16 @@ static void MX_NVIC_Init(void)
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void) {
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1) {
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT

46
Src/spi.c
View file

@ -24,37 +24,37 @@
/* USER CODE END 0 */
/* SPI1 init function */
void MX_SPI1_Init(void)
/* SPI2 init function */
void MX_SPI2_Init(void)
{
/* USER CODE BEGIN SPI1_Init 0 */
/* USER CODE BEGIN SPI2_Init 0 */
/* USER CODE END SPI1_Init 0 */
/* USER CODE END SPI2_Init 0 */
LL_SPI_InitTypeDef SPI_InitStruct = {0};
LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
/* Peripheral clock enable */
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SPI1);
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_SPI2);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB);
/**SPI1 GPIO Configuration
PA7 ------> SPI1_MOSI
PB3 ------> SPI1_SCK
PB4 ------> SPI1_MISO
/**SPI2 GPIO Configuration
PC1 ------> SPI2_MOSI
PB10 ------> SPI2_SCK
PB14 ------> SPI2_MISO
*/
GPIO_InitStruct.Pin = LL_GPIO_PIN_7;
GPIO_InitStruct.Pin = LL_GPIO_PIN_1;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
GPIO_InitStruct.Alternate = LL_GPIO_AF_5;
LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Alternate = LL_GPIO_AF_7;
LL_GPIO_Init(GPIOC, &GPIO_InitStruct);
GPIO_InitStruct.Pin = LL_GPIO_PIN_3|LL_GPIO_PIN_4;
GPIO_InitStruct.Pin = LL_GPIO_PIN_10|LL_GPIO_PIN_14;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
@ -62,24 +62,24 @@ void MX_SPI1_Init(void)
GPIO_InitStruct.Alternate = LL_GPIO_AF_5;
LL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN SPI1_Init 1 */
/* USER CODE BEGIN SPI2_Init 1 */
/* USER CODE END SPI1_Init 1 */
/* USER CODE END SPI2_Init 1 */
SPI_InitStruct.TransferDirection = LL_SPI_FULL_DUPLEX;
SPI_InitStruct.Mode = LL_SPI_MODE_MASTER;
SPI_InitStruct.DataWidth = LL_SPI_DATAWIDTH_8BIT;
SPI_InitStruct.DataWidth = LL_SPI_DATAWIDTH_16BIT;
SPI_InitStruct.ClockPolarity = LL_SPI_POLARITY_LOW;
SPI_InitStruct.ClockPhase = LL_SPI_PHASE_1EDGE;
SPI_InitStruct.ClockPhase = LL_SPI_PHASE_2EDGE;
SPI_InitStruct.NSS = LL_SPI_NSS_SOFT;
SPI_InitStruct.BaudRate = LL_SPI_BAUDRATEPRESCALER_DIV2;
SPI_InitStruct.BaudRate = LL_SPI_BAUDRATEPRESCALER_DIV64;
SPI_InitStruct.BitOrder = LL_SPI_MSB_FIRST;
SPI_InitStruct.CRCCalculation = LL_SPI_CRCCALCULATION_DISABLE;
SPI_InitStruct.CRCPoly = 10;
LL_SPI_Init(SPI1, &SPI_InitStruct);
LL_SPI_SetStandard(SPI1, LL_SPI_PROTOCOL_MOTOROLA);
/* USER CODE BEGIN SPI1_Init 2 */
LL_SPI_Init(SPI2, &SPI_InitStruct);
LL_SPI_SetStandard(SPI2, LL_SPI_PROTOCOL_MOTOROLA);
/* USER CODE BEGIN SPI2_Init 2 */
/* USER CODE END SPI1_Init 2 */
/* USER CODE END SPI2_Init 2 */
}

37
Src/stm32f4xx_it.c
View file

@ -1,20 +1,20 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
******************************************************************************
* @file stm32f4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
@ -72,9 +72,8 @@ void NMI_Handler(void)
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
while (1)
{
}
while (1) {
}
/* USER CODE END NonMaskableInt_IRQn 1 */
}
@ -245,7 +244,7 @@ void TIM1_CC_IRQHandler(void)
void TIM2_IRQHandler(void)
{
/* USER CODE BEGIN TIM2_IRQn 0 */
tim1msCallBack();
/* USER CODE END TIM2_IRQn 0 */
/* USER CODE BEGIN TIM2_IRQn 1 */

4
Src/tim.c
View file

@ -112,10 +112,6 @@ void MX_TIM2_Init(void)
/* Peripheral clock enable */
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
/* TIM2 interrupt Init */
NVIC_SetPriority(TIM2_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
NVIC_EnableIRQ(TIM2_IRQn);
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */

174
processor32f446.ioc
View file

@ -21,18 +21,6 @@ ADC1.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
ADC1.SamplingTime-1\#ChannelInjectedConversion=ADC_SAMPLETIME_3CYCLES
ADC1.SamplingTime-2\#ChannelInjectedConversion=ADC_SAMPLETIME_3CYCLES
ADC1.master=1
ADC2.Channel-5\#ChannelRegularConversion=ADC_CHANNEL_15
ADC2.ClockPrescaler=ADC_CLOCK_SYNC_PCLK_DIV4
ADC2.IPParameters=Rank-5\#ChannelRegularConversion,Channel-5\#ChannelRegularConversion,SamplingTime-5\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler
ADC2.NbrOfConversionFlag=1
ADC2.Rank-5\#ChannelRegularConversion=1
ADC2.SamplingTime-5\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_10
ADC3.ClockPrescaler=ADC_CLOCK_SYNC_PCLK_DIV4
ADC3.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler
ADC3.NbrOfConversionFlag=1
ADC3.Rank-0\#ChannelRegularConversion=1
ADC3.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
CAD.formats=
CAD.pinconfig=
CAD.provider=
@ -41,70 +29,67 @@ CAN2.CalculateTimeBit=1066
CAN2.CalculateTimeQuantum=355.55555555555554
CAN2.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate
File.Version=6
GPIO.groupedBy=Group By Peripherals
KeepUserPlacement=false
Mcu.CPN=STM32F446RET6
Mcu.Family=STM32F4
Mcu.IP0=ADC1
Mcu.IP1=ADC2
Mcu.IP10=USART1
Mcu.IP11=USART2
Mcu.IP2=ADC3
Mcu.IP3=CAN2
Mcu.IP4=NVIC
Mcu.IP5=RCC
Mcu.IP6=SPI1
Mcu.IP7=SYS
Mcu.IP8=TIM1
Mcu.IP9=TIM2
Mcu.IPNb=12
Mcu.IP1=CAN2
Mcu.IP2=NVIC
Mcu.IP3=RCC
Mcu.IP4=SPI2
Mcu.IP5=SYS
Mcu.IP6=TIM1
Mcu.IP7=TIM2
Mcu.IP8=USART1
Mcu.IP9=USART2
Mcu.IPNb=10
Mcu.Name=STM32F446R(C-E)Tx
Mcu.Package=LQFP64
Mcu.Pin0=PH0-OSC_IN
Mcu.Pin1=PH1-OSC_OUT
Mcu.Pin10=PA5
Mcu.Pin11=PA6
Mcu.Pin12=PA7
Mcu.Pin13=PC4
Mcu.Pin14=PC5
Mcu.Pin15=PB0
Mcu.Pin16=PB1
Mcu.Pin17=PB12
Mcu.Pin18=PB13
Mcu.Pin19=PA8
Mcu.Pin2=PC0
Mcu.Pin20=PA9
Mcu.Pin21=PA10
Mcu.Pin22=PA11
Mcu.Pin10=PB0
Mcu.Pin11=PB1
Mcu.Pin12=PB10
Mcu.Pin13=PB12
Mcu.Pin14=PB13
Mcu.Pin15=PB14
Mcu.Pin16=PB15
Mcu.Pin17=PC6
Mcu.Pin18=PA8
Mcu.Pin19=PA9
Mcu.Pin2=PC1
Mcu.Pin20=PA10
Mcu.Pin21=PA11
Mcu.Pin22=PA12
Mcu.Pin23=PA13
Mcu.Pin24=PA14
Mcu.Pin25=PC10
Mcu.Pin26=PC11
Mcu.Pin27=PC12
Mcu.Pin28=PD2
Mcu.Pin29=PB3
Mcu.Pin3=PC1
Mcu.Pin30=PB4
Mcu.Pin31=PB6
Mcu.Pin32=PB7
Mcu.Pin33=VP_SYS_VS_Systick
Mcu.Pin34=VP_TIM1_VS_no_output4
Mcu.Pin35=VP_TIM2_VS_ClockSourceINT
Mcu.Pin4=PC3
Mcu.Pin5=PA0-WKUP
Mcu.Pin6=PA1
Mcu.Pin7=PA2
Mcu.Pin8=PA3
Mcu.Pin9=PA4
Mcu.PinsNb=36
Mcu.Pin29=PB6
Mcu.Pin3=PA0-WKUP
Mcu.Pin30=PB7
Mcu.Pin31=VP_SYS_VS_Systick
Mcu.Pin32=VP_TIM1_VS_no_output4
Mcu.Pin33=VP_TIM2_VS_ClockSourceINT
Mcu.Pin4=PA1
Mcu.Pin5=PA2
Mcu.Pin6=PA3
Mcu.Pin7=PA4
Mcu.Pin8=PA5
Mcu.Pin9=PA6
Mcu.PinsNb=34
Mcu.ThirdPartyNb=0
Mcu.UserConstants=
Mcu.UserName=STM32F446RETx
MxCube.Version=6.7.0
MxDb.Version=DB.6.0.70
NVIC.ADC_IRQn=true\:0\:0\:false\:true\:true\:4\:true\:true\:true
NVIC.ADC_IRQn=true\:2\:0\:true\:true\:true\:4\:true\:true\:true
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.CAN2_RX0_IRQn=true\:0\:0\:false\:true\:true\:2\:true\:true\:true
NVIC.CAN2_RX1_IRQn=true\:0\:0\:false\:true\:true\:3\:true\:true\:true
NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:true\:true\:5\:true\:true\:true
NVIC.CAN2_RX1_IRQn=true\:5\:0\:true\:true\:true\:6\:true\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.ForceEnableDMAVector=true
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
@ -115,18 +100,26 @@ NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
NVIC.TIM1_BRK_TIM9_IRQn=true\:1\:0\:true\:true\:true\:1\:true\:true\:true
NVIC.TIM1_CC_IRQn=true\:0\:0\:false\:true\:true\:6\:true\:true\:true
NVIC.TIM2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
NVIC.TIM1_CC_IRQn=true\:3\:0\:true\:true\:true\:2\:true\:true\:true
NVIC.TIM2_IRQn=true\:4\:0\:true\:true\:true\:3\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
PA0-WKUP.Mode=CTS_RTS
PA0-WKUP.Signal=USART2_CTS
PA1.Mode=CTS_RTS
PA1.Signal=USART2_RTS
PA10.Signal=S_TIM1_CH3
PA11.GPIOParameters=GPIO_Label
PA11.GPIO_Label=SD1
PA11.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PA11.GPIO_Label=EN_U
PA11.GPIO_PuPd=GPIO_PULLDOWN
PA11.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA11.Locked=true
PA11.Signal=GPIO_Output
PA12.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PA12.GPIO_Label=EN_V
PA12.GPIO_PuPd=GPIO_PULLDOWN
PA12.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA12.Locked=true
PA12.Signal=GPIO_Output
PA13.Mode=Serial_Wire
PA13.Signal=SYS_JTMS-SWDIO
PA14.Mode=Serial_Wire
@ -138,29 +131,35 @@ PA3.Signal=USART2_RX
PA4.Signal=ADCx_IN4
PA5.Signal=ADCx_IN5
PA6.Signal=ADCx_IN6
PA7.Mode=Full_Duplex_Master
PA7.Signal=SPI1_MOSI
PA8.Signal=S_TIM1_CH1
PA9.Signal=S_TIM1_CH2
PB0.Locked=true
PB0.Signal=ADCx_IN8
PB1.Locked=true
PB1.Signal=ADCx_IN9
PB10.Mode=Full_Duplex_Master
PB10.Signal=SPI2_SCK
PB12.Locked=true
PB12.Mode=CAN_Activate
PB12.Signal=CAN2_RX
PB13.Mode=CAN_Activate
PB13.Signal=CAN2_TX
PB3.Mode=Full_Duplex_Master
PB3.Signal=SPI1_SCK
PB4.Mode=Full_Duplex_Master
PB4.Signal=SPI1_MISO
PB14.Locked=true
PB14.Mode=Full_Duplex_Master
PB14.Signal=SPI2_MISO
PB15.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
PB15.GPIO_Label=AS5045_CS
PB15.GPIO_PuPd=GPIO_PULLUP
PB15.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PB15.Locked=true
PB15.PinState=GPIO_PIN_SET
PB15.Signal=GPIO_Output
PB6.Mode=Asynchronous
PB6.Signal=USART1_TX
PB7.Mode=Asynchronous
PB7.Signal=USART1_RX
PC0.Signal=ADCx_IN10
PC1.Signal=ADCx_IN11
PC1.Mode=Full_Duplex_Master
PC1.Signal=SPI2_MOSI
PC10.GPIOParameters=GPIO_Label
PC10.GPIO_Label=LED1
PC10.Locked=true
@ -173,11 +172,16 @@ PC12.GPIOParameters=GPIO_Label
PC12.GPIO_Label=LED3
PC12.Locked=true
PC12.Signal=GPIO_Output
PC3.Signal=ADCx_IN13
PC4.Signal=ADCx_IN14
PC5.Signal=ADCx_IN15
PD2.GPIOParameters=GPIO_Label
PC6.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PC6.GPIO_Label=EN_W
PC6.GPIO_PuPd=GPIO_PULLDOWN
PC6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PC6.Locked=true
PC6.Signal=GPIO_Output
PD2.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PD2.GPIO_Label=spi1_cs
PD2.GPIO_PuPd=GPIO_PULLDOWN
PD2.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PD2.Locked=true
PD2.Signal=GPIO_Output
PH0-OSC_IN.Mode=HSE-External-Oscillator
@ -212,7 +216,7 @@ ProjectManager.StackSize=0x400
ProjectManager.TargetToolchain=STM32CubeIDE
ProjectManager.ToolChainLocation=
ProjectManager.UnderRoot=true
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-LL-true,2-SystemClock_Config-RCC-false-LL-false,3-MX_ADC1_Init-ADC1-false-LL-true,4-MX_ADC2_Init-ADC2-false-LL-true,5-MX_ADC3_Init-ADC3-false-LL-true,6-MX_SPI1_Init-SPI1-false-LL-true,7-MX_TIM1_Init-TIM1-false-LL-true,8-MX_USART1_UART_Init-USART1-false-LL-true,9-MX_USART2_UART_Init-USART2-false-LL-true,10-MX_CAN2_Init-CAN2-false-HAL-true,11-MX_TIM2_Init-TIM2-false-LL-true
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-LL-true,2-SystemClock_Config-RCC-false-LL-false,3-MX_ADC1_Init-ADC1-false-LL-true,4-MX_ADC2_Init-ADC2-false-LL-true,5-MX_ADC3_Init-ADC3-false-LL-true,6-MX_TIM1_Init-TIM1-false-LL-true,7-MX_USART1_UART_Init-USART1-false-LL-true,8-MX_USART2_UART_Init-USART2-false-LL-true,9-MX_CAN2_Init-CAN2-false-HAL-true,10-MX_TIM2_Init-TIM2-false-LL-true,11-MX_SPI2_Init-SPI2-false-LL-true
RCC.AHBFreq_Value=180000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4
RCC.APB1Freq_Value=45000000
@ -258,16 +262,6 @@ RCC.VCOInputFreq_Value=2000000
RCC.VCOOutputFreq_Value=360000000
RCC.VCOSAIInputFreq_Value=500000
RCC.VCOSAIOutputFreq_Value=96000000
SH.ADCx_IN10.0=ADC3_IN10,IN10
SH.ADCx_IN10.ConfNb=1
SH.ADCx_IN11.0=ADC3_IN11,IN11
SH.ADCx_IN11.ConfNb=1
SH.ADCx_IN13.0=ADC2_IN13,IN13
SH.ADCx_IN13.ConfNb=1
SH.ADCx_IN14.0=ADC2_IN14,IN14
SH.ADCx_IN14.ConfNb=1
SH.ADCx_IN15.0=ADC2_IN15,IN15
SH.ADCx_IN15.ConfNb=1
SH.ADCx_IN4.0=ADC1_IN4,IN4
SH.ADCx_IN4.ConfNb=1
SH.ADCx_IN5.0=ADC1_IN5,IN5
@ -284,11 +278,15 @@ SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2
SH.S_TIM1_CH2.ConfNb=1
SH.S_TIM1_CH3.0=TIM1_CH3,PWM Generation3 CH3
SH.S_TIM1_CH3.ConfNb=1
SPI1.CalculateBaudRate=45.0 MBits/s
SPI1.Direction=SPI_DIRECTION_2LINES
SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate
SPI1.Mode=SPI_MODE_MASTER
SPI1.VirtualType=VM_MASTER
SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64
SPI2.CLKPhase=SPI_PHASE_2EDGE
SPI2.CLKPolarity=SPI_POLARITY_LOW
SPI2.CalculateBaudRate=703.125 KBits/s
SPI2.DataSize=SPI_DATASIZE_16BIT
SPI2.Direction=SPI_DIRECTION_2LINES
SPI2.IPParameters=VirtualType,Mode,Direction,DataSize,CLKPolarity,CLKPhase,CalculateBaudRate,BaudRatePrescaler
SPI2.Mode=SPI_MODE_MASTER
SPI2.VirtualType=VM_MASTER
TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
TIM1.BreakState=TIM_BREAK_DISABLE
TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1