|
|
@@ -1,1270 +0,0 @@
|
|
|
-#include "arm_math.h"
|
|
|
-#include "PMSM_FOC_Core.h"
|
|
|
-#include "foc/foc_config.h"
|
|
|
-#include "foc/mc_config.h"
|
|
|
-#include "foc/motor/motor_param.h"
|
|
|
-#include "foc/core/e_ctrl.h"
|
|
|
-#include "foc/core/etcs.h"
|
|
|
-#include "math/fix_math.h"
|
|
|
-#include "math/fast_math.h"
|
|
|
-#include "foc/motor/current.h"
|
|
|
-#include "foc/motor/motor.h"
|
|
|
-#include "foc/core/svpwm.h"
|
|
|
-#include "foc/core/thro_torque.h"
|
|
|
-#include "foc/core/foc_observer.h"
|
|
|
-#include "foc/core/F_Calc.h"
|
|
|
-#include "foc/samples.h"
|
|
|
-#include "foc/limit.h"
|
|
|
-#include "foc/mc_error.h"
|
|
|
-#include "app/nv_storage.h"
|
|
|
-#include "bsp/bsp_driver.h"
|
|
|
-#include "libs/logger.h"
|
|
|
-#include "math/fir.h"
|
|
|
-
|
|
|
-#define _DEBUG(fmt, args...) sys_debug(fmt, ##args)
|
|
|
-
|
|
|
-PMSM_FOC_Ctrl gFoc_Ctrl;
|
|
|
-static bool g_focinit = false;
|
|
|
-
|
|
|
-static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
|
|
|
- float c,s;
|
|
|
-#if 0
|
|
|
- arm_sin_cos(angle, &s, &c);
|
|
|
-#else
|
|
|
- s = gFoc_Ctrl.out.sin;
|
|
|
- c = gFoc_Ctrl.out.cos;
|
|
|
-#endif
|
|
|
-
|
|
|
- alpha_beta->a = dq->d * c - dq->q * s;
|
|
|
- alpha_beta->b = dq->d * s + dq->q * c;
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void RevClark(AB_t *alpha_beta, float *ABC){
|
|
|
- ABC[0] = alpha_beta->a;
|
|
|
- ABC[1] = -alpha_beta->a * 0.5f + alpha_beta->b * SQRT3_BY_2;
|
|
|
- ABC[2] = -alpha_beta->a * 0.5f - alpha_beta->b * SQRT3_BY_2;
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void Clark(float A, float B, float C, AB_t *alpha_beta){
|
|
|
- alpha_beta->a = A;
|
|
|
- alpha_beta->b = ONE_BY_SQRT3 * (B - C);
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
|
|
|
- float c,s;
|
|
|
-#if 0
|
|
|
- arm_sin_cos(angle, &s, &c);
|
|
|
-#else
|
|
|
- s = gFoc_Ctrl.out.sin;
|
|
|
- c = gFoc_Ctrl.out.cos;
|
|
|
-#endif
|
|
|
- dq->d = alpha_beta->a * c + alpha_beta->b * s;
|
|
|
- dq->q = -alpha_beta->a * s + alpha_beta->b * c;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_ABC2Dq(float a, float b, float c, float *d, float *q) {
|
|
|
- AB_t ab;
|
|
|
- DQ_t dq;
|
|
|
- Clark(a, b, c, &ab);
|
|
|
- Park(&ab, 0, &dq);
|
|
|
- *d = dq.d;
|
|
|
- *q = dq.q;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {
|
|
|
- float cp = c->s_Cp;
|
|
|
- c->s_FinalTgt = target;
|
|
|
- c->s_Step = (c->s_FinalTgt - cp) / (float)time;
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE float FOC_Get_DqRamp(dq_Rctrl *c) {
|
|
|
- if (++c->n_StepCount == c->n_CtrlCount) {
|
|
|
- c->s_Cp += c->s_Step;
|
|
|
- if (c->s_Step < 0) {
|
|
|
- if (c->s_Cp < c->s_FinalTgt) {
|
|
|
- c->s_Cp = c->s_FinalTgt;
|
|
|
- }
|
|
|
- }else {
|
|
|
- if (c->s_Cp > c->s_FinalTgt) {
|
|
|
- c->s_Cp = c->s_FinalTgt;
|
|
|
- }
|
|
|
- }
|
|
|
- c->n_StepCount = 0;
|
|
|
- }
|
|
|
- return c->s_Cp;
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void FOC_DqRamp_init(dq_Rctrl *c, int count) {
|
|
|
- c->n_CtrlCount = count;
|
|
|
- c->n_StepCount = 0;
|
|
|
- c->s_Cp = 0;
|
|
|
- c->s_FinalTgt = 0;
|
|
|
- c->s_Step = 0;
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, float target) {
|
|
|
- FOC_Set_DqRamp(c, target, (/*CONFIG_IDQ_CTRL_TS/CONFIG_SPD_CTRL_TS - 1*/CURRENT_LOOP_RAMP_COUNT));
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
|
|
|
- FOC_Set_DqRamp(c, target, (CONFIG_FOC_VDQ_RAMP_FINAL_TIME/1000*((CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS))));
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-static void PMSM_FOC_Reset_PID(void) {
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_id, 0);
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_iq, 0);
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_power, 0);
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, 0);
|
|
|
- ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, 0);
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel, 0);
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, 0);
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-static void PMSM_FOC_Conf_PID(void) {
|
|
|
- float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
|
|
|
- gFoc_Ctrl.pi_id.kp = mc_conf()->c.pid[PID_ID_ID].kp;
|
|
|
- gFoc_Ctrl.pi_id.ki = mc_conf()->c.pid[PID_ID_ID].ki;
|
|
|
- gFoc_Ctrl.pi_id.kd = mc_conf()->c.pid[PID_ID_ID].kd;
|
|
|
- gFoc_Ctrl.pi_id.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
|
|
|
-
|
|
|
- gFoc_Ctrl.pi_iq.kp = mc_conf()->c.pid[PID_IQ_ID].kp;
|
|
|
- gFoc_Ctrl.pi_iq.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
|
|
|
- gFoc_Ctrl.pi_iq.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
|
|
|
- gFoc_Ctrl.pi_iq.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
|
|
|
-
|
|
|
- gFoc_Ctrl.pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
|
|
|
- gFoc_Ctrl.pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
|
|
|
- gFoc_Ctrl.pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
|
|
|
- gFoc_Ctrl.pi_power.ts = slow_ctrl_ts;
|
|
|
-
|
|
|
- gFoc_Ctrl.pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
|
|
|
- gFoc_Ctrl.pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
|
|
|
- gFoc_Ctrl.pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
|
|
|
- gFoc_Ctrl.pi_lock.ts = slow_ctrl_ts;
|
|
|
-
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_init(&gFoc_Ctrl.vel_lim_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
|
|
|
- ladrc_init(&gFoc_Ctrl.vel_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_Wo, nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
|
|
|
-#else
|
|
|
- gFoc_Ctrl.pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
|
|
|
- gFoc_Ctrl.pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
|
|
|
- gFoc_Ctrl.pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
|
|
|
- gFoc_Ctrl.pi_vel_lim.ts = slow_ctrl_ts;
|
|
|
- gFoc_Ctrl.pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
|
|
|
- gFoc_Ctrl.pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
|
|
|
- gFoc_Ctrl.pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
|
|
|
- gFoc_Ctrl.pi_vel.ts = slow_ctrl_ts;
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-static void PMSM_FOC_UserInit(void) {
|
|
|
- memset(&gFoc_Ctrl.userLim, 0, sizeof(gFoc_Ctrl.userLim));
|
|
|
- gFoc_Ctrl.userLim.s_iDCLim = min(mc_conf()->c.max_idc, gFoc_Ctrl.hwLim.s_iDCMax);
|
|
|
- gFoc_Ctrl.userLim.s_motRPMLim = min(mc_conf()->c.max_rpm, gFoc_Ctrl.hwLim.s_motRPMMax);
|
|
|
- gFoc_Ctrl.userLim.s_torqueLim = mc_conf()->c.max_torque;//MAX_TORQUE;
|
|
|
- gFoc_Ctrl.userLim.s_PhaseCurrLim = min(mc_conf()->c.max_phase_curr, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
|
|
|
- gFoc_Ctrl.userLim.s_vDCMaxLim = mc_conf()->c.max_dc_vol;
|
|
|
- gFoc_Ctrl.userLim.s_vDCMinLim = mc_conf()->c.min_dc_vol;
|
|
|
- gFoc_Ctrl.userLim.s_iDCeBrkLim = 0xFF;
|
|
|
- gFoc_Ctrl.userLim.s_PhaseVoleBrkLim = gFoc_Ctrl.hwLim.s_PhaseVolMax;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_RT_LimInit(void) {
|
|
|
- gFoc_Ctrl.protLim.s_iDCLim = HW_LIMIT_NONE;
|
|
|
- gFoc_Ctrl.protLim.s_TorqueLim = HW_LIMIT_NONE;
|
|
|
-
|
|
|
- eRamp_init_target2(&gFoc_Ctrl.rtLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim, CONFIG_LIMIT_RAMP_TIME);
|
|
|
- eRamp_init_target2(&gFoc_Ctrl.rtLim.torqueLimRamp, gFoc_Ctrl.userLim.s_torqueLim, CONFIG_LIMIT_RAMP_TIME);
|
|
|
- eRamp_init_target2(&gFoc_Ctrl.rtLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim, CONFIG_LIMIT_RAMP_TIME);
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_CoreInit(void) {
|
|
|
-
|
|
|
- PMSM_FOC_Conf_PID();
|
|
|
-
|
|
|
- memset(&gFoc_Ctrl.in, 0, sizeof(FOC_InP));
|
|
|
- memset(&gFoc_Ctrl.out, 0, sizeof(FOC_OutP));
|
|
|
-
|
|
|
- gFoc_Ctrl.hwLim.s_iDCMax = CONFIG_HW_MAX_DC_CURRENT;
|
|
|
- gFoc_Ctrl.hwLim.s_motRPMMax = CONFIG_HW_MAX_MOTOR_RPM;
|
|
|
- gFoc_Ctrl.hwLim.s_PhaseCurrMax = CONFIG_HW_MAX_PHASE_CURR;
|
|
|
- gFoc_Ctrl.hwLim.s_PhaseVolMax = CONFIG_HW_MAX_PHASE_VOL;
|
|
|
- gFoc_Ctrl.hwLim.s_vDCMax = CONFIG_HW_MAX_DC_VOLTAGE;
|
|
|
- gFoc_Ctrl.hwLim.s_torqueMax = mc_conf()->m.max_torque; //电机的最大扭矩
|
|
|
- gFoc_Ctrl.hwLim.s_FWDCurrMax = mc_conf()->m.max_fw_id; //电池能支持的最大弱磁电流
|
|
|
- if (!g_focinit) {
|
|
|
- PMSM_FOC_UserInit();
|
|
|
- PMSM_FOC_RT_LimInit();
|
|
|
- g_focinit = true;
|
|
|
- //_DEBUG("User Limit:\n");
|
|
|
- //_DEBUG("dc %f, rpm %f, torque %f, phase %f, vDCmax %f, vDCmin %f, ebrk %f\n", gFoc_Ctrl.userLim.s_iDCLim, gFoc_Ctrl.userLim.s_motRPMLim, gFoc_Ctrl.userLim.s_torqueLim,
|
|
|
- // gFoc_Ctrl.userLim.s_PhaseCurrLim, gFoc_Ctrl.userLim.s_vDCMaxLim, gFoc_Ctrl.userLim.s_vDCMinLim, gFoc_Ctrl.userLim.s_TorqueBrkLim);
|
|
|
- //_DEBUG("Hw Limit:\n");
|
|
|
- //_DEBUG("dc %f, rpm %f, torque %f, phase %f\n", gFoc_Ctrl.hwLim.s_iDCMax, gFoc_Ctrl.hwLim.s_motRPMMax, gFoc_Ctrl.hwLim.s_torqueMax, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
|
|
|
- }
|
|
|
- gFoc_Ctrl.userLim.s_TorqueBrkLim = mc_get_ebrk_torque();
|
|
|
- gFoc_Ctrl.params.n_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
|
|
|
- gFoc_Ctrl.params.n_poles = mc_conf()->m.poles;//MOTOR_POLES;
|
|
|
- gFoc_Ctrl.params.lq = mc_conf()->m.lq;
|
|
|
- gFoc_Ctrl.params.ld = mc_conf()->m.lq;
|
|
|
- gFoc_Ctrl.params.flux = mc_conf()->m.flux;
|
|
|
- gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
|
|
|
- gFoc_Ctrl.in.s_dqAngle = INVALID_ANGLE;
|
|
|
- gFoc_Ctrl.in.s_vDC = sample_vbus_raw();
|
|
|
- gFoc_Ctrl.in.s_angleLast = INVALID_ANGLE;
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
|
|
|
- gFoc_Ctrl.out.f_vdqRation = 0;
|
|
|
-
|
|
|
- eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_CRUISE_RAMP_TIME);
|
|
|
-
|
|
|
- FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[0], 1);
|
|
|
- FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[1], 1);
|
|
|
-
|
|
|
- FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[0], (CONFIG_FOC_VDQ_RAMP_TS));
|
|
|
- FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_FOC_VDQ_RAMP_TS));
|
|
|
- PMSM_FOC_Reset_PID();
|
|
|
-
|
|
|
- foc_observer_init();
|
|
|
-
|
|
|
- gFoc_Ctrl.plot_type = Plot_None;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-#define CONFIG_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
|
|
|
-#define CONFIG_PHASE_UNBALANCE_THROLD 4.0F
|
|
|
-#define CONFIG_PHASE_UNBALANCE_R 0.1F
|
|
|
-static float phase_unbalance_r = 0.0f;
|
|
|
-static float phase_a_max, phase_b_max, phase_c_max;
|
|
|
-static u32 phase_unbalance_cnt;
|
|
|
-static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
|
|
|
- static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
|
|
|
- static float a_max = 0, b_max = 0, c_max = 0;
|
|
|
- static u32 _unbalance_cnt = 0;
|
|
|
- static u32 _unbalance_time = 0;
|
|
|
- float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US / 2.0f;
|
|
|
- if (lowpass > 1.0f) {
|
|
|
- lowpass = 1.0f;
|
|
|
- }
|
|
|
- LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], lowpass);
|
|
|
- LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], lowpass);
|
|
|
- gFoc_Ctrl.in.s_iABCFilter[2] = -(gFoc_Ctrl.in.s_iABCFilter[0] + gFoc_Ctrl.in.s_iABCFilter[1]);
|
|
|
- if ((gFoc_Ctrl.in.s_angleLast == INVALID_ANGLE) || (gFoc_Ctrl.in.s_motVelRadusPers < 100)) {
|
|
|
- gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
|
|
|
- a_max = b_max = c_max = 0;
|
|
|
- _unbalance_cnt = 0;
|
|
|
- _unbalance_time = get_tick_ms();
|
|
|
- _cycle_cnt = 0;
|
|
|
- _last_mod_cnt = 0;
|
|
|
- phase_unbalance_r = 0;
|
|
|
- return;
|
|
|
- }
|
|
|
- float delta_angle = gFoc_Ctrl.in.s_motAngle - gFoc_Ctrl.in.s_angleLast;
|
|
|
- if (delta_angle > 200 || delta_angle < -200) { //one cycle
|
|
|
- _cycle_cnt ++;
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
|
|
|
- u32 mod_cnt = _cycle_cnt % CONFIG_PEAK_CNT;
|
|
|
- bool trigger = false;
|
|
|
- if ((mod_cnt == 0) && (_last_mod_cnt != mod_cnt)) {
|
|
|
- trigger = true;
|
|
|
- }
|
|
|
- _last_mod_cnt = mod_cnt;
|
|
|
-
|
|
|
- a_max = MAX(a_max, gFoc_Ctrl.in.s_iABCFilter[0] * (2.2f));
|
|
|
- b_max = MAX(b_max, gFoc_Ctrl.in.s_iABCFilter[1] * (2.2f));
|
|
|
- c_max = MAX(c_max, gFoc_Ctrl.in.s_iABCFilter[2] * (2.2f));
|
|
|
- if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
|
|
|
- float i_min = 1000.0f, i_max = 0;
|
|
|
- if (a_max > i_max) {
|
|
|
- i_max = a_max;
|
|
|
- }
|
|
|
- if (a_max < i_min) {
|
|
|
- i_min = a_max;
|
|
|
- }
|
|
|
- if (b_max > i_max) {
|
|
|
- i_max = b_max;
|
|
|
- }
|
|
|
- if (b_max < i_min) {
|
|
|
- i_min = b_max;
|
|
|
- }
|
|
|
- if (c_max > i_max) {
|
|
|
- i_max = c_max;
|
|
|
- }
|
|
|
- if (c_max < i_min) {
|
|
|
- i_min = c_max;
|
|
|
- }
|
|
|
- float unbalance_r = (i_max - i_min - CONFIG_PHASE_UNBALANCE_THROLD)/(i_max + 1e-8f);
|
|
|
- if (unbalance_r >= CONFIG_PHASE_UNBALANCE_R) {
|
|
|
- if ((_unbalance_cnt++ >= 500) || (get_delta_ms(_unbalance_time) >= 1000*10)) {
|
|
|
- if (mc_set_critical_error(FOC_CRIT_PHASE_UNBalance_Err)) {
|
|
|
- mc_crit_err_add(FOC_CRIT_PHASE_UNBalance_Err, (s16)i_max, (s16)i_min);
|
|
|
- }
|
|
|
- }
|
|
|
- }else {
|
|
|
- _unbalance_cnt = 0;
|
|
|
- _unbalance_time = get_tick_ms();
|
|
|
- }
|
|
|
- phase_unbalance_r = unbalance_r;
|
|
|
- phase_a_max = a_max;
|
|
|
- phase_b_max = b_max;
|
|
|
- phase_c_max = c_max;
|
|
|
- phase_unbalance_cnt = _unbalance_cnt;
|
|
|
- a_max = b_max = c_max = 0;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-/* 死区补偿 */
|
|
|
-static __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
|
|
|
-#ifdef CONFIG_START_LINE_DTC_CURRENT
|
|
|
- float deadTime = (float)CONFIG_HW_DeadTime/2.0f;
|
|
|
- s32 dutyDTCA = 0;
|
|
|
- s32 dutyDTCB = 0;
|
|
|
- s32 dutyDTCC = 0;
|
|
|
- float r, delta;
|
|
|
- float iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[0]);
|
|
|
- if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
|
|
|
- delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
|
|
|
- r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
|
|
|
- if (r > 1.0f) {
|
|
|
- r = 1.0f;
|
|
|
- }
|
|
|
- if (gFoc_Ctrl.in.s_iABC_DT[0] < 0) {
|
|
|
- r = -r;
|
|
|
- }
|
|
|
- dutyDTCA = (s32)(r * deadTime);
|
|
|
- }
|
|
|
- iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[1]);
|
|
|
- if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
|
|
|
- delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
|
|
|
- r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
|
|
|
- if (r > 1.0f) {
|
|
|
- r = 1.0f;
|
|
|
- }
|
|
|
- if (gFoc_Ctrl.in.s_iABC_DT[1] < 0) {
|
|
|
- r = -r;
|
|
|
- }
|
|
|
- dutyDTCB = (s32)(r * deadTime);
|
|
|
- }
|
|
|
- iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[2]);
|
|
|
- if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
|
|
|
- delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
|
|
|
- r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
|
|
|
- if (r > 1.0f) {
|
|
|
- r = 1.0f;
|
|
|
- }
|
|
|
- if (gFoc_Ctrl.in.s_iABC_DT[2] < 0) {
|
|
|
- r = -r;
|
|
|
- }
|
|
|
- dutyDTCC = (s32)(r * deadTime);
|
|
|
- }
|
|
|
- s32 dutyA = (s32)gFoc_Ctrl.out.n_Duty[0] + dutyDTCA;
|
|
|
- s32 dutyB = (s32)gFoc_Ctrl.out.n_Duty[1] + dutyDTCB;
|
|
|
- s32 dutyC = (s32)gFoc_Ctrl.out.n_Duty[2] + dutyDTCC;
|
|
|
- gFoc_Ctrl.out.n_Duty[0] = sclamp(dutyA, 0, PWM_Half_Period);
|
|
|
- gFoc_Ctrl.out.n_Duty[1] = sclamp(dutyB, 0, PWM_Half_Period);
|
|
|
- gFoc_Ctrl.out.n_Duty[2] = sclamp(dutyC, 0, PWM_Half_Period);
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void Phase_Voltage_update(float lowpass) {
|
|
|
- float v_ABC[3];
|
|
|
- get_uvw_phases_raw(v_ABC);
|
|
|
- LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[0], v_ABC[0], lowpass);
|
|
|
- LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[1], v_ABC[1], lowpass);
|
|
|
- LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[2], v_ABC[2], lowpass);
|
|
|
- /* phase voltage = phase-phase voltage / sqrt(3), 1.4是滤波器幅值补偿系数 */
|
|
|
- float phase_vAN = (gFoc_Ctrl.in.s_SamplePhaseV[0] - gFoc_Ctrl.in.s_SamplePhaseV[1]) * ONE_BY_SQRT3 * 1.4f;
|
|
|
- float phase_vBN = (gFoc_Ctrl.in.s_SamplePhaseV[1] - gFoc_Ctrl.in.s_SamplePhaseV[2]) * ONE_BY_SQRT3 * 1.4f;
|
|
|
- float phase_vCN = (gFoc_Ctrl.in.s_SamplePhaseV[2] - gFoc_Ctrl.in.s_SamplePhaseV[0]) * ONE_BY_SQRT3 * 1.4f;
|
|
|
- Clark(phase_vAN, phase_vBN, phase_vCN, &gFoc_Ctrl.out.s_SampleAB);
|
|
|
- Park(&gFoc_Ctrl.out.s_SampleAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_SamplevDQ);
|
|
|
-}
|
|
|
-
|
|
|
-//#define UPDATE_Lq_By_iq /* Q轴电感 通过Iq电流补偿 */
|
|
|
-#define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
|
|
|
-#define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
|
|
|
-static float encoder_angle,obser_angle, obser_vel = 111111;
|
|
|
-static __INLINE bool PMSM_FOC_Update_Input(void) {
|
|
|
- AB_t iAB;
|
|
|
- float *iabc = gFoc_Ctrl.in.s_iABC;
|
|
|
-
|
|
|
- phase_current_get(iabc);
|
|
|
-
|
|
|
- Clark(iabc[0], iabc[1], iabc[2], &iAB);
|
|
|
-
|
|
|
- foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * TWO_BY_THREE, gFoc_Ctrl.out.s_OutVAB.b * TWO_BY_THREE, iAB.a, iAB.b);
|
|
|
-
|
|
|
- float enc_angle = motor_encoder_get_angle();
|
|
|
- float enc_vel = motor_encoder_get_speed();
|
|
|
- if (!foc_observer_diagnostic(enc_angle, enc_vel)){
|
|
|
- /* detect encoder angle error, do something here */
|
|
|
- if (!foc_observer_sensorless_stable()) {
|
|
|
- gFoc_Ctrl.in.s_motVelocity = 0;
|
|
|
- return false;
|
|
|
- }
|
|
|
- if (obser_vel == 111111) {
|
|
|
- obser_vel = foc_observer_sensorless_speed();
|
|
|
- obser_angle = foc_observer_sensorless_angle();
|
|
|
- encoder_angle = enc_angle;
|
|
|
- }
|
|
|
- enc_angle = foc_observer_sensorless_angle();
|
|
|
- enc_vel = foc_observer_sensorless_speed();
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
|
|
|
- gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
|
|
|
- }else {
|
|
|
- gFoc_Ctrl.in.s_motAngle = enc_angle;
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.s_motVelocity = enc_vel;
|
|
|
- LowPass_Filter(gFoc_Ctrl.in.s_motVelocityFiltered, gFoc_Ctrl.in.s_motVelocity, 0.01f);
|
|
|
- gFoc_Ctrl.in.s_motVelRadusPers = gFoc_Ctrl.in.s_motVelocityFiltered / 30.0f * PI * gFoc_Ctrl.params.n_poles;
|
|
|
-
|
|
|
- PMSM_FOC_Phase_Unbalance();
|
|
|
-
|
|
|
-#ifdef CONFIG_DQ_STEP_RESPONSE
|
|
|
- gFoc_Ctrl.in.s_motAngle = 0;
|
|
|
-#endif
|
|
|
-
|
|
|
- gFoc_Ctrl.in.s_vDC = get_vbus_float();
|
|
|
-
|
|
|
- arm_sin_cos(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
|
|
|
- Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
|
|
|
-
|
|
|
- float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US;
|
|
|
- float iqLowPass = lowpass * 2.0f;
|
|
|
- if (iqLowPass > 1.0f) {
|
|
|
- iqLowPass = 1.0f;
|
|
|
- }else if (iqLowPass <= 0.0001f) {
|
|
|
- iqLowPass = 0.001f;
|
|
|
- }
|
|
|
- LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, iqLowPass);
|
|
|
- LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, iqLowPass);
|
|
|
- /* 使用低通后的dq电流重新变换得到abc电流,给死区补偿使用 */
|
|
|
- RevPark(&gFoc_Ctrl.out.s_FilterIdq, gFoc_Ctrl.in.s_motAngle, &iAB);
|
|
|
- RevClark(&iAB, gFoc_Ctrl.in.s_iABC_DT);
|
|
|
-
|
|
|
- Phase_Voltage_update(lowpass);
|
|
|
-
|
|
|
-#ifdef CONFIG_START_LINE_DTC_CURRENT
|
|
|
- gFoc_Ctrl.out.s_OutVdqDTC.d = 0;
|
|
|
- gFoc_Ctrl.out.s_OutVdqDTC.q = 0;
|
|
|
-#else
|
|
|
- AB_t vAB;
|
|
|
- vAB.a = (1.0f / 3.0f) * (2.0f * SIGN(gFoc_Ctrl.in.s_iABC_DT[0]) - SIGN(gFoc_Ctrl.in.s_iABC_DT[1]) - SIGN(gFoc_Ctrl.in.s_iABC_DT[2]));
|
|
|
- vAB.b = ONE_BY_SQRT3 * (SIGN(gFoc_Ctrl.in.s_iABC_DT[1]) - SIGN(gFoc_Ctrl.in.s_iABC_DT[2]));
|
|
|
- float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * gFoc_Ctrl.in.s_vDC;
|
|
|
- vAB.a = vAB.a * dtc;
|
|
|
- vAB.b = vAB.b * dtc;
|
|
|
- Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVdqDTC); //used for vbus current calc
|
|
|
-#endif
|
|
|
-#ifdef CONFIG_Volvec_Delay_Comp
|
|
|
- if (gFoc_Ctrl.in.s_motVelocityFiltered >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
|
|
|
- float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelRadusPers / PI * 180.0f * (FOC_CTRL_US * 0.8f);
|
|
|
- rand_angle(next_angle);
|
|
|
- arm_sin_cos(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
|
|
|
- }
|
|
|
-#endif
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE float id_feedforward(float eW) {
|
|
|
-#ifdef CONFIG_CURRENT_LOOP_DECOUPE
|
|
|
- return -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
|
|
|
-#else
|
|
|
- return 0;
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE float iq_feedforward(float eW) {
|
|
|
-#ifdef CONFIG_CURRENT_LOOP_DECOUPE
|
|
|
- return (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
|
|
|
-#else
|
|
|
- return 0;
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Schedule(void) {
|
|
|
-
|
|
|
- gFoc_Ctrl.ctrl_count++;
|
|
|
-
|
|
|
- if (!PMSM_FOC_Update_Input()){
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
|
|
|
-
|
|
|
- float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
|
|
|
- float max_vd = max_Vdc * SQRT3_BY_2;
|
|
|
-
|
|
|
- /* limiter Vd output for PI controller */
|
|
|
- gFoc_Ctrl.pi_id.max = max_vd;
|
|
|
- gFoc_Ctrl.pi_id.min = -max_vd;
|
|
|
- #ifndef CONFIG_DQ_STEP_RESPONSE
|
|
|
- float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
|
|
|
- #endif
|
|
|
- float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
|
|
|
- float id_ff = id_feedforward(gFoc_Ctrl.in.s_motVelRadusPers);
|
|
|
- gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_Current(&gFoc_Ctrl.pi_id, err, id_ff);
|
|
|
-#ifdef UPDATE_Lq_By_iq
|
|
|
- /* update kp&ki from lq for iq PI controller */
|
|
|
- float lq = motor_get_lq_from_iq((s16)gFoc_Ctrl.out.s_FilterIdq.q);
|
|
|
- LowPass_Filter(gFoc_Ctrl.params.lq, lq, 0.01f);
|
|
|
- gFoc_Ctrl.pi_iq.kp = ((float)nv_get_foc_params()->n_currentBand * gFoc_Ctrl.params.lq);
|
|
|
- gFoc_Ctrl.pi_iq.ki = (nv_get_motor_params()->r/gFoc_Ctrl.params.lq);
|
|
|
-#endif
|
|
|
- /* limiter Vq output for PI controller */
|
|
|
- float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
|
|
|
- gFoc_Ctrl.pi_iq.max = max_vq;
|
|
|
- gFoc_Ctrl.pi_iq.min = -max_vq;
|
|
|
- #ifndef CONFIG_DQ_STEP_RESPONSE
|
|
|
- float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
|
|
|
- #endif
|
|
|
- err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
|
|
|
- float iq_ff = iq_feedforward(gFoc_Ctrl.in.s_motVelRadusPers);
|
|
|
- gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_Current(&gFoc_Ctrl.pi_iq, err, iq_ff);
|
|
|
- }else {
|
|
|
- float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
|
|
|
- float max_vd = max_Vdc * SQRT3_BY_2;
|
|
|
- float vd_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[0]);
|
|
|
- gFoc_Ctrl.in.s_targetVdq.d = fclamp(vd_ref, -max_vd, max_vd);
|
|
|
-
|
|
|
- float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
|
|
|
- float vq_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[1]);
|
|
|
- gFoc_Ctrl.in.s_targetVdq.q = fclamp(vq_ref, -max_vq, max_vq);
|
|
|
- }
|
|
|
-
|
|
|
- gFoc_Ctrl.out.s_OutVdq.d = gFoc_Ctrl.in.s_targetVdq.d;
|
|
|
- gFoc_Ctrl.out.s_OutVdq.q = gFoc_Ctrl.in.s_targetVdq.q;
|
|
|
-
|
|
|
- RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVAB);
|
|
|
-
|
|
|
- SVM_Duty_Fix(&gFoc_Ctrl.out.s_OutVAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);
|
|
|
-
|
|
|
- PMSM_FOC_DeadTime_Compensate((s32)FOC_PWM_Half_Period);
|
|
|
-
|
|
|
- phase_current_point(&gFoc_Ctrl.out);
|
|
|
-
|
|
|
- pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
|
|
|
- pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
|
|
|
-
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_LogDebug(void) {
|
|
|
- sys_debug("DC curr %f --- %f, %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_FilteriDC, gFoc_Ctrl.out.s_CalciDC2);
|
|
|
- sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
|
|
|
- sys_debug("unbalance: %d, %f, %f, %f, %f\n", phase_unbalance_cnt, phase_unbalance_r, phase_a_max, phase_b_max, phase_c_max);
|
|
|
- if (obser_vel != 111111) {
|
|
|
- sys_debug("AB error: %f,%f,%f\n", obser_angle, encoder_angle, obser_vel);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-/*called in media task */
|
|
|
-u8 PMSM_FOC_CtrlMode(void) {
|
|
|
- u8 preMode = gFoc_Ctrl.out.n_RunMode;
|
|
|
-
|
|
|
- if (!gFoc_Ctrl.in.b_motEnable) {
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
|
|
|
- }else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
|
|
|
- }else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_SPD || gFoc_Ctrl.in.b_cruiseEna){
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
|
|
|
- }else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
|
|
|
- }else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_EBRAKE) {
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_EBRAKE;
|
|
|
- }else {
|
|
|
- if (!gFoc_Ctrl.in.b_cruiseEna) {
|
|
|
- gFoc_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
|
|
|
- }
|
|
|
- }
|
|
|
- if (preMode != gFoc_Ctrl.out.n_RunMode) {
|
|
|
- if ((preMode == CTRL_MODE_SPD) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- //ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
|
|
|
- ladrc_copy(&gFoc_Ctrl.vel_lim_adrc, &gFoc_Ctrl.vel_adrc);
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, gFoc_Ctrl.in.s_targetTorque);
|
|
|
-#endif
|
|
|
- }else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel, gFoc_Ctrl.in.s_targetTorque);
|
|
|
-#endif
|
|
|
- }else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, gFoc_Ctrl.in.s_targetTorque);
|
|
|
-#endif
|
|
|
- }else if ((preMode != gFoc_Ctrl.out.n_RunMode) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
|
|
|
- eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
|
|
|
- eCtrl_set_TgtTorque(motor_get_ebreak_toruqe(gFoc_Ctrl.in.s_motVelocity));
|
|
|
- }else if ((preMode == CTRL_MODE_EBRAKE) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, F_get_air());
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel, F_get_air());
|
|
|
-#endif
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- return gFoc_Ctrl.out.n_RunMode;
|
|
|
-}
|
|
|
-
|
|
|
-static void crosszero_step_towards(float *value, float target) {
|
|
|
- static float no_cro_step = CONFIG_CrossZero_NorStep;
|
|
|
- float v_now = *value;
|
|
|
- bool cross_zero = false;
|
|
|
- float nor_step = mc_conf()->cz.normal_step;
|
|
|
- float min_step = mc_conf()->cz.min_step;
|
|
|
- float min_ramp_torque = mc_conf()->cz.low;
|
|
|
- float high_ramp_torque = mc_conf()->cz.high;
|
|
|
- if (target > 0) {
|
|
|
- if (v_now < -min_ramp_torque) {
|
|
|
- step_towards(value, -min_ramp_torque + 0.001f, nor_step);
|
|
|
- cross_zero = true;
|
|
|
- }else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
|
|
|
- step_towards(value, target, min_step);
|
|
|
- cross_zero = true;
|
|
|
- }
|
|
|
- }else if (target == 0) {
|
|
|
- if (v_now > high_ramp_torque) {
|
|
|
- step_towards(value, high_ramp_torque - 0.001f, nor_step);
|
|
|
- cross_zero = true;
|
|
|
- }else if (v_now >= min_ramp_torque && v_now <= high_ramp_torque) {
|
|
|
- step_towards(value, target, min_step);
|
|
|
- cross_zero = true;
|
|
|
- }
|
|
|
- }else {
|
|
|
- if (v_now > high_ramp_torque) {
|
|
|
- step_towards(value, high_ramp_torque - 0.001f, nor_step);
|
|
|
- cross_zero = true;
|
|
|
- }else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
|
|
|
- step_towards(value, target, min_step);
|
|
|
- cross_zero = true;
|
|
|
- }
|
|
|
- }
|
|
|
- if (!cross_zero) {
|
|
|
- step_towards(&no_cro_step, nor_step, 0.1f);
|
|
|
- step_towards(value, target, no_cro_step);
|
|
|
- }else {
|
|
|
- no_cro_step = 0.5f;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-/* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
|
|
|
-static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
|
|
|
-#if 1
|
|
|
- gFoc_Ctrl.pi_power.max = maxTrq;
|
|
|
- float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.DCCurrLimRamp) - (gFoc_Ctrl.out.s_FilteriDC);
|
|
|
- return PI_Controller_Run(&gFoc_Ctrl.pi_power, errRef);
|
|
|
-#else
|
|
|
- return maxTrq;
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- float lim = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp);
|
|
|
- ladrc_set_range(&gFoc_Ctrl.vel_lim_adrc, 0, maxTrq);
|
|
|
- return ladrc_run(&gFoc_Ctrl.vel_lim_adrc, lim, gFoc_Ctrl.in.s_motVelocity);
|
|
|
-#else
|
|
|
- gFoc_Ctrl.pi_vel_lim.max = maxTrq;
|
|
|
- gFoc_Ctrl.pi_vel_lim.min = 0;
|
|
|
-
|
|
|
- float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motVelocity;
|
|
|
- return PI_Controller_RunVel(&gFoc_Ctrl.pi_vel_lim, err);
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-static __INLINE void PMSM_FOC_idq_Assign(void) {
|
|
|
- if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
|
|
|
- if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_dqAngle != INVALID_ANGLE)) {
|
|
|
- float s, c;
|
|
|
- normal_sincosf(degree_2_pi(gFoc_Ctrl.in.s_dqAngle + 90.0f), &s, &c);
|
|
|
- gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.in.s_targetCurrent * c;
|
|
|
-
|
|
|
- if (gFoc_Ctrl.in.s_targetIdq.d > gFoc_Ctrl.hwLim.s_FWDCurrMax) {
|
|
|
- gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.hwLim.s_FWDCurrMax;
|
|
|
- }else if (gFoc_Ctrl.in.s_targetIdq.d < -gFoc_Ctrl.hwLim.s_FWDCurrMax) {
|
|
|
- gFoc_Ctrl.in.s_targetIdq.d = -gFoc_Ctrl.hwLim.s_FWDCurrMax;
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.s_targetIdq.q = sqrtf(SQ(gFoc_Ctrl.in.s_targetCurrent) - SQ(gFoc_Ctrl.in.s_targetIdq.d));
|
|
|
- if (s < 0) {
|
|
|
- gFoc_Ctrl.in.s_targetIdq.q = -gFoc_Ctrl.in.s_targetIdq.q;
|
|
|
- }
|
|
|
- }else {
|
|
|
- gFoc_Ctrl.in.s_targetIdq.d = 0;
|
|
|
- gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent;
|
|
|
- }
|
|
|
- }else if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) ||
|
|
|
- (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
|
|
|
- motor_mpta_fw_lookup(gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
|
|
|
- }
|
|
|
- u32 mask = cpu_enter_critical();
|
|
|
- FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);
|
|
|
- FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[1], gFoc_Ctrl.in.s_targetIdq.q);
|
|
|
- cpu_exit_critical(mask);
|
|
|
-}
|
|
|
-
|
|
|
-/*called in media task */
|
|
|
-void PMSM_FOC_idqCalc(void) {
|
|
|
- if (gFoc_Ctrl.in.b_AutoHold) {
|
|
|
- float hold_torque = min(gFoc_Ctrl.protLim.s_TorqueLim, mc_conf()->c.max_autohold_torque);
|
|
|
- gFoc_Ctrl.pi_lock.max = hold_torque;
|
|
|
- gFoc_Ctrl.pi_lock.min = -hold_torque;
|
|
|
- float vel_count = motor_encoder_get_vel_count();
|
|
|
- float errRef = 0 - vel_count;
|
|
|
- gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(&gFoc_Ctrl.pi_lock ,errRef);
|
|
|
- PMSM_FOC_idq_Assign();
|
|
|
- return;
|
|
|
- }
|
|
|
- if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
|
|
|
- gFoc_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
|
|
|
- }else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE) {
|
|
|
- float maxTrq = eCtrl_get_RefTorque();
|
|
|
- if (eCtrl_get_FinalTorque() < 0.0001f && gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
|
|
|
- maxTrq = 0;
|
|
|
- }
|
|
|
- crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
|
|
|
- }else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
|
|
|
- float refTorque = min(eCtrl_get_RefTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp));
|
|
|
- float maxTrq = PMSM_FOC_Limit_Speed(refTorque);
|
|
|
- maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
|
|
|
- crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
|
|
|
- }else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
|
|
|
- float maxSpeed = eCtrl_get_FinalSpeed();
|
|
|
- float refSpeed = eCtrl_get_RefSpeed();
|
|
|
- if (gFoc_Ctrl.in.b_cruiseEna) {
|
|
|
- maxSpeed = eRamp_get_target(&gFoc_Ctrl.in.cruiseRpmRamp);
|
|
|
- refSpeed = eRamp_get_intepolation(&gFoc_Ctrl.in.cruiseRpmRamp);//gFoc_Ctrl.in.s_cruiseRPM;
|
|
|
- }
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- if (maxSpeed >= 0) {
|
|
|
- ladrc_set_range(&gFoc_Ctrl.vel_adrc, -CONFIG_MAX_NEG_TORQUE, eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp));
|
|
|
- }else if (maxSpeed < 0) {
|
|
|
- ladrc_set_range(&gFoc_Ctrl.vel_adrc, -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp), CONFIG_MAX_NEG_TORQUE);
|
|
|
- }
|
|
|
-
|
|
|
- if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
|
|
|
- ladrc_set_range(&gFoc_Ctrl.vel_adrc, 0, 0);
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.s_targetRPM = refSpeed;
|
|
|
- float maxTrq = ladrc_run(&gFoc_Ctrl.vel_adrc, refSpeed, gFoc_Ctrl.in.s_motVelocity);
|
|
|
-#else
|
|
|
- if (maxSpeed >= 0) {
|
|
|
- gFoc_Ctrl.pi_vel.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
|
|
|
-#ifdef CONFIG_SERVO_MOTOR
|
|
|
- gFoc_Ctrl.pi_vel.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
|
|
|
-#else
|
|
|
- gFoc_Ctrl.pi_vel.min = -CONFIG_MAX_NEG_TORQUE;
|
|
|
-#endif
|
|
|
- }else if (maxSpeed < 0) {
|
|
|
- gFoc_Ctrl.pi_vel.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
|
|
|
-#ifdef CONFIG_SERVO_MOTOR
|
|
|
- gFoc_Ctrl.pi_vel.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
|
|
|
-#else
|
|
|
- gFoc_Ctrl.pi_vel.max = CONFIG_MAX_NEG_TORQUE;
|
|
|
-#endif
|
|
|
- }
|
|
|
-
|
|
|
- if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
|
|
|
- gFoc_Ctrl.pi_vel.max = 0;
|
|
|
- gFoc_Ctrl.pi_vel.min = 0; //防止倒转
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.s_targetRPM = refSpeed;
|
|
|
- float errRef = refSpeed - gFoc_Ctrl.in.s_motVelocity;
|
|
|
- float maxTrq = PI_Controller_RunVel(&gFoc_Ctrl.pi_vel, errRef);
|
|
|
-#endif
|
|
|
- maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
|
|
|
- crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
|
|
|
- }
|
|
|
-
|
|
|
- PMSM_FOC_idq_Assign();
|
|
|
-}
|
|
|
-
|
|
|
-u8 PMSM_FOC_RunTime_Limit(void) {
|
|
|
- u8 changed = FOC_LIM_NO_CHANGE;
|
|
|
- float dc_lim = (float)vbus_under_vol_limit();
|
|
|
- float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
|
|
|
-
|
|
|
- if (gFoc_Ctrl.protLim.s_iDCLim != dc_lim || gFoc_Ctrl.protLim.s_TorqueLim != torque_lim) {
|
|
|
- if ((dc_lim > gFoc_Ctrl.protLim.s_iDCLim) || (torque_lim > gFoc_Ctrl.protLim.s_TorqueLim)) {
|
|
|
- changed = FOC_LIM_CHANGE_H;
|
|
|
- }else {
|
|
|
- changed = FOC_LIM_CHANGE_L;
|
|
|
- }
|
|
|
- gFoc_Ctrl.protLim.s_iDCLim = dc_lim;
|
|
|
- gFoc_Ctrl.protLim.s_TorqueLim = torque_lim;
|
|
|
- }
|
|
|
- return changed;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-bool PMSM_FOC_iDC_is_Limited(void) {
|
|
|
- return (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE);
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Torque_is_Limited(void) {
|
|
|
- return (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE);
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Slow_Task(void) {
|
|
|
- eRamp_running(&gFoc_Ctrl.rtLim.torqueLimRamp);
|
|
|
- eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
|
|
|
- eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
|
|
|
- eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
|
|
|
- PMSM_FOC_idqCalc();
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Change_VelLoop_Params(float wcv, float b0) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_change_b0(&gFoc_Ctrl.vel_adrc, b0);
|
|
|
- ladrc_change_K(&gFoc_Ctrl.vel_adrc, wcv);
|
|
|
-#else
|
|
|
- PI_Controller_Change_Kpi(&gFoc_Ctrl.pi_vel, wcv, b0);
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Change_TrqLoop_Params(float wcv, float b0) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, b0);
|
|
|
- ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, wcv);
|
|
|
-#else
|
|
|
- PI_Controller_Change_Kpi(&gFoc_Ctrl.pi_vel_lim, wcv, b0);
|
|
|
-#endif
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-float PMSM_FOC_Get_Real_dqVector(void) {
|
|
|
- if (gFoc_Ctrl.out.s_RealCurrentFiltered == 0) {
|
|
|
- gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
|
|
|
- }
|
|
|
- return gFoc_Ctrl.out.s_RealCurrentFiltered;
|
|
|
-}
|
|
|
-
|
|
|
-PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
|
|
|
- return &gFoc_Ctrl;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Start(u8 nCtrlMode) {
|
|
|
- if (gFoc_Ctrl.in.b_motEnable) {
|
|
|
- return;
|
|
|
- }
|
|
|
- PMSM_FOC_CoreInit();
|
|
|
- eCtrl_Reset();
|
|
|
- gFoc_Ctrl.in.n_ctlMode = nCtrlMode;
|
|
|
- gFoc_Ctrl.in.b_motEnable = true;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Stop(void) {
|
|
|
- if (!gFoc_Ctrl.in.b_motEnable) {
|
|
|
- return;
|
|
|
- }
|
|
|
- PMSM_FOC_CoreInit();
|
|
|
- gFoc_Ctrl.in.b_motEnable = false;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Is_Start(void) {
|
|
|
- return gFoc_Ctrl.in.b_motEnable;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_DCCurrLimit(float ibusLimit) {
|
|
|
- if (ibusLimit > gFoc_Ctrl.hwLim.s_iDCMax) {
|
|
|
- ibusLimit = gFoc_Ctrl.hwLim.s_iDCMax;
|
|
|
- }
|
|
|
- if (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE) {
|
|
|
- ibusLimit = min(ibusLimit, gFoc_Ctrl.protLim.s_iDCLim);
|
|
|
- }
|
|
|
- gFoc_Ctrl.userLim.s_iDCLim = ibusLimit;
|
|
|
-
|
|
|
- if (ABS(gFoc_Ctrl.in.s_motVelocity) <= CONFIG_ZERO_SPEED_RPM){
|
|
|
- eRamp_reset_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, ibusLimit);
|
|
|
- }else {
|
|
|
- eRamp_set_step_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, ibusLimit, CONFIG_eCTRL_STEP_TS);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-float PMSM_FOC_GetDCCurrLimit(void) {
|
|
|
- return gFoc_Ctrl.userLim.s_iDCLim;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SpeedRampLimit(float speedLimit, float speed) {
|
|
|
- if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
|
|
|
- speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
|
|
|
- }
|
|
|
- gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
|
|
|
- if (ABS(speed) <= CONFIG_ZERO_SPEED_RPM) {
|
|
|
- eRamp_reset_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit);
|
|
|
- }else {
|
|
|
- eRamp_set_step_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SpeedLimit(float speedLimit) {
|
|
|
- PMSM_FOC_SpeedRampLimit(speedLimit, gFoc_Ctrl.in.s_motVelocity);
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SpeedDirectLimit(float limit) {
|
|
|
- PMSM_FOC_SpeedRampLimit(limit, 0);
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-float PMSM_FOC_GetSpeedLimit(void) {
|
|
|
- return gFoc_Ctrl.userLim.s_motRPMLim;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_TorqueLimit(float torqueLimit) {
|
|
|
- if (torqueLimit > gFoc_Ctrl.hwLim.s_torqueMax) {
|
|
|
- torqueLimit = gFoc_Ctrl.hwLim.s_torqueMax;
|
|
|
- }
|
|
|
-
|
|
|
- if (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE) {
|
|
|
- torqueLimit = min(torqueLimit, gFoc_Ctrl.protLim.s_TorqueLim);
|
|
|
- }
|
|
|
-
|
|
|
- gFoc_Ctrl.userLim.s_torqueLim = torqueLimit;
|
|
|
-
|
|
|
- if (ABS(gFoc_Ctrl.in.s_motVelocity) <= CONFIG_ZERO_SPEED_RPM){
|
|
|
- eRamp_reset_target(&gFoc_Ctrl.rtLim.torqueLimRamp, torqueLimit);
|
|
|
- }else {
|
|
|
- eRamp_set_step_target(&gFoc_Ctrl.rtLim.torqueLimRamp, torqueLimit, CONFIG_eCTRL_STEP_TS);
|
|
|
- }
|
|
|
-}
|
|
|
-float PMSM_FOC_GetTorqueLimit(void) {
|
|
|
- return gFoc_Ctrl.userLim.s_torqueLim;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SetEbrkTorque(s16 torque) {
|
|
|
- gFoc_Ctrl.userLim.s_TorqueBrkLim = (float)torque;
|
|
|
- //gFoc_Ctrl.userLim.s_iDCeBrkLim = fclamp(dc_curr, 0, nv_get_foc_params()->s_iDCeBrkLim);
|
|
|
-}
|
|
|
-
|
|
|
-float PMSM_FOC_GetEbrkTorque(void) {
|
|
|
- if (!foc_observer_is_encoder()) {
|
|
|
- return 0; //无感运行关闭能量回收
|
|
|
- }
|
|
|
- return gFoc_Ctrl.userLim.s_TorqueBrkLim;
|
|
|
-}
|
|
|
-
|
|
|
-float PMSM_FOC_GetVbusVoltage(void) {
|
|
|
- return gFoc_Ctrl.in.s_vDC;
|
|
|
-}
|
|
|
-
|
|
|
-float PMSM_FOC_GetVbusCurrent(void) {
|
|
|
- return gFoc_Ctrl.out.s_FilteriDC;
|
|
|
-}
|
|
|
-
|
|
|
-DQ_t* PMSM_FOC_GetDQCurrent(void) {
|
|
|
- return &gFoc_Ctrl.out.s_RealIdq;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_SetCtrlMode(u8 mode) {
|
|
|
- if (mode > CTRL_MODE_EBRAKE) {
|
|
|
- PMSM_FOC_SetErrCode(FOC_Param_Err);
|
|
|
- return false;
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.n_ctlMode = mode;
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-u8 PMSM_FOC_GetCtrlMode(void) {
|
|
|
- return gFoc_Ctrl.in.n_ctlMode;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_PhaseCurrLim(float lim) {
|
|
|
- if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
|
|
|
- lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
|
|
|
- }
|
|
|
- gFoc_Ctrl.userLim.s_PhaseCurrLim = lim;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_RT_PhaseCurrLim(float lim) {
|
|
|
- if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
|
|
|
- lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
|
|
|
- }
|
|
|
- eRamp_init_target2(&gFoc_Ctrl.rtLim.torqueLimRamp, lim, CONFIG_LIMIT_RAMP_TIME);
|
|
|
-}
|
|
|
-
|
|
|
-float PMSM_FOC_GetPhaseCurrLim(void) {
|
|
|
- return gFoc_Ctrl.userLim.s_PhaseCurrLim;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SetOpenVdq(float vd, float vq) {
|
|
|
- FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[0], vd);
|
|
|
- FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[1], vq);
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SetOpenVdq_Immediate(float vd, float vq) {
|
|
|
- gFoc_Ctrl.vdq_ctl[0].s_Step = 0;
|
|
|
- gFoc_Ctrl.vdq_ctl[0].s_FinalTgt = vd;
|
|
|
- gFoc_Ctrl.vdq_ctl[0].s_Cp = vd;
|
|
|
-
|
|
|
- gFoc_Ctrl.vdq_ctl[1].s_Step = 0;
|
|
|
- gFoc_Ctrl.vdq_ctl[1].s_FinalTgt = vq;
|
|
|
- gFoc_Ctrl.vdq_ctl[1].s_Cp = vq;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_EnableCruise(bool enable) {
|
|
|
- if (enable != gFoc_Ctrl.in.b_cruiseEna) {
|
|
|
- float motSpd = PMSM_FOC_GetSpeed();
|
|
|
- if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
|
|
|
- PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
|
|
|
- return false;
|
|
|
- }
|
|
|
- eRamp_reset_target(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd);
|
|
|
- gFoc_Ctrl.in.s_cruiseRPM = motSpd;
|
|
|
- gFoc_Ctrl.in.b_cruiseEna = enable;
|
|
|
- }
|
|
|
-
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_PauseCruise(void) {
|
|
|
- gFoc_Ctrl.in.b_cruiseEna = false;
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_ResumeCruise(void) {
|
|
|
- gFoc_Ctrl.in.b_cruiseEna = true;
|
|
|
- eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, PMSM_FOC_GetSpeed(), CONFIG_CRUISE_RAMP_TIME);
|
|
|
- eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Is_CruiseEnabled(void) {
|
|
|
- return (gFoc_Ctrl.in.b_cruiseEna && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD));
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Set_TgtSpeed(float rpm) {
|
|
|
- if (gFoc_Ctrl.in.b_cruiseEna) {
|
|
|
- return false;
|
|
|
- }
|
|
|
- eCtrl_set_TgtSpeed(min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm));
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-bool PMSM_FOC_Set_Current(float is) {
|
|
|
- if (is > gFoc_Ctrl.userLim.s_PhaseCurrLim) {
|
|
|
- is = gFoc_Ctrl.userLim.s_PhaseCurrLim;
|
|
|
- }else if (is < -gFoc_Ctrl.userLim.s_PhaseCurrLim) {
|
|
|
- is = -gFoc_Ctrl.userLim.s_PhaseCurrLim;
|
|
|
- }
|
|
|
- eCtrl_set_TgtCurrent(is);
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Set_Torque(float trq) {
|
|
|
- if (trq > gFoc_Ctrl.userLim.s_torqueLim) {
|
|
|
- trq = gFoc_Ctrl.userLim.s_torqueLim;
|
|
|
- }else if (trq < -gFoc_Ctrl.userLim.s_torqueLim) {
|
|
|
- trq = -gFoc_Ctrl.userLim.s_torqueLim;
|
|
|
- }
|
|
|
- eCtrl_set_TgtTorque(trq);
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Reset_Torque(void) {
|
|
|
- float real_trq = PMSM_FOC_Get_Real_dqVector();
|
|
|
- eCtrl_reset_Torque(real_trq);
|
|
|
-}
|
|
|
-
|
|
|
-bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
|
|
|
- if (PMSM_FOC_Is_CruiseEnabled()) {
|
|
|
- if (rpm < CONFIG_MIN_CRUISE_RPM) {
|
|
|
- rpm = CONFIG_MIN_CRUISE_RPM;
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.s_cruiseRPM = min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm);
|
|
|
- eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
|
|
|
- return true;
|
|
|
- }
|
|
|
- PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_MTPA_Calibrate(bool enable) {
|
|
|
- if (enable) {
|
|
|
- gFoc_Ctrl.in.b_MTPA_calibrate = true;
|
|
|
- gFoc_Ctrl.in.s_dqAngle = 0;
|
|
|
- }else {
|
|
|
- gFoc_Ctrl.in.s_dqAngle = INVALID_ANGLE;
|
|
|
- gFoc_Ctrl.in.b_MTPA_calibrate = false;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Set_MotAngle(float angle) {
|
|
|
- gFoc_Ctrl.in.s_manualAngle = (angle);
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Set_Dq_Angle(float angle) {
|
|
|
- gFoc_Ctrl.in.s_dqAngle = (angle);
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
|
|
|
- dq->d = gFoc_Ctrl.in.s_targetIdq.d;
|
|
|
- dq->q = gFoc_Ctrl.in.s_targetIdq.q;
|
|
|
-}
|
|
|
-
|
|
|
-float PMSM_FOC_GetSpeed(void) {
|
|
|
- float speed = gFoc_Ctrl.in.s_motVelocity;
|
|
|
- if (!gFoc_Ctrl.in.b_motEnable || foc_observer_is_encoder()) {
|
|
|
- speed = motor_encoder_get_speed();
|
|
|
- }else {
|
|
|
- if (foc_observer_sensorless_stable()) {
|
|
|
- speed = foc_observer_sensorless_speed();
|
|
|
- }else {
|
|
|
- speed = 0;
|
|
|
- }
|
|
|
- }
|
|
|
- return speed;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-void PMSM_FOC_AutoHold(bool lock) {
|
|
|
- if (gFoc_Ctrl.in.b_AutoHold != lock) {
|
|
|
- motor_encoder_lock_pos(lock);
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
|
|
|
- if (!lock) {
|
|
|
- float hold_torque = gFoc_Ctrl.in.s_targetTorque * 1.1f;
|
|
|
- if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, hold_torque);
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, hold_torque);
|
|
|
-#endif
|
|
|
- }else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
|
|
|
-#ifdef CONFIG_SPEED_LADRC
|
|
|
- ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, hold_torque);
|
|
|
-#else
|
|
|
- PI_Controller_Reset(&gFoc_Ctrl.pi_vel, hold_torque);
|
|
|
-#endif
|
|
|
- }
|
|
|
- etcs_reset_torque(hold_torque);
|
|
|
- gFoc_Ctrl.out.f_autohold_trq = hold_torque;
|
|
|
- }else {
|
|
|
- gFoc_Ctrl.out.f_autohold_trq = 0;
|
|
|
- }
|
|
|
- gFoc_Ctrl.in.b_AutoHold = lock;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-bool PMSM_FOC_AutoHoldding(void) {
|
|
|
- return gFoc_Ctrl.in.b_AutoHold;
|
|
|
-}
|
|
|
-
|
|
|
-static PI_Controller *_pid(u8 id) {
|
|
|
- PI_Controller *pi = NULL;
|
|
|
- if (id == PID_ID_ID) {
|
|
|
- pi = &gFoc_Ctrl.pi_id;
|
|
|
- }else if (id == PID_IQ_ID) {
|
|
|
- pi = &gFoc_Ctrl.pi_iq;
|
|
|
- }else if (id == PID_VelLim_ID) {
|
|
|
-#ifndef CONFIG_SPEED_LADRC
|
|
|
- pi = &gFoc_Ctrl.pi_vel_lim;
|
|
|
-#endif
|
|
|
- }else if (id == PID_Vel_ID) {
|
|
|
-#ifndef CONFIG_SPEED_LADRC
|
|
|
- pi = &gFoc_Ctrl.pi_vel;
|
|
|
-#endif
|
|
|
- }else if (id == PID_AutoHold_ID) {
|
|
|
- pi = &gFoc_Ctrl.pi_lock;
|
|
|
- }
|
|
|
- return pi;
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd) {
|
|
|
- if (id > PID_Max_ID) {
|
|
|
- return;
|
|
|
- }
|
|
|
- PI_Controller *pi = _pid(id);
|
|
|
- if (pi != NULL) {
|
|
|
- pi->kp = kp;
|
|
|
- pi->ki = ki;
|
|
|
- pi->kd = kd;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kd) {
|
|
|
- if (id > PID_Max_ID) {
|
|
|
- return;
|
|
|
- }
|
|
|
- PI_Controller *pi = _pid(id);
|
|
|
- if (pi != NULL) {
|
|
|
- *kp = pi->kp;
|
|
|
- *ki = pi->ki;
|
|
|
- *kd = pi->kd;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_SetErrCode(u8 error) {
|
|
|
- if (gFoc_Ctrl.out.n_Error != error) {
|
|
|
- gFoc_Ctrl.out.n_Error = error;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-u8 PMSM_FOC_GetErrCode(void) {
|
|
|
- return gFoc_Ctrl.out.n_Error;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-void PMSM_FOC_Set_PlotType(Plot_t t) {
|
|
|
- gFoc_Ctrl.plot_type = t;
|
|
|
-}
|
|
|
-//获取母线电流和实际输出电流矢量大小
|
|
|
-void PMSM_FOC_Calc_Current(void) {
|
|
|
- float vd = gFoc_Ctrl.out.s_OutVdq.d - gFoc_Ctrl.out.s_OutVdqDTC.d * TWO_BY_THREE;
|
|
|
- float vq = gFoc_Ctrl.out.s_OutVdq.q - gFoc_Ctrl.out.s_OutVdqDTC.q * TWO_BY_THREE;
|
|
|
-
|
|
|
- float id = gFoc_Ctrl.out.s_FilterIdq.d;
|
|
|
- float iq = gFoc_Ctrl.out.s_FilterIdq.q;
|
|
|
- /*
|
|
|
- 根据公式（等幅值变换，功率不等）:
|
|
|
- iDC x vDC = 3/2(iq x vq + id x vd);
|
|
|
- */
|
|
|
- float m_pow = (vd * id + vq * iq);
|
|
|
- float raw_idc = 0.0f;
|
|
|
- float v_dc = get_vbus_float();
|
|
|
- if (v_dc != 0.0f) {
|
|
|
- raw_idc = m_pow / v_dc;
|
|
|
- }
|
|
|
- LowPass_Filter(gFoc_Ctrl.out.s_CalciDC, raw_idc, 0.02f);
|
|
|
-
|
|
|
- m_pow = (gFoc_Ctrl.out.s_SamplevDQ.d * id + gFoc_Ctrl.out.s_SamplevDQ.q * iq) * 1.5f;
|
|
|
- if (v_dc != 0.0f) {
|
|
|
- raw_idc = m_pow / v_dc;
|
|
|
- }
|
|
|
- LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, raw_idc, 0.02f);
|
|
|
-
|
|
|
- raw_idc = get_vbus_current();
|
|
|
- if (raw_idc != NO_VALID_CURRENT) {
|
|
|
- LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.05f);
|
|
|
- }else {
|
|
|
- gFoc_Ctrl.out.s_FilteriDC = gFoc_Ctrl.out.s_CalciDC;
|
|
|
- }
|
|
|
-
|
|
|
- gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
|
|
|
-
|
|
|
-}
|
|
|
-
|
|
|
-void PMSM_FOC_Brake(bool brake) {
|
|
|
- gFoc_Ctrl.in.b_eBrake = brake;
|
|
|
- if (gFoc_Ctrl.in.b_eBrake & gFoc_Ctrl.in.b_cruiseEna) {
|
|
|
- gFoc_Ctrl.in.b_cruiseEna = false;
|
|
|
- }
|
|
|
- eCtrl_brake_signal(brake);
|
|
|
-}
|
|
|
-
|
kevin