ladrc sensorless observer limit the max vel

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

Applications/foc/core/ladrc_observer.c

@@ -26,6 +26,7 @@ void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off) {
 	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.Vel_El = 0;
@@ -91,6 +92,11 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 		observer.angle_idx = 0;
 	}
  	float vel = observer.angle_sum / (ANGLE_BUF_NUM * observer.ts);
+	if (vel > observer.max_eVel) {
+		vel = observer.max_eVel;
+	}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;
 

Applications/foc/core/ladrc_observer.h

@@ -11,6 +11,7 @@ typedef struct {
 	float B1,B2;
 	float ts;
 	float Wo; //初始带宽
+	float max_eVel; //最大的电角速度
 	float vel_min; //初始带宽对应的角频率
 	float ld, lq, r, poles;
 	float Ealpha, Ebeta;