A1电机的电流环带宽设置为1.6kHz

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/app/app.c

@@ -147,7 +147,7 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-int plot_type = 2;
+int plot_type = 4;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_smo_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_hallAngle;
@@ -161,7 +161,7 @@ static void plot_smo_angle(void) {
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		can_plot2((s16)(PMSM_FOC_Get()->in.s_targetTorque*10.0f), (s16)PMSM_FOC_GetSpeed());
+		can_plot2((s16)PMSM_FOC_Get()->in.s_motRPMFilted, (s16)PMSM_FOC_GetSpeed());
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_RefTorque(), eCtrl_get_FinalTorque());
 	}else if (plot_type == 3) {
@@ -169,7 +169,7 @@ static u32 _app_plot_task(void * args) {
 	}else if (plot_type == 4) {
 		can_plot2((s16)PMSM_FOC_Get()->out.s_RealIdq.d, (s16)PMSM_FOC_Get()->out.s_RealIdq.q);
 	}else if (plot_type == 5) {
-		can_plot2((s16)PMSM_FOC_Get()->out.s_FilterIdq.d, (s16)PMSM_FOC_Get()->out.s_FilterIdq.q);
+		can_plot2((s16)PMSM_FOC_Get()->idq_ctl[0].s_Cp, (s16)PMSM_FOC_Get()->idq_ctl[1].s_Cp);
 	}else if (plot_type == 6) {
 		can_plot2((s16)(PMSM_FOC_Get()->in.s_iABC[0]), (s16)(PMSM_FOC_Get()->in.s_iABC[1]));
 	}else if (plot_type == 7) {

Applications/app/nv_storage.c

@@ -494,7 +494,16 @@ void nv_storage_init(void) {
 #endif
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
 	m_params.offset = 0.0f; //编码器做了零位置校准
-	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
+	if (foc_params.n_currentBand != CONFIG_CURRENT_BANDWITH) {
+		foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
+		foc_params.pid_conf[PID_D_id].kp = (foc_params.n_currentBand * MOTOR_Ld);
+		foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
+		foc_params.pid_conf[PID_D_id].kd = 0;
+
+		foc_params.pid_conf[PID_Q_id].kp = (foc_params.n_currentBand * MOTOR_Lq);
+		foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
+		foc_params.pid_conf[PID_Q_id].kd = 0;
+	}
 	if (CONFIG_DEFAULT_PHASE_CURR_LIM != foc_params.s_PhaseCurrLim) {
 		foc_params.s_PhaseCurrLim = CONFIG_DEFAULT_PHASE_CURR_LIM;
 	}

Applications/bsp/board_mc100_v1.h

@@ -115,8 +115,8 @@
 
 #define CONFIG_PWM_UV_SWAP 1
 
-//#define CONFIG_HW_MUTISAMPLE ADC_OVERSAMPLING_RATIO_MUL2
-//#define CONFIG_HW_MUTISAMPLE_SHIFT ADC_OVERSAMPLING_SHIFT_1B
+//#define CONFIG_HW_MUTISAMPLE ADC_OVERSAMPLING_RATIO_MUL8
+//#define CONFIG_HW_MUTISAMPLE_SHIFT ADC_OVERSAMPLING_SHIFT_3B
 //#define CONFIG_SW_MUTISAMPLE 1
 
 /* 母线电压采集 */

Applications/bsp/board_mc105_v3.h

@@ -116,8 +116,8 @@
 
 #define CONFIG_PWM_UV_SWAP 1
 
-//#define CONFIG_HW_MUTISAMPLE ADC_OVERSAMPLING_RATIO_MUL2
-//#define CONFIG_HW_MUTISAMPLE_SHIFT ADC_OVERSAMPLING_SHIFT_1B
+//#define CONFIG_HW_MUTISAMPLE ADC_OVERSAMPLING_RATIO_MUL8
+//#define CONFIG_HW_MUTISAMPLE_SHIFT ADC_OVERSAMPLING_SHIFT_3B
 //#define CONFIG_SW_MUTISAMPLE 1
 
 /* 母线电压采集 */

Applications/foc/core/PI_Controller.h

@@ -149,8 +149,8 @@ typedef struct {
 	float DT;
 }PLL_t;
 
-static __INLINE void PLL_Reset(PLL_t *pll) {
-	pll->observer = 0.0f;
+static __INLINE void PLL_Reset(PLL_t *pll, float sample) {
+	pll->observer = sample;
 	pll->out = 0.0f;
 	pll->ob_wp = false;
 }

Applications/foc/core/PMSM_FOC_Core.c

@@ -241,7 +241,8 @@ void PMSM_FOC_CoreInit(void) {
 
 	gFoc_Ctrl.plot_type = Plot_None;
 }
-
+//#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
+//#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
 static __INLINE void PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
 
@@ -259,7 +260,8 @@ static __INLINE void PMSM_FOC_Update_Input(void) {
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
 	}
 	gFoc_Ctrl.in.s_motRPM = foc_observer_speed();
-
+	LowPass_Filter(gFoc_Ctrl.in.s_motRPMFilted, gFoc_Ctrl.in.s_motRPM, 0.005f);
+	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motRPM / 30.0f * PI * gFoc_Ctrl.params.n_poles;
 #ifdef CONFIG_DQ_STEP_RESPONSE
 	gFoc_Ctrl.in.s_hallAngle = 0;
 	gFoc_Ctrl.in.s_motAngle = 0;
@@ -275,6 +277,11 @@ static __INLINE void PMSM_FOC_Update_Input(void) {
 
 	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
 	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
+#ifdef Volvec_Delay_Comp
+	float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelDegreePers / PI * 180.0f * (FOC_CTRL_US - 2e-6f);
+	rand_angle(next_angle);
+	SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
+#endif
 }
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
@@ -304,17 +311,16 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 	}
 	return 1;
 }
-//#define UPDATE_Lq_By_iq
 
 static __INLINE void id_feedforward(float eW) {
-#ifdef MOTOR_Flux
+#ifdef CONFIG_CURRENT_LOOP_DECOUPE
 	gFoc_Ctrl.in.s_targetVdq.d += -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
 	gFoc_Ctrl.in.s_targetVdq.d = fclamp(gFoc_Ctrl.in.s_targetVdq.d, gFoc_Ctrl.pi_id.min, gFoc_Ctrl.pi_id.max);
 #endif
 }
 
 static __INLINE void iq_feedforward(float eW) {
-#ifdef MOTOR_Flux
+#ifdef CONFIG_CURRENT_LOOP_DECOUPE
 	gFoc_Ctrl.in.s_targetVdq.q += (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
 	gFoc_Ctrl.in.s_targetVdq.q = fclamp(gFoc_Ctrl.in.s_targetVdq.q, gFoc_Ctrl.pi_iq.min, gFoc_Ctrl.pi_iq.max);
 #endif
@@ -325,7 +331,6 @@ void PMSM_FOC_Schedule(void) {
 	gFoc_Ctrl.ctrl_count++;
 
 	PMSM_FOC_Update_Input();
-
 	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
 		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
@@ -337,10 +342,9 @@ void PMSM_FOC_Schedule(void) {
 	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
 	#endif
-		float eW = gFoc_Ctrl.in.s_motRPM / 30.0f * PI * gFoc_Ctrl.params.n_poles;
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
 		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(&gFoc_Ctrl.pi_id, err);
-		id_feedforward(eW);
+		id_feedforward(gFoc_Ctrl.in.s_motVelDegreePers);
 #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);
@@ -357,7 +361,7 @@ void PMSM_FOC_Schedule(void) {
 	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
 		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(&gFoc_Ctrl.pi_iq, err);
-		iq_feedforward(eW);
+		iq_feedforward(gFoc_Ctrl.in.s_motVelDegreePers);
 	}else {
 		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
 		float max_vd = max_Vdc * SQRT3_BY_2;
@@ -645,7 +649,6 @@ void PMSM_FOC_Slow_Task(void) {
 	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
 	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
 	eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
-
 	PMSM_FOC_idqCalc();
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -104,6 +104,8 @@ typedef struct {
 	bool    b_eBrake;
 	bool    b_epmMode;
 	bool    b_fwEnable;
+	float   s_motRPMFilted; //电机滤波后的转速
+	float   s_motVelDegreePers; //电机的电角速度
 	volatile bool    b_MTPA_calibrate;
 	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
 }FOC_InP;

Applications/foc/motor/encoder.c

@@ -26,6 +26,14 @@ static void _detect_off_finished(void);
 
 encoder_t g_encoder;
 
+static __INLINE u32 _abi_count(void) {
+#ifdef ENCODER_CC_INVERT
+	return (g_encoder.cpr - ENC_COUNT);
+#else
+	return ENC_COUNT;
+#endif
+}
+
 static __INLINE void encoder_pll_update_gain(void) {
 	if (g_encoder.pll_bandwidth_shadow != g_encoder.pll_bandwidth) {
 		g_encoder.pll_bandwidth = g_encoder.pll_bandwidth_shadow;
@@ -40,7 +48,7 @@ static void _init_pll(void) {
 	g_encoder.pll_bandwidth = 0;
 	g_encoder.pll_bandwidth_shadow = nv_get_motor_params()->est_pll_band;
 	encoder_pll_update_gain();
-	PLL_Reset(&g_encoder.est_pll);
+	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
 }
 
 
@@ -59,7 +67,6 @@ void encoder_set_bandwidth(float bandwidth) {
 }
 
 void encoder_init_clear(void) {
-	_init_pll();
 	g_encoder.cpr = ENC_MAX_RES;
 	g_encoder.enc_offset = nv_get_motor_params()->offset;
 	g_encoder.motor_poles = nv_get_motor_params()->poles;
@@ -74,6 +81,7 @@ void encoder_init_clear(void) {
 	g_encoder.cali_angle = INVALID_ANGLE;
 	g_encoder.enc_count_off = 0xFFFFFFFF;
 	g_encoder.b_cali_err = false;
+	_init_pll();
 }
 
 void encoder_lock_position(bool enable) {
@@ -115,14 +123,6 @@ static __INLINE bool encoder_run_pll(float cnt) {
 	return snap_to_zero_vel;
 }
 
-static __INLINE u32 _abi_count(void) {
-#ifdef ENCODER_CC_INVERT
-	return (g_encoder.cpr - ENC_COUNT);
-#else
-	return ENC_COUNT;
-#endif
-}
-
 /* 偏心补偿 */
 static __INLINE float _eccentricity_compensation(int cnt) {
 #ifdef FIR_PHASE_SHIFT

Applications/foc/motor/motor_param.h

@@ -59,10 +59,11 @@ float motor_get_lq_from_iq(s16 iq);
 #define TRO_PI_KD 0.0F
 #define MOTOR_NR 0x16
 
-#define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
+//4000->10, 34; 8000->8, 15
+#define CONFIG_CURRENT_BANDWITH  (1600 * 2 * PI) /* 电流环带宽 = bandwith / (2 * pi)*/
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 500
 #define CONFIG_MAX_FW_D_CURR     300.0F //d轴最大的退磁电流
-
+#define CONFIG_CURRENT_LOOP_DECOUPE    //电流环解耦
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW2
 #define MOTOR_R   0.013f
 #define MOTOR_Ld (0.000140f*0.5f)