add ladrc sensorless observer

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

Applications/app/app.c

@@ -146,7 +146,7 @@ static u32 _app_report_task(void *p) {
 }
 int plot_type = 0;
 static void plot_smo_angle(void) {
-	float smo_angle = foc_observer_smo_angle();
+	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_hallAngle;
 	float s, c;
 	arm_sin_cos_f32(delta, &s, &c);

Applications/bsp/gd32/board_mc105_v3.h

@@ -31,9 +31,10 @@
 
 #define CONFIG_96V_MODE_VOL (60.0F)
 
-#define CONFIG_SMO_OBSERVER 1
-#define CONFIG_SPEED_LADRC  1
-//#define CONFIG_FORCE_96V_MODE 1
+//#define CONFIG_SMO_OBSERVER 
+#define CONFIG_LADRC_OBSERVER
+#define CONFIG_SPEED_LADRC  
+//#define CONFIG_FORCE_96V_MODE
 
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5

Applications/foc/commands.c

@@ -534,12 +534,12 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Use_SensorLess_Angle:
 		{
 			bool sensorless = decode_u8((u8 *)command->data)?true:false;
-			if (sensorless && mc_is_start() && PMSM_FOC_GetSpeed() >= CONFIG_SMO_MIN_SPEED) {
+			if (sensorless && mc_is_start() && PMSM_FOC_GetSpeed() >= foc_observer_sensorless_working_speed()) {
 				sys_debug("use smo %d\n", sensorless);
-				foc_observer_use_smo(sensorless);
+				foc_observer_use_sensorless(sensorless);
 			}else {
 				sys_debug("unuse smo\n");
-				foc_observer_use_smo(false);
+				foc_observer_use_sensorless(false);
 			}
 			break;
 		}

Applications/foc/core/PMSM_FOC_Core.c

@@ -256,7 +256,7 @@ static __INLINE void PMSM_FOC_Update_Input(void) {
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
 		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
 	}else {
-		gFoc_Ctrl.in.s_hallAngle = foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a, gFoc_Ctrl.out.s_OutVAB.b, iAB.a, iAB.b);
+		gFoc_Ctrl.in.s_hallAngle = foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a, gFoc_Ctrl.out.s_OutVAB.b, iAB.a * 0.66667f, iAB.b * 0.66667f);
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
 	}
 	gFoc_Ctrl.in.s_motRPM = foc_observer_speed();
@@ -398,7 +398,7 @@ void PMSM_FOC_Schedule(void) {
 				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_smo_angle();
+				float smo_angle = foc_observer_sensorless_angle();
 				float delta = smo_angle - gFoc_Ctrl.in.s_hallAngle;
 				if (delta > 180) {
 					delta -= 360;

Applications/foc/core/foc_observer.c

@@ -1,22 +1,27 @@
 #include "bsp/bsp_driver.h"
 #include "foc/core/foc_observer.h"
+#include "foc/core/ladrc_observer.h"
 #include "foc/core/smo_observer.h"
 #include "foc/motor/motor.h"
 
 static foc_observer_t foc_obser;
 void foc_observer_init(void) {
-	foc_obser.smo_enabled = false;
-	foc_obser.smo_used = false;
+	foc_obser.is_sensorless_enable = false;
+	foc_obser.is_sensorless_running = false;
 	foc_obser.enc_angle = INVALID_ANGLE;
 	foc_obser.enc_est_angle = INVALID_ANGLE;
 	foc_obser.enc_speed = 0;
-	foc_obser.smo_angle = INVALID_ANGLE;
-	foc_obser.smo_est_angle = INVALID_ANGLE;
-	foc_obser.smo_speed = 0;
+	foc_obser.sensorless_angle = INVALID_ANGLE;
+	foc_obser.sensorless_est_angle = INVALID_ANGLE;
+	foc_obser.sensorless_speed = 0;
 	foc_obser.fusion_ceof = 1.0f;
 #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.smo_enabled = true;
+	foc_obser.is_sensorless_enable = true;
+#endif
+#ifdef CONFIG_LADRC_OBSERVER
+	ladrc_observer_init(CONFIG_LADRC_OBSERVER_MIN_Wo, CONFIG_LADRC_OBSERVER_MIN_eVEL, CONFIG_LADRC_OBSERVER_LPF_FREQ);
+	foc_obser.is_sensorless_enable = true;
 #endif
 }
 
@@ -29,7 +34,7 @@ float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 	foc_obser.enc_angle = motor_encoder_get_angle();
 	foc_obser.enc_speed = motor_encoder_get_speed();
 
-	if (!foc_obser.smo_enabled) {
+	if (!foc_obser.is_sensorless_enable) {
 		return foc_obser.enc_angle;
 	}
 	float est_enc_delta = RPM_2_degree(prev_enc_speed);
@@ -56,12 +61,16 @@ float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 		}
 		foc_obser.enc_est_angle = foc_obser.enc_angle;
 	}
-	foc_obser.smo_est_angle = foc_obser.smo_angle + RPM_2_degree(foc_obser.smo_speed);
-	foc_obser.smo_angle = smo_observer_update(uAlp, uBeta, iAlp, iBeta);
-	foc_obser.smo_speed = smo_observer_est_rpm();
-
-	if (foc_obser.smo_used) {
-		return foc_obser.smo_angle;
+	foc_obser.sensorless_est_angle = foc_obser.sensorless_angle + RPM_2_degree(foc_obser.sensorless_speed);
+#ifdef CONFIG_SMO_OBSERVER
+	foc_obser.sensorless_angle = smo_observer_update(uAlp, uBeta, iAlp, iBeta);
+	foc_obser.sensorless_speed = smo_observer_vel();
+#elif defined CONFIG_LADRC_OBSERVER
+	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;
 	}
 #if 0
 	return (foc_obser.enc_est_angle * foc_obser.fusion_ceof + foc_obser.smo_angle * (1.0f - foc_obser.fusion_ceof));
@@ -71,8 +80,8 @@ float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 }
 
 float foc_observer_speed(void) {
-	if (foc_obser.smo_used) {
-		return foc_obser.smo_speed;
+	if (foc_obser.is_sensorless_running) {
+		return foc_obser.sensorless_speed;
 	}
 	return foc_obser.enc_speed;
 }
@@ -89,24 +98,34 @@ float foc_observer_speed(void) {
 
 #endif
 bool  foc_observer_is_encoder(void) {
-	return !foc_obser.smo_used;
+	return !foc_obser.is_sensorless_running;
 }
-void  foc_observer_use_smo(bool use_smo) {
-	if (foc_obser.smo_enabled) {
-		foc_obser.smo_used = use_smo;
+void  foc_observer_use_sensorless(bool use_smo) {
+	if (foc_obser.is_sensorless_enable) {
+		foc_obser.is_sensorless_running = use_smo;
 	}else {
-		foc_obser.smo_used = false;
+		foc_obser.is_sensorless_running = false;
 	}
 }
-void  foc_observer_enable_smo(bool enable) {
-	foc_obser.smo_enabled = enable;
+void  foc_observer_enable_sensorless(bool enable) {
+	foc_obser.is_sensorless_enable = enable;
 }
 
-float foc_observer_smo_angle(void) {
-	return foc_obser.smo_angle;
+float foc_observer_sensorless_angle(void) {
+	return foc_obser.sensorless_angle;
 }
 
-float foc_observer_smo_speed(void) {
-	return foc_obser.smo_speed;
+float foc_observer_sensorless_speed(void) {
+	return foc_obser.sensorless_speed;
+}
+
+float foc_observer_sensorless_working_speed(void) {
+#ifdef CONFIG_SMO_OBSERVER
+	return CONFIG_SMO_MIN_SPEED;
+#elif defined CONFIG_LADRC_OBSERVER
+	return CONFIG_LADRC_OBSERVER_MIN_SPEED;
+#else
+	return 20000.0f;
+#endif
 }
 

Applications/foc/core/foc_observer.h

@@ -8,20 +8,21 @@ typedef struct {
 	float enc_angle;
 	float enc_speed;
 	float enc_est_angle; //通过encoder速度，估计当前角度
-	float smo_angle;
-	float smo_speed;
-	float smo_est_angle; //通过smo速度，估计当前角度
-	bool  smo_enabled;
-	bool  smo_used;
+	float sensorless_angle;
+	float sensorless_speed;
+	float sensorless_est_angle; //通过smo速度，估计当前角度
+	bool  is_sensorless_enable;
+	bool  is_sensorless_running;
 	float fusion_ceof;  //融合系数
 }foc_observer_t;
 void foc_observer_init(void);
 float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta);
 float foc_observer_speed(void);
 bool  foc_observer_is_encoder(void);
-void  foc_observer_use_smo(bool use_smo);
-void  foc_observer_enable_smo(bool enable);
-float foc_observer_smo_angle(void);
-float foc_observer_smo_speed(void);
+void  foc_observer_use_sensorless(bool use_smo);
+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);
 #endif /*_FOC_OBSERVER_H__*/
 

Applications/foc/core/ladrc_observer.c

@@ -0,0 +1,79 @@
+#include "ladrc_observer.h"
+#include "app/nv_storage.h"
+#include "math/fast_math.h"
+
+static ladrc_observer observer;
+#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
+
+static __inline float ladrc_observer_band(float vel) {
+	float ration = vel / observer.vel_min;
+	float Wo = observer.Wo;
+	if (ration > 1) {
+		Wo = ration * Wo;
+	}
+	observer.B1 = 2 * Wo;
+	observer.B2 = SQ(Wo);
+	return Wo;
+}
+
+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.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;
+	ladrc_observer_band(0);
+}
+
+void 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);
+	/* est alpha emf */
+	float F0 = -observer.r/observer.ld * ia;
+	float e = observer.alpha.z1 - ia;
+
+	observer.alpha.z2 += (-e * observer.B2);
+	observer.alpha.z1 += (observer.alpha.z2 + F0 + va/observer.ld + e * observer.B1 * observer.Vel_El - induct * observer.beta.z1);
+	observer.Ealpha = observer.alpha.z2 * (-observer.ld);
+
+	/* est beta emf */
+	F0 = -observer.r/observer.ld * ib;
+	e = observer.beta.z1 - ib;
+
+	observer.beta.z2 += (-e * observer.B2);
+	observer.beta.z1 += (observer.beta.z2 + F0 + va/observer.ld - e * observer.B1 * observer.Vel_El + induct * observer.alpha.z1);
+	observer.Ebeta = observer.beta.z2 * (-observer.ld);
+
+	float angle = fast_atan_2(-observer.Ealpha, observer.Ebeta);
+	/* 补偿ladrc相位延时， 同时电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
+	float angle_comp = fast_atan_2(observer.Vel_El * Wo, SQ(Wo) - SQ(observer.Vel_El)) + observer.Vel_El * observer.ts;
+	angle += angle_comp;
+	angle_clamp(angle);
+	float delta_angle = angle - observer.angle_last;
+	float delta_angle_abs = ABS(delta_angle);
+	if (delta_angle_abs >= M_PI) {
+		delta_angle = 2 * M_PI - delta_angle_abs;
+	}
+	observer.angle_sum += 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_idx = 0;
+	}
+ 	float vel = observer.angle_sum / (ANGLE_BUF_NUM * observer.ts);
+	LowPass_Filter(observer.Vel_El, vel, (observer.lpf_cutoff_freq * observer.ts));
+	observer.angle_last = angle;
+}
+
+float ladrc_observer_angle(void) {
+	return observer.angle_last;
+}
+
+float ladrc_observer_vel(void) {
+	return (observer.Vel_El * (30.0f / M_PI) / observer.poles);
+}

Applications/foc/core/ladrc_observer.h

@@ -0,0 +1,30 @@
+#ifndef _LADRC_OBSERVER_H__
+#define _LADRC_OBSERVER_H__
+#include "os/os_types.h"
+typedef struct {
+	float z1,z2;
+}zStats;
+#define ANGLE_BUF_NUM 16
+typedef struct {
+	//扩张状态观测器
+	zStats alpha, beta;
+	float B1,B2;
+	float ts;
+	float Wo; //初始带宽
+	float vel_min; //初始带宽对应的角频率
+	float ld, lq, r, poles;
+	float Ealpha, Ebeta;
+	float Vel_El;
+	float lpf_cutoff_freq; //低通滤波器截止频率 (截止频率 x 2 x pi)
+	float angle_last;
+	float angle_array[ANGLE_BUF_NUM];
+	int   angle_idx;
+	float angle_sum;
+}ladrc_observer;
+
+void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off);
+void ladrc_observer_update(float va, float vb, float ia, float ib);
+float ladrc_observer_angle(void);
+float ladrc_observer_vel(void);
+#endif /* _LADRC_OBSERVER_H__ */
+

Applications/foc/core/smo_observer.c

@@ -151,12 +151,12 @@ static void smo_pll(void) {
 }
 #endif
 
-float smo_observer_est_angle(void) {
+float smo_observer_angle(void) {
 	return pi_2_degree(smo.est_angle_out);
 }
 
 //机械角度 rpm
-float smo_observer_est_rpm(void) {
+float smo_observer_vel(void) {
 	return smo.est_rpm;
 }
 

Applications/foc/core/smo_observer.h

@@ -38,8 +38,8 @@ typedef struct {
 
 void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta);
 float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta);
-float smo_observer_est_angle(void);
-float smo_observer_est_rpm(void);
+float smo_observer_angle(void);
+float smo_observer_vel(void);
 smo_observer_t *get_smo(void);
 
 

Applications/foc/foc_config.h

@@ -72,5 +72,11 @@
 	#define CONFIG_SMO_GAIN_K        100.0f
 	#define CONFIG_SMO_SIGMOID_MAX   120.0F
 #endif
+#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
+#endif
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/motor.c

@@ -1011,7 +1011,7 @@ static bool mc_can_stop_foc(void) {
 		}
 	}
 	/* 启用无感观测器，同时速度低于观测器允许的最小速度，关闭输出，滑行 */
-	if (!foc_observer_is_encoder() && PMSM_FOC_GetSpeed() < CONFIG_SMO_MIN_SPEED) {
+	if (!foc_observer_is_encoder() && PMSM_FOC_GetSpeed() < foc_observer_sensorless_working_speed()) {
 		return true;
 	}
 	return false;