1. 更新pwm驱动，通过宏定义控制adc触发通过update事件，一个周期两次，暂时不用

2. 更新ladrc observer，1000rpm以上误差+-5度
3. 更新foc_observer.c, 修改判断encoder错误的方法
4. 修改s_motRPM to s_motVelocity
5. update simulink， ladrc observer等效2个低通滤波器串联

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

Applications/bsp/gd32/adc.h

@@ -17,7 +17,11 @@ inserted ADC 由timer0 ch3触发，
 #define IL_OFFSET   20
 
 #define ADC_SAMPLE_TIME ADC_SAMPLETIME_13POINT5
+#ifdef CONFIG_SENSORLESS_TOW_SAMPLES
+#define ADC_TRIGGER_PHASE ADC0_1_EXTTRIG_INSERTED_T0_TRGO
+#else
 #define ADC_TRIGGER_PHASE ADC0_1_EXTTRIG_INSERTED_T0_CH3
+#endif
 #define ADC_TRIGGER_PHASE2 ADC0_1_EXTTRIG_INSERTED_T1_CH0
 #define ADC_TRIGGER_NONE  ADC0_1_2_EXTTRIG_INSERTED_NONE
 #define ADC_TRIGGER_VBUS ADC0_1_EXTTRIG_INSERTED_T1_CH0

Applications/bsp/gd32/board_mc105_v3.h

@@ -17,7 +17,7 @@
 #define CONFIG_MAX_PHASE_VOL    (CONFIG_MOS_MAX_VOL - 20.0F)
 #define CONFIG_MAX_TORQUE       CONFIG_MAX_PHASE_CURR
 #define CONFIG_MAX_LOCK_TORQUE  20
-
+#define CONFIG_MAX_ACTIVE_EMF   5000.0F
 //#define CONFIG_BEEP 
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
 #define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转
@@ -31,10 +31,16 @@
 
 #define CONFIG_96V_MODE_VOL (60.0F)
 
+//#define CONFIG_SENSORLESS_TOW_SAMPLES
 //#define CONFIG_SMO_OBSERVER 
 #define CONFIG_LADRC_OBSERVER
 #define CONFIG_SPEED_LADRC  
 //#define CONFIG_FORCE_96V_MODE
+#ifdef CONFIG_SENSORLESS_TOW_SAMPLES
+#define CONFIG_SENSORLESS_TS (FOC_CTRL_US/2.0f)
+#else
+#define CONFIG_SENSORLESS_TS FOC_CTRL_US
+#endif
 
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5

Applications/bsp/gd32/pwm.c

@@ -130,7 +130,11 @@ static u8 _dead_time(u16 t) {
 		return (((u8)7<<3) + (t - 32)/16);
 	}
 }
-
+#ifdef CONFIG_SENSORLESS_TOW_SAMPLES
+#define PWM_CNT_REPEAT 0
+#else
+#define PWM_CNT_REPEAT 1
+#endif
 static void _init_pwm_timer(bool enable_brk) {
 	timer_oc_parameter_struct timer_ocintpara;
 	timer_parameter_struct timer_initpara;
@@ -149,7 +153,7 @@ static void _init_pwm_timer(bool enable_brk) {
 	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
     timer_initpara.period          = half_period;
     timer_initpara.clockdivision    = TIMER_CKDIV_DIV1;
-    timer_initpara.repetitioncounter = 1;
+    timer_initpara.repetitioncounter = PWM_CNT_REPEAT;
     timer_init(timer,&timer_initpara);
     /* auto-reload preload enable */
     timer_auto_reload_shadow_enable(timer);
@@ -207,8 +211,12 @@ static void _init_pwm_timer(bool enable_brk) {
 	_gpio_brakein_irq_enable();
 #endif
 
+#ifdef CONFIG_SENSORLESS_TOW_SAMPLES
+	timer_master_slave_mode_config(timer,TIMER_MASTER_SLAVE_MODE_ENABLE);
+	timer_master_output_trigger_source_select(timer,TIMER_TRI_OUT_SRC_UPDATE);
+#else
 	timer_master_slave_mode_config(timer,TIMER_MASTER_SLAVE_MODE_DISABLE);
-
+#endif
 	pwm_enable_channel();
 
 	timer_interrupt_disable(timer, TIMER_INT_UP);

Applications/bsp/gd32/pwm.h

@@ -84,6 +84,11 @@
 
 #define get_deadtime() (TIMER_CCHP(MOS_PWM_TIMER) & 0xFF)
 
+#ifdef CONFIG_SENSORLESS_TOW_SAMPLES
+#define PWM_Direction_Down() ((TIMER_CTL0(MOS_PWM_TIMER) & TIMER_CTL0_DIR) == 0)
+#else
+#define PWM_Direction_Down() true
+#endif
 void pwm_3phase_init(void);
 void pwm_3phase_sides(bool hon, bool lon);
 void pwm_start(void);

Applications/foc/core/PMSM_FOC_Core.c

@@ -243,9 +243,9 @@ void PMSM_FOC_CoreInit(void) {
 }
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 //#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
-static __INLINE void PMSM_FOC_Update_Input(void) {
+static __INLINE float PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
-
+	bool count_down = PWM_Direction_Down();
 	float *iabc = gFoc_Ctrl.in.s_iABC;
 
 	phase_current_get(iabc);
@@ -254,16 +254,25 @@ static __INLINE void PMSM_FOC_Update_Input(void) {
 
 	foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * 0.66667f, gFoc_Ctrl.out.s_OutVAB.b * 0.66667f, iAB.a, iAB.b);
 
+	if (!count_down) {
+		return false;
+	}
+	float enc_angle = motor_encoder_get_angle();
+	float enc_vel = motor_encoder_get_speed();
+	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
+		/* deltect encoder angle error, do something here */
+	}
+	
 	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
-		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
+		gFoc_Ctrl.in.s_hallAngle = enc_angle;
 	}else {
-		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
+		gFoc_Ctrl.in.s_hallAngle = enc_angle;
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
 	}
-	gFoc_Ctrl.in.s_motRPM = motor_encoder_get_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;
+	gFoc_Ctrl.in.s_motVelocity = enc_vel;
+	LowPass_Filter(gFoc_Ctrl.in.s_motRPMFilted, gFoc_Ctrl.in.s_motVelocity, 0.005f);
+	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motVelocity / 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;
@@ -284,6 +293,7 @@ static __INLINE void PMSM_FOC_Update_Input(void) {
 	rand_angle(next_angle);
 	SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 #endif
+	return true;
 }
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
@@ -307,7 +317,7 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 		}else if (gFoc_Ctrl.plot_type == Plot_DQ_Curr) {
 			plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.out.s_FilteriDC));
 		}else if (gFoc_Ctrl.plot_type == Plot_Spd_flow) {
-			plot_2data16(gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
+			plot_2data16(gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motVelocity);
 		}
 #endif
 	}
@@ -332,7 +342,9 @@ void PMSM_FOC_Schedule(void) {
 
 	gFoc_Ctrl.ctrl_count++;
 
-	PMSM_FOC_Update_Input();
+	if (!PMSM_FOC_Update_Input()){
+		return;
+	}
 	
 	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
@@ -392,27 +404,6 @@ void PMSM_FOC_Schedule(void) {
 	
 	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);
-
-	if (gFoc_Ctrl.plot_type != Plot_None) {
-		if (gFoc_Ctrl.ctrl_count % 5 == 0) {
-			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
-				plot_3data16(FtoS16(gFoc_Ctrl.in.s_iABC[0]), FtoS16(gFoc_Ctrl.in.s_iABC[1]), FtoS16(gFoc_Ctrl.in.s_iABC[2]));
-			}else if (gFoc_Ctrl.plot_type == Plot_Phase_vol) {
-				plot_3data16(FtoS16(gFoc_Ctrl.in.s_vABC[0]), FtoS16(gFoc_Ctrl.in.s_vABC[1]), FtoS16(gFoc_Ctrl.in.s_vABC[2]));
-			}else if (gFoc_Ctrl.plot_type == Plot_SMO_OBS) {
-#ifdef CONFIG_SMO_OBSERVER
-				float smo_angle = foc_observer_sensorless_angle();
-				float delta = smo_angle - gFoc_Ctrl.in.s_hallAngle;
-				if (delta > 180) {
-					delta -= 360;
-				}else if (delta < -180) {
-					delta += 360;
-				}
-				plot_3data16(gFoc_Ctrl.in.s_hallAngle, smo_angle, delta);
-#endif
-			}
-		}
-	}
 }
 
 void PMSM_FOC_LogDebug(void) {
@@ -441,14 +432,14 @@ u8 PMSM_FOC_CtrlMode(void) {
 	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_motRPM, gFoc_Ctrl.in.s_targetTorque);
+			//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_torque, 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_reset(&gFoc_Ctrl.vel_adrc, gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque);
+			//ladrc_reset(&gFoc_Ctrl.vel_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
 			ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
 #else
 			float target_troque = gFoc_Ctrl.in.s_targetTorque;
@@ -459,7 +450,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 #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_motRPM, gFoc_Ctrl.in.s_targetTorque);
+			ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
 #else
 			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 #endif
@@ -518,13 +509,13 @@ 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_motRPM);
+	return ladrc_run(&gFoc_Ctrl.vel_lim_adrc, lim, gFoc_Ctrl.in.s_motVelocity);
 #else
 #if 1
 	gFoc_Ctrl.pi_torque->max = maxTrq;
 	gFoc_Ctrl.pi_torque->min = 0;
 
-	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
+	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motVelocity;
 	return PI_Controller_RunLimit(&gFoc_Ctrl.pi_torque, err);
 #else
 	return maxTrq;
@@ -551,7 +542,7 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 		}
 	}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_motRPM, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
+		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);
@@ -574,7 +565,7 @@ void PMSM_FOC_idqCalc(void) {
 		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_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+		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);
@@ -597,11 +588,11 @@ void PMSM_FOC_idqCalc(void) {
 			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_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
+		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_motRPM);
+		float maxTrq = ladrc_run(&gFoc_Ctrl.vel_adrc, refSpeed, gFoc_Ctrl.in.s_motVelocity);
 #else
 		if (maxSpeed >= 0) {
 			gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
@@ -611,12 +602,12 @@ void PMSM_FOC_idqCalc(void) {
 			gFoc_Ctrl.pi_speed->max = CONFIG_MAX_NEG_TORQUE;
 		}
 
-		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
+		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
 			gFoc_Ctrl.pi_speed->max = 0;
 			gFoc_Ctrl.pi_speed->min = 0; //防止倒转
 		}
 		gFoc_Ctrl.in.s_targetRPM = refSpeed;
-		float errRef = refSpeed - gFoc_Ctrl.in.s_motRPM;
+		float errRef = refSpeed - gFoc_Ctrl.in.s_motVelocity;
 		float maxTrq = PI_Controller_Run(&gFoc_Ctrl.pi_speed, errRef);
 #endif
 		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_iDC(maxTrq);
@@ -697,7 +688,7 @@ void PMSM_FOC_DCCurrLimit(float ibusLimit) {
 	}
 	gFoc_Ctrl.userLim.s_iDCLim = ibusLimit;
 
-	if (ABS(gFoc_Ctrl.in.s_motRPM) <= CONFIG_ZERO_SPEED_RPM){
+	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);
@@ -721,7 +712,7 @@ void PMSM_FOC_SpeedRampLimit(float speedLimit, float speed) {
 }
 
 void PMSM_FOC_SpeedLimit(float speedLimit) {
-	PMSM_FOC_SpeedRampLimit(speedLimit, gFoc_Ctrl.in.s_motRPM);
+	PMSM_FOC_SpeedRampLimit(speedLimit, gFoc_Ctrl.in.s_motVelocity);
 }
 
 void PMSM_FOC_SpeedDirectLimit(float limit) {
@@ -744,7 +735,7 @@ void PMSM_FOC_TorqueLimit(float torqueLimit) {
 
 	gFoc_Ctrl.userLim.s_torqueLim = torqueLimit;
 
-	if (ABS(gFoc_Ctrl.in.s_motRPM) <= CONFIG_ZERO_SPEED_RPM){
+	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);
@@ -912,7 +903,7 @@ void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
 }
 
 float PMSM_FOC_GetSpeed(void) {
-	return gFoc_Ctrl.in.s_motRPM;
+	return gFoc_Ctrl.in.s_motVelocity;
 }
 
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -85,7 +85,7 @@ typedef struct {
 	float 	s_iABCFilter[3];
 	float   s_iABCComp[3];
 	float   s_vABC[3];
-	float   s_motRPM;   //from hall or encoder
+	float   s_motVelocity;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
 	float 	s_hallAngle;//from hall or encoder
 	float   s_targetRPM;

Applications/foc/core/foc_observer.c

@@ -14,7 +14,7 @@ void foc_observer_init(void) {
 	foc_obser.sensorless_angle = INVALID_ANGLE;
 	foc_obser.sensorless_est_angle = INVALID_ANGLE;
 	foc_obser.sensorless_speed = 0;
-	foc_obser.fusion_ceof = 1.0f;
+	foc_obser.enc_err_cnt = 0;
 #ifdef CONFIG_SMO_OBSERVER
 	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
 	foc_obser.is_sensorless_enable = true;
@@ -25,7 +25,7 @@ void foc_observer_init(void) {
 #endif
 }
 
-#define RPM_2_degree(rpm)  ((rpm) * 60.0f * nv_get_motor_params()->poles * FOC_CTRL_US)
+#define RPM_2_degree(rpm)  ((rpm) * 6.0f * nv_get_motor_params()->poles * FOC_CTRL_US)
 
 float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 	foc_obser.sensorless_est_angle = foc_obser.sensorless_angle + RPM_2_degree(foc_obser.sensorless_speed);
@@ -36,11 +36,44 @@ float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 	foc_obser.sensorless_angle = ladrc_observer_update(uAlp, uBeta, iAlp, iBeta);
 	foc_obser.sensorless_speed = ladrc_observer_vel();
 #endif
-	if (foc_obser.is_sensorless_running) {
-		return foc_obser.sensorless_angle;
-	}
+	return foc_obser.sensorless_angle;
+}
 
-	return foc_obser.enc_angle;
+bool foc_observer_diagnostic(float enc_angle, float enc_vel) {
+	if (enc_vel <= 50.0f) {
+		foc_obser.enc_angle = enc_angle;
+		foc_obser.enc_speed = ABS(enc_vel);
+		return true;
+	}
+	float delta_angle_est = RPM_2_degree((foc_obser.enc_speed + ABS(enc_vel))/2.0f);
+	float delta_angle_enc = enc_angle - foc_obser.enc_angle;
+	delta_angle_enc = ABS(delta_angle_enc);
+	if (delta_angle_enc > 180.0f) {
+		delta_angle_enc = 360.0f - delta_angle_enc;
+	}
+	float r = ABS(delta_angle_enc - delta_angle_est)/(delta_angle_est + 1e-20f);
+	float thro = 2.0f;
+	if (delta_angle_est < 0.5f) {
+		thro = 6.0f;
+	}else if (delta_angle_est < 2.0f) {
+		thro = 2.0f;
+	}else if (delta_angle_est < 4.0f) {
+		thro = 1.0f;
+	}else if (delta_angle_est < 6.0f) {
+		thro = 0.7f;
+	}else if (delta_angle_est < 10.0f) {
+		thro = 0.5f;
+	}else {
+		thro = 0.4f;
+	}
+	if (r >= thro) {
+		if (foc_obser.enc_err_cnt < 65535) {
+			foc_obser.enc_err_cnt ++;
+		}
+	}
+	foc_obser.enc_angle = enc_angle;
+	foc_obser.enc_speed = ABS(enc_vel);
+	return true;
 }
 
 float foc_observer_speed(void) {
@@ -69,6 +102,10 @@ float foc_observer_sensorless_speed(void) {
 	return foc_obser.sensorless_speed;
 }
 
+u16 foc_observer_enc_errcount(void) {
+	return foc_obser.enc_err_cnt;
+}
+
 float foc_observer_sensorless_working_speed(void) {
 #ifdef CONFIG_SMO_OBSERVER
 	return CONFIG_SMO_MIN_SPEED;

Applications/foc/core/foc_observer.h

@@ -13,10 +13,11 @@ typedef struct {
 	float sensorless_est_angle; //通过smo速度，估计当前角度
 	bool  is_sensorless_enable;
 	bool  is_sensorless_running;
-	float fusion_ceof;  //融合系数
+	u16   enc_err_cnt;  //融合系数
 }foc_observer_t;
 void foc_observer_init(void);
 float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta);
+bool  foc_observer_diagnostic(float enc_angle, float enc_vel);
 float foc_observer_speed(void);
 bool  foc_observer_is_encoder(void);
 void  foc_observer_use_sensorless(bool use_smo);
@@ -24,5 +25,7 @@ void  foc_observer_enable_sensorless(bool enable);
 float foc_observer_sensorless_angle(void);
 float foc_observer_sensorless_speed(void);
 float foc_observer_sensorless_working_speed(void);
+u16 foc_observer_enc_errcount(void);
+
 #endif /*_FOC_OBSERVER_H__*/
 

Applications/foc/core/ladrc_observer.c

@@ -9,9 +9,12 @@ static ladrc_observer observer;
 static __inline float ladrc_observer_band(float vel) {
 	float ration = vel / observer.vel_min;
 	float Wo = observer.Wo;
-	if (ration > 1) {
-		Wo = ration * Wo;
+	if (ration > 7.0f) { //限制最高带宽，实际调试
+		ration = 7.0f;
+	}else if (ration < 0.1f) {
+		ration = 0.1f;
 	}
+	Wo = ration * Wo;
 	observer.B1 = 2 * Wo;
 	observer.B2 = SQ(Wo);
 	return Wo;
@@ -20,16 +23,19 @@ static __inline float ladrc_observer_band(float vel) {
 void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off) {
 	observer.Wo = Wo;
 	observer.vel_min = vel_min;
-	observer.ts = FOC_CTRL_US;
-	observer.lpf_cutoff_freq = lpf_cut_off * 2 * M_PI;
+	observer.ts = CONFIG_SENSORLESS_TS;
+	observer.lpf_ceof = lpf_cut_off * observer.ts;
 	observer.ld = nv_get_motor_params()->ld;
 	observer.lq = nv_get_motor_params()->lq;
 	observer.r = nv_get_motor_params()->r;
 	observer.poles = nv_get_motor_params()->poles;
 	observer.max_eVel = CONFIG_MAX_MOT_RPM/30.0f * M_PI * nv_get_motor_params()->poles;
-	observer.max_z1 = 500;
-	observer.max_z2 = 500 / observer.ld;
+	observer.max_z1 = CONFIG_MAX_PHASE_CURR * 10.0f;
+	observer.max_z2 = CONFIG_MAX_ACTIVE_EMF / observer.ld;
 	observer.Vel_El = 0;
+	observer.Vel_El_filter = 0;
+	observer.angle_atan = 0;
+	observer.angle_out = 0;
 	observer.alpha.z1 = 0;
 	observer.alpha.z2 = 0;
 	observer.beta.z1 = 0;
@@ -37,24 +43,26 @@ void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off) {
 	observer.Ealpha = 0;
 	observer.Ebeta = 0;
 	observer.angle_idx = 0;
-	observer.angle_last = 0;
 	observer.angle_sum = 0;
+	for (int i = 0; i < ANGLE_BUF_NUM; i++) {
+		observer.angle_array[i] = 0;
+	}
 	ladrc_observer_band(0);
 }
 
 float ladrc_observer_update(float va, float vb, float ia, float ib) {
 	float induct = observer.Vel_El * (observer.ld - observer.lq) / observer.ld;
 	/* update Wc for current est vel */
-	float Wo = ladrc_observer_band(observer.Vel_El);
+	float Wo = ladrc_observer_band(observer.Vel_El_filter);
 	/* est alpha emf */
 	float F0 = -observer.r/observer.ld * ia;
 	float e = observer.alpha.z1 - ia;
-
+	
 	float alpha_z1 = observer.alpha.z1;
 	observer.alpha.z2 += (-e * observer.B2) * observer.ts;
-	observer.alpha.z2 = fclamp(observer.alpha.z2, -observer.max_z2, -observer.max_z2);
+	observer.alpha.z2 = fclamp(observer.alpha.z2, -observer.max_z2, observer.max_z2);
 	observer.alpha.z1 += (observer.alpha.z2 + F0 + va/observer.ld - e * observer.B1 - induct * observer.beta.z1) * observer.ts;
-	observer.alpha.z1 = fclamp(observer.alpha.z1, -observer.max_z1, -observer.max_z1);
+	observer.alpha.z1 = fclamp(observer.alpha.z1, -observer.max_z1, observer.max_z1);
 
 	observer.Ealpha = observer.alpha.z2 * (-observer.ld);
 
@@ -63,32 +71,27 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 	e = observer.beta.z1 - ib;
 
 	observer.beta.z2 += (-e * observer.B2) * observer.ts;
-	observer.beta.z2 = fclamp(observer.beta.z2, -observer.max_z2, -observer.max_z2);
+	observer.beta.z2 = fclamp(observer.beta.z2, -observer.max_z2, observer.max_z2);
 	observer.beta.z1 += (observer.beta.z2 + F0 + vb/observer.ld - e * observer.B1 + induct * alpha_z1) * observer.ts;
-	observer.beta.z1 = fclamp(observer.beta.z1, -observer.max_z1, -observer.max_z1);
+	observer.beta.z1 = fclamp(observer.beta.z1, -observer.max_z1, observer.max_z1);
 	
 	observer.Ebeta = observer.beta.z2 * (-observer.ld);
 
 	float angle = fast_atan_2(-observer.Ealpha, observer.Ebeta);
 	UTILS_NAN_ZERO(angle);
-	/* 补偿ladrc相位延时 */
-	float angle_comp = fast_atan_2(observer.Vel_El * Wo, SQ(Wo) - SQ(observer.Vel_El));
-	UTILS_NAN_ZERO(angle_comp);
-	/* 电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
-	angle += (angle_comp + observer.Vel_El * observer.ts);
 	angle_clamp(angle);
 
 	/* 速度计算 */
-	float delta_angle = angle - observer.angle_last;
+	float delta_angle = angle - observer.angle_atan;
 	float delta_angle_abs = ABS(delta_angle);
 	if (delta_angle_abs >= M_PI) {
 		delta_angle = 2 * M_PI - delta_angle_abs;
 	}
+	observer.angle_atan = angle;
 	observer.angle_sum += delta_angle;
 	observer.angle_sum -= observer.angle_array[observer.angle_idx];
-
-	observer.angle_array[observer.angle_idx++] = delta_angle;
-	if (observer.angle_idx == ANGLE_BUF_NUM) {
+	observer.angle_array[observer.angle_idx] = delta_angle;
+	if (++observer.angle_idx == ANGLE_BUF_NUM) {
 		observer.angle_idx = 0;
 	}
  	float vel = observer.angle_sum / (ANGLE_BUF_NUM * observer.ts);
@@ -97,19 +100,24 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 	}else if (vel < -observer.max_eVel) {
 		vel = -observer.max_eVel;
 	}
-	LowPass_Filter(observer.Vel_El, vel, (observer.lpf_cutoff_freq * observer.ts));
-	observer.angle_last = angle;
-
-	return angle;
-
+	LowPass_Filter(observer.Vel_El, vel, observer.lpf_ceof);
+	/* 补偿ladrc相位延时,LADRC等效截止频率为Wo/2pi的两个低通滤波器串联 */
+	angle = fast_atan_2(observer.Vel_El, Wo) * 2.0f;
+	/* 电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
+	observer.angle_out = observer.angle_atan + (angle + 0/*observer.Vel_El * observer.ts*/);
+	angle_clamp(observer.angle_out);
+
+	LowPass_Filter(observer.Vel_El_filter, observer.Vel_El, 0.01f); //需要再加一级低通滤波，给计算Wo和输出使用
+	
+	return pi_2_degree(observer.angle_out);
 }
 
 float ladrc_observer_angle(void) {
-	return observer.angle_last;
+	return pi_2_degree(observer.angle_out);
 }
 
 float ladrc_observer_vel(void) {
-	return (observer.Vel_El * (30.0f / M_PI) / observer.poles);
+	return (observer.Vel_El_filter * (30.0f / M_PI) / observer.poles);
 }
 
 ladrc_observer *ladrc_observer_get(void) {

Applications/foc/core/ladrc_observer.h

@@ -1,6 +1,8 @@
 #ifndef _LADRC_OBSERVER_H__
 #define _LADRC_OBSERVER_H__
 #include "os/os_types.h"
+
+
 typedef struct {
 	float z1,z2;
 }zStats;
@@ -16,8 +18,10 @@ typedef struct {
 	float ld, lq, r, poles;
 	float Ealpha, Ebeta;
 	float Vel_El;
-	float lpf_cutoff_freq; //低通滤波器截止频率 (截止频率 x 2 x pi)
-	float angle_last;
+	float Vel_El_filter;
+	float lpf_ceof; //低通滤波器系数(截止频率 x 2 x pi / Fs)
+	float angle_atan;
+	float angle_out;
 	float angle_array[ANGLE_BUF_NUM];
 	int   angle_idx;
 	float angle_sum;

Applications/foc/core/smo_observer.c

@@ -12,13 +12,13 @@ static void smo_arctan(void);
 static void smo_pll(void);
 #endif
 void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta) {
-	smo.ts = FOC_CTRL_US;
+	smo.ts = CONFIG_SENSORLESS_TS;
 	smo.bandwith = pll_bandwith;
 	smo.pll_kp = pll_bandwith * 2;
 	smo.pll_ki = SQ(pll_bandwith);
 	smo.pll_max_rad_pers = CONFIG_MAX_MOT_RPM/30.0f * M_PI * nv_get_motor_params()->poles;
 	smo.lpf_wc = lpf_wc;
-	smo.lpf_ceof = (lpf_wc*FOC_CTRL_US);
+	smo.lpf_ceof = (lpf_wc*smo.ts);
 	smo.Ksmo = Ksmo;
 	smo.Ksta = Ksta;
 	smo.motor_r = nv_get_motor_params()->r;
@@ -26,6 +26,7 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.motor_lq = nv_get_motor_params()->lq;
 	smo.motor_poles = nv_get_motor_params()->poles;
 	smo.dir_ccw = true;
+	PLL_Reset(&smo.pll, 0);
 	smo.pll.DT = smo.ts;
 	smo.pll.kp = pll_bandwith * 2;
 	smo.pll.ki = 0.25f * SQ(smo.pll.kp);

Applications/foc/foc_config.h

@@ -75,8 +75,8 @@
 #ifdef CONFIG_LADRC_OBSERVER
 	#define CONFIG_LADRC_OBSERVER_MIN_SPEED    1000 //RPM
 	#define CONFIG_LADRC_OBSERVER_MIN_eVEL 400
-	#define CONFIG_LADRC_OBSERVER_MIN_Wo   (300 * 2 * PI)
-	#define CONFIG_LADRC_OBSERVER_LPF_FREQ 800
+	#define CONFIG_LADRC_OBSERVER_MIN_Wo   (200 * 2 * PI)
+	#define CONFIG_LADRC_OBSERVER_LPF_FREQ (100 * 2 * PI)
 #endif
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/encoder.c

@@ -332,6 +332,7 @@ static void encoder_sync_pwm_abs(void) {
 	g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
 	g_encoder.abi_angle = g_encoder.pwm_angle;
 	g_encoder.b_index_found = true;
+	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
 }
 
 /*I 信号的中断处理，一圈一个中断*/

Applications/foc/motor/motor.c

@@ -932,6 +932,7 @@ void MC_Protect_IRQHandler(void){
 	mc_error.b_smo_running = !foc_observer_is_encoder();
 	mc_error.mos_temp = get_mos_temp();
 	mc_error.mot_temp = get_motor_temp();
+	mc_error.enc_error = foc_observer_enc_errcount();
 
 	mc_set_critical_error(FOC_CRIT_Phase_Err);
 	_mc_internal_init(CTRL_MODE_OPEN, false);

Applications/foc/motor/motor.h

@@ -55,12 +55,13 @@ typedef struct {
 	s16 vq_x10;
 	s16 id_ref_x10;
 	s16 iq_ref_x10;
-	u8    run_mode;
+	u8  run_mode;
 	s16 torque_ref_x10;
 	s16 rpm;
 	bool  b_smo_running;
 	s16    mos_temp;
 	s16    mot_temp;
+	u16    enc_error;
 }motor_err_t;
 
 motor_t * mc_params(void);