扭矩限速Kp，Ki跟着电机，更新smo

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

Applications/app/app.c

@@ -5,6 +5,7 @@
 #include "libs/utils.h"
 #include "foc/motor/motor.h"
 #include "foc/motor/current.h"
+#include "foc/core/smo_observer.h"
 #include "foc/samples.h"
 #include "prot/can_foc_msg.h"
 #include "prot/can_message.h"
@@ -113,8 +114,8 @@ static u32 _app_plot_task(void * args) {
 	//can_report_plot_values(0x45);
 	//can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
 	//can_plot2(PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation, PMSM_FOC_GetSpeed());
-	//can_plot2(PMSM_FOC_Get()->in.s_motAngle, PMSM_FOC_Get()->in.s_smoAngle + 360.0f);
-	can_plot2(PMSM_FOC_Get()->in.s_iABC[1], PMSM_FOC_Get()->in.s_iABC[2]);
+	can_plot2(PMSM_FOC_GetSpeed(), smo_observer_est_rpm());
+	//can_plot2(PMSM_FOC_Get()->in.s_iABC[1], PMSM_FOC_Get()->in.s_iABC[2]);
 #if 0
 	if (!_mc_start) {
 		_mc_start = true;

Applications/app/nv_storage.c

@@ -71,8 +71,8 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
 	foc_params.pid_conf[PID_Q_id].kb = 0;
 
-	foc_params.pid_conf[PID_TRQ_id].kp = 0.13f;
-	foc_params.pid_conf[PID_TRQ_id].ki = 0.5f;
+	foc_params.pid_conf[PID_TRQ_id].kp = TRQ_PI_KP;
+	foc_params.pid_conf[PID_TRQ_id].ki = TRQ_PI_KI;
 	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;
 
 	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
@@ -285,7 +285,7 @@ void nv_storage_init(void) {
 	nv_read_foc_params();
 	nv_read_gear_configs();
 	sys_debug("encoder_off = %f\n", m_params.offset);
-	if (m_params.mot_nr != MOTOR_NR) {
+	if ((m_params.mot_nr != MOTOR_NR) || (foc_params.pid_conf[PID_TRQ_id].kp != TRQ_PI_KP) || (foc_params.pid_conf[PID_TRQ_id].ki != TRQ_PI_KI)) {
 		nv_default_motor_params();
 		nv_default_foc_params();
 		nv_save_foc_params();

Applications/bsp/board_mc100_v1.h

@@ -32,6 +32,8 @@
 
 #define CONFIG_96V_MODE_VOL (55.0F)
 
+//#define CONFIG_SMO_OBSERVER 1
+
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5
 #define SCHED_TIMER_IRQ TIMER5_IRQn

Applications/foc/core/PMSM_FOC_Core.c

@@ -387,6 +387,16 @@ void PMSM_FOC_Schedule(void) {
 				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 delta = gFoc_Ctrl.in.s_smoAngle - gFoc_Ctrl.in.s_motAngle;
+				if (delta > M_PI) {
+					delta -= 2 * M_PI;
+				}else if (delta < -M_PI) {
+					delta += 2 * M_PI;
+				}
+				plot_3data16(gFoc_Ctrl.in.s_motAngle, gFoc_Ctrl.in.s_smoAngle, delta);
+#endif
 			}
 		}
 	}

Applications/foc/core/PMSM_FOC_Core.h

@@ -30,6 +30,7 @@ typedef enum {
 	Plot_Q_flow,
 	Plot_D_Step,
 	Plot_Q_Step,
+	Plot_SMO_OBS,
 	Plot_t_Max,
 }Plot_t;
 

Applications/foc/core/smo_observer.c

@@ -21,6 +21,9 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.motor_ld = nv_get_motor_params()->ld;
 	smo.motor_lq = nv_get_motor_params()->lq;
 	smo.motor_poles = nv_get_motor_params()->poles;
+
+	smo.ldq_diff_dm = (smo.motor_ld-smo.motor_lq)/smo.motor_lq*smo.motor_ld;
+	smo.ld_inv = 1.0f / smo.motor_ld;
 }	
 
 float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta) {
@@ -30,11 +33,15 @@ float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta)
 	return pi_2_degree(smo.est_angle_out);
 }
 
+smo_observer_t *get_smo(void) {
+	return &smo;
+}
+
 static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
-	float est_ab = (smo.motor_ld-smo.motor_lq)/smo.motor_lq * smo.est_rad_pers;
+	float est_ab = smo.ldq_diff_dm * smo.est_rad_pers;//(smo.motor_ld-smo.motor_lq)/smo.motor_lq * smo.est_rad_pers;
 
 	/* est alpha back emf */
-	float calc_alpha = uAlpha/smo.motor_ld - smo.Ialpha_hat*(smo.motor_r/smo.motor_ld) - smo.est_eAlpha / smo.motor_ld - est_ab * smo.IBeta_hat;
+	float calc_alpha = (uAlpha - smo.Ialpha_hat*smo.motor_r - smo.est_eAlpha - est_ab * smo.IBeta_hat) * smo.ld_inv;
 	smo.Ialpha_hat += calc_alpha * smo.ts; //积分
 
 	float err_iAlpha = smo.Ialpha_hat - iAlpha;
@@ -43,7 +50,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 	smo.est_eAlpha_Filted = do_lpf(smo.est_eAlpha_Filted, smo.est_eAlpha, smo.lpf_ceof);
 
 	/* est beta back emf */
