更新滑膜观测器算法，使用pll锁相环

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

Applications/app/app.c

@@ -140,7 +140,7 @@ static u32 _app_report_task(void *p) {
 		//thro_torque_log();
 		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
 		encoder_log();
-		sample_log();
+		//sample_log();
 		///PMSM_FOC_LogDebug();
 		//eCtrl_debug_log();
 		//err_code_log();

Applications/foc/core/smo_observer.c

@@ -3,7 +3,7 @@
 #include "PMSM_FOC_Core.h"
 #include "smo_observer.h"
 
-//#define USE_ARCTAN 1
+#define USE_ARCTAN 0
 static smo_observer_t smo;
 static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta);
 #ifdef USE_ARCTAN
@@ -15,7 +15,7 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.ts = FOC_CTRL_US;
 	smo.bandwith = pll_bandwith;
 	smo.pll_kp = pll_bandwith * 2;
-	smo.pll_ki = SQ(smo.pll_kp);
+	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);
@@ -112,7 +112,7 @@ static void smo_arctan(void) {
 		smo.pll.out = smo.est_rad_pers;
 	}
 	smo.est_rad_pers_filted = smo.est_rad_pers;
-	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
+	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc);
 	angle_clamp(smo.est_angle_out);
 	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
 	if (smo.est_rpm > CONFIG_MAX_MOT_RPM) {
@@ -139,7 +139,6 @@ static void smo_pll(void) {
 	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
 	
 	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
-	
 	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) {
@@ -148,7 +147,6 @@ static void smo_pll(void) {
 		smo.est_rpm = 0;
 	}
 	smo.est_angle_out = smo.est_angle;
-	angle_clamp(smo.est_angle_out);
 }
 #endif
 

Applications/foc/motor/motor_param.h

@@ -83,8 +83,8 @@ float motor_get_lq_from_iq(s16 iq);
 
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
 #define MOTOR_R   0.008f
-#define MOTOR_Ld (0.000125f*0.5f)
-#define MOTOR_Lq (0.000091f*0.5f)
+#define MOTOR_Lq (0.000125f*0.5f)
+#define MOTOR_Ld (0.000091f*0.5f)
 #define MOTOR_POLES  5
 #define MOTOR_ENC_OFFSET 145.0F
 #define CONFIG_MAX_MOTOR_TORQUE 200.0F
@@ -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 0x13
+#define MOTOR_NR 0x14
 
 #define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200

Applications/math/fast_math.c

@@ -233,45 +233,3 @@ void fast_sincosf(float angle, float *sin, float *cos){
 }
 #endif
 
-void TD_run(TD_t *td, float in)
-{
-  float rtb_DiscreteTimeIntegrator;
-
-  /* Outputs for Atomic SubSystem: '<Root>/Subsystem' */
-  /* DiscreteIntegrator: '<S1>/Discrete-Time Integrator' */
-  rtb_DiscreteTimeIntegrator = td->Integrator;
-
-  /* Outport: '<Root>/target' incorporates:
-   *  DiscreteIntegrator: '<S1>/Discrete-Time Integrator1'
-   */
-  td->target = td->Integrator1;
-
-  /* Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator' incorporates:
-   *  DiscreteIntegrator: '<S1>/Discrete-Time Integrator1'
-   *  Gain: '<S1>/Gain'
-   *  Gain: '<S1>/Gain1'
-   *  Inport: '<Root>/in'
-   *  Inport: '<Root>/time'
-   *  Math: '<S1>/Math Function'
-   *  Product: '<S1>/Product'
-   *  Product: '<S1>/Product1'
-   *  Sum: '<S1>/Sum'
-   *  Sum: '<S1>/Sum1'
-   */
-  td->Integrator += ((td->Integrator1 - in) * (-(td->time * td->time)) + (-2.0) * td->time * td->Integrator) * td->DT;
-
-  /* Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator1' */
-  td->Integrator1 += td->DT * rtb_DiscreteTimeIntegrator;
-
-  /* End of Outputs for SubSystem: '<Root>/Subsystem' */
-
-  /* Outport: '<Root>/diff' */
-  td->diff = rtb_DiscreteTimeIntegrator;
-}
-
-void TD_Init(TD_t *td, float wc, float DT) {
-	memset(td, 0, sizeof(TD_t));
-	td->DT = DT;
-	td->time = wc *wc;
-}
-

Applications/math/fast_math.h

@@ -170,41 +170,5 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 /* 后向差分离散化 */
 #define do_lpf(value, sample, filter_constant)	((sample * filter_constant + value)/(1.0f + filter_constant))
 
-
-static float limitRPM(float vel_limit, float vel_estimate, float vel_gain, float torque) {
-    float Tmax = (vel_limit - vel_estimate) * vel_gain;
-    float Tmin = (-vel_limit - vel_estimate) * vel_gain;
-	if (torque < Tmin) {
-		return Tmin;
-	}
-	if (Tmax > torque) {
-		return torque;
-	}
-	return Tmax;
-}
-
-#define MAX_H 1.1F
-static float lp_compestion(float w, float wc) {
-	float comp = sqrtf(1.0f + SQ(w/wc));
-	if (comp > MAX_H) {
-		comp = MAX_H;
-	}else if (comp < -MAX_H) {
-		comp = -MAX_H;
-	}
-	return comp;
-}
-
-
-typedef struct {                         /* '<Root>/in' */
-  float time;                         /* '<Root>/time' */
-  float target;
-  float diff;
-  float Integrator;
-  float Integrator1;
-  float DT;
-}TD_t;
-
-void TD_run(TD_t *td, float in);
-void TD_Init(TD_t *td, float wc, float DT);
 #endif /* _Fast_Math_H__ */