update for smo

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

Applications/app/app.c

@@ -158,7 +158,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_motRPMFilted, (s16)PMSM_FOC_GetSpeed());
+		can_plot2((s16)foc_observer_smo_speed(), (s16)PMSM_FOC_GetSpeed());
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_RefTorque(), eCtrl_get_FinalTorque());
 	}else if (plot_type == 3) {

Applications/foc/foc_config.h

@@ -67,8 +67,8 @@
 #ifdef CONFIG_SMO_OBSERVER
 	#define CONFIG_SMO_MIN_SPEED    1000 //RPM
 	//#define CONFIG_SMO_PLL_BANDWITH 2000.0f //计算角度和速度的pll
-	#define CONFIG_SMO_PLL_BANDWITH (1000.0f * 2 * PI) //计算速度的pll
-	#define CONFIG_SMO_LFP_WC        (50.0F * 2* PI)
+	#define CONFIG_SMO_PLL_BANDWITH (200.0f * 2 * PI) //计算速度的pll
+	#define CONFIG_SMO_LFP_WC        (200.0F * 2* PI)
 	#define CONFIG_SMO_GAIN_K        100.0f
 	#define CONFIG_SMO_SIGMOID_MAX   120.0F
 #endif

Applications/foc/limit.c

@@ -144,6 +144,7 @@ u16 vbus_current_vol_lower_limit(void) {
 		u16 lim_value = _vol_limiter(vol, lim);
 		if (lim_value != HW_LIMIT_NONE) {
 			mc_set_critical_error(FOC_CRIT_UN_Vol_Err);
+			err_add_record(FOC_CRIT_UN_Vol_Err, vol);
 			return lim_value;
 		}
 	}

Applications/foc/motor/motor.c

@@ -924,17 +924,17 @@ void MC_Protect_IRQHandler(void){
 	if (!motor.b_start) {
 		return;
 	}
-	mc_error.vbus = (s16)get_vbus_float(); 
-	mc_error.ibus = (s16)get_vbus_current();
-	mc_error.id_ref = (s16)PMSM_FOC_Get()->idq_ctl[0].s_Cp;
-	mc_error.iq_ref = (s16)PMSM_FOC_Get()->idq_ctl[1].s_Cp;
+	mc_error.vbus_x10 = (s16)(get_vbus_float() * 10.0f); 
+	mc_error.ibus_x10 = (s16)(get_vbus_current() * 10.0f);
+	mc_error.id_ref_x10 = (s16)(PMSM_FOC_Get()->idq_ctl[0].s_Cp * 10.0f);
+	mc_error.iq_ref_x10 = (s16)(PMSM_FOC_Get()->idq_ctl[1].s_Cp * 10.0f);
+	mc_error.id_x10 = (s16)(PMSM_FOC_Get()->out.s_RealIdq.d * 10.0f);
+	mc_error.iq_x10 = (s16)(PMSM_FOC_Get()->out.s_RealIdq.q * 10.0f);
+	mc_error.vd_x10 = (s16)(PMSM_FOC_Get()->out.s_OutVdq.d * 10.0f);
+	mc_error.vq_x10 = (s16)(PMSM_FOC_Get()->out.s_OutVdq.q * 10.0f);
+	mc_error.torque_ref_x10 = (s16)(PMSM_FOC_Get()->in.s_targetTorque * 10.0f);
 	mc_error.run_mode = PMSM_FOC_Get()->out.n_RunMode;
-	mc_error.id = (s16)PMSM_FOC_Get()->out.s_RealIdq.d;
-	mc_error.iq = (s16)PMSM_FOC_Get()->out.s_RealIdq.q;
-	mc_error.vd = (s16)PMSM_FOC_Get()->out.s_OutVdq.d;
-	mc_error.vq = (s16)PMSM_FOC_Get()->out.s_OutVdq.q;
 	mc_error.rpm = (s16)motor_encoder_get_speed();
-	mc_error.torque_ref = (s16)PMSM_FOC_Get()->in.s_targetTorque;
 	mc_error.b_smo_running = !foc_observer_is_encoder();
 	mc_error.mos_temp = get_mos_temp();
 	mc_error.mot_temp = get_motor_temp();
@@ -948,7 +948,40 @@ void MC_Protect_IRQHandler(void){
 }
 
 void motor_debug(void) {
-	sys_debug("err: %f, %f, %f, %d\n", mc_error.vbus, mc_error.id_ref, mc_error.iq_ref, mc_error.run_mode);
+	sys_debug("err: %f, %f, %f, %d\n", (float)mc_error.vbus_x10/10.0f, (float)mc_error.id_ref_x10/10.0f, (float)mc_error.iq_ref_x10/10.0f, mc_error.run_mode);
+}
+
+int mc_get_phase_errinfo(u8 *data, int dlen) {
+	int len = 0;
+	encode_s16(data, mc_error.vbus_x10);
+	len += 2;
+	encode_s16(data, mc_error.ibus_x10);
+	len += 2;
+	encode_s16(data, mc_error.id_ref_x10);
+	len += 2;
+	encode_s16(data, mc_error.iq_ref_x10);
+	len += 2;
+	encode_s16(data, mc_error.id_x10);
+	len += 2;
+	encode_s16(data, mc_error.iq_x10);
+	len += 2;
+	encode_s16(data, mc_error.vd_x10);
+	len += 2;
+	encode_s16(data, mc_error.vq_x10);
+	len += 2;
+	encode_s16(data, mc_error.torque_ref_x10);
+	len += 2;
+	encode_s16(data, mc_error.rpm);
+	len += 2;
+	encode_u8(data, mc_error.run_mode);
+	len += 1;
+	encode_u8(data, mc_error.b_smo_running);
+	len += 1;
+	encode_s16(data, mc_error.mos_temp);
+	len += 2;
+	encode_s16(data, mc_error.mot_temp);
+	len += 2;
+	return len;
 }
 
 void TIMER_UP_IRQHandler(void){

Applications/foc/motor/motor.h

@@ -47,16 +47,16 @@ typedef struct {
 }motor_t;
 
 typedef struct {
-	s16 vbus;
-	s16 ibus;
-	s16 id;
-	s16 iq;
-	s16 vd;
-	s16 vq;
-	s16 id_ref;
-	s16 iq_ref;
+	s16 vbus_x10;
+	s16 ibus_x10;
+	s16 id_x10;
+	s16 iq_x10;
+	s16 vd_x10;
+	s16 vq_x10;
+	s16 id_ref_x10;
+	s16 iq_ref_x10;
 	u8    run_mode;
-	s16 torque_ref;
+	s16 torque_ref_x10;
 	s16 rpm;
 	bool  b_smo_running;
 	s16    mos_temp;
@@ -97,6 +97,7 @@ bool mc_is_cruise_enabled(void);
 bool mc_set_cruise_speed(bool rpm_abs, float target_rpm);
 void mc_set_idc_limit(s16 limit);
 mc_gear_t *mc_get_gear_config(void);
+int mc_get_phase_errinfo(u8 *data, int dlen);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL

Applications/foc/motor/motor_param.h

@@ -93,7 +93,7 @@ float motor_get_lq_from_iq(s16 iq);
 #define TRQ_PI_KI 0.5F
 #define TRO_PI_KD 0.0F
 
-#define MOTOR_NR 0x14
+#define MOTOR_NR 0x13
 
 #define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200