-	float calc_beta = uBeta/smo.motor_ld - smo.IBeta_hat*(smo.motor_r/smo.motor_ld) - smo.est_eBeta / smo.motor_ld + est_ab * smo.Ialpha_hat;
+	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta + est_ab * smo.Ialpha_hat) * smo.ld_inv;
 	smo.IBeta_hat += calc_beta * smo.ts; //积分
 	
 	float err_iBeta = smo.IBeta_hat - iBeta;
@@ -54,6 +61,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 }
 #define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
 
+#define ANGLE_COMP_RPM_CEOF (2.44E-4F)
 static void smo_pll(void) {
 	float eab_sqr = sqrtf(SQ(smo.est_eAlpha_Filted) + SQ(smo.est_eBeta_Filted) + (1e-10f));
 	float ealpha_in = -smo.est_eAlpha_Filted/eab_sqr;
@@ -71,8 +79,16 @@ static void smo_pll(void) {
 	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
 	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
 	angle_clamp(smo.est_angle);
+	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
+	if (smo.est_rpm > CONFIG_MAX_MOT_RPM) {
+		smo.est_rpm = CONFIG_MAX_MOT_RPM;
+	}else if (smo.est_rpm < 0) {
+		smo.est_rpm = 0;
+	}
 	/* 对低通进行角度补偿 */
-	smo.est_angle_out = smo.est_angle + fast_arctan2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
+	smo.est_angle_out = smo.est_angle;// + fast_arctan2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
+	/* 通过速度对角度再次补偿, ANGLE_COMP_RPM_CEOF 这个值通过校准获取 */
+	smo.est_angle_out += smo.est_rpm * ANGLE_COMP_RPM_CEOF;
 	angle_clamp(smo.est_angle_out);
 }
 
@@ -83,5 +99,5 @@ float smo_observer_est_angle(void) {
 
 //机械角度 rpm
 float smo_observer_est_rpm(void) {
-	return (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
+	return smo.est_rpm;
 }

Applications/foc/core/smo_observer.h

@@ -25,16 +25,21 @@ typedef struct {
 	float est_angle_out;
 	float est_rad_pers; //每秒度数
 	float est_rad_pers_filted;
+	float est_rpm;
 	float motor_r;
 	float motor_ld;
 	float motor_lq;
 	float motor_poles;
+
+	float ldq_diff_dm; //(lq-ld)/lq*ld
+	float ld_inv; //1/ld
 }smo_observer_t;
 
 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);
+smo_observer_t *get_smo(void);
 
 
 #endif /* _SMO_OBSERVER_H__ */

Applications/foc/foc_config.h

@@ -1,6 +1,7 @@
 #ifndef _FOC_CONFIG_H__
 #define _FOC_CONFIG_H__
 #include "math/fast_math.h"
+#include "bsp/bsp.h"
 
 #define CONFIG_DEFAULT_IDC_LIM 45
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200
@@ -56,12 +57,12 @@
 
 #define CONFIG_MAX_NEG_CURRENT 2.0F
 
-#define CONFIG_SMO_OBSERVER 1
 #ifdef CONFIG_SMO_OBSERVER
+	#define CONFIG_SMO_MIN_SPEED    1200 //RPM
 	#define CONFIG_SMO_PLL_BANDWITH 2000.0f
-	#define CONFIG_SMO_LFP_WC       100.0F
-	#define CONFIG_SMO_GAIN_K            4.0F
-	#define CONFIG_SMO_SIGMOID_MAX   40.0F
+	#define CONFIG_SMO_LFP_WC       50.0F
+	#define CONFIG_SMO_GAIN_K            1.0F
+	#define CONFIG_SMO_SIGMOID_MAX   90.0F
 #endif
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/motor_param.h

@@ -8,6 +8,10 @@
 #define MOTOR_Lq (0.000200f*0.5f)
 #define MOTOR_POLES  4
 #define MOTOR_ENC_OFFSET 0.0F
+
+#define TRQ_PI_KP 0.14F
+#define TRQ_PI_KI 0.15F
+
 #define MOTOR_NR 0x11
 #elif defined MOTOR_BLUESHARK_NEW2
 #define MOTOR_R   0.013f
@@ -15,6 +19,9 @@
 #define MOTOR_Lq (0.000320f*0.5f)
 #define MOTOR_POLES  4
 #define MOTOR_ENC_OFFSET 0.0F
+#define TRQ_PI_KP 0.14F
+#define TRQ_PI_KI 0.15F
+
 #define MOTOR_NR 0x12
 
 #elif defined MOTOR_BLUESHARK_ZD_100
@@ -23,6 +30,8 @@
 #define MOTOR_Lq (0.000091f*0.5f)
 #define MOTOR_POLES  5
 #define MOTOR_ENC_OFFSET 145.0F
+#define TRQ_PI_KP 0.13F
+#define TRQ_PI_KI 0.5F
 #define MOTOR_NR 0x13
 
 #elif defined MOTOR_BLUESHARK_OLD
@@ -31,6 +40,9 @@
 #define MOTOR_Lq (0.000205f*0.5f)
 #define MOTOR_POLES  5
 #define MOTOR_ENC_OFFSET 180.0F
+#define TRQ_PI_KP 0.14F
+#define TRQ_PI_KI 0.15F
+
 #define MOTOR_NR 0x14
 
 #elif defined MOTOR_3505
@@ -39,6 +51,9 @@
 #define MOTOR_Ld 0.000032f
 #define MOTOR_Lq 0.000032f
 #define MOTOR_POLES  10
+#define TRQ_PI_KP 0.14F
+#define TRQ_PI_KI 0.15F
+
 #define MOTOR_NR 0x15
 
 #endif