foc 中执行smo观测器处理

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -8,6 +8,7 @@
 #include "foc/motor/motor.h"
 #include "foc/core/svpwm.h"
 #include "foc/core/torque.h"
+#include "foc/core/smo_observer.h"
 #include "foc/samples.h"
 #include "foc/limit.h"
 #include "app/nv_storage.h"
@@ -232,13 +233,16 @@ void PMSM_FOC_CoreInit(void) {
 	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS));	
 	PMSM_FOC_Reset_PID();
 
+#ifdef CONFIG_SMO_OBSERVER
+	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
+#endif
 	gFoc_Ctrl.plot_type = Plot_None;
 }
 
 //#define PHASE_LFP_FIR
 //#define PHASE_LFP
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
-	AB_t vAB;
+	AB_t iAB;
 #ifdef PHASE_LFP	
 	float *iabc = gFoc_Ctrl.in.s_iABCComp;
 #elif defined PHASE_LFP_FIR
@@ -265,15 +269,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	//sample current
 	phase_current_get(gFoc_Ctrl.in.s_iABC);
 	get_phase_vols(gFoc_Ctrl.in.s_vABC);
-#if 0
-	gFoc_Ctrl.in.s_vABC[0] -= gFoc_Ctrl.in.s_vDC/2.0f;
-	gFoc_Ctrl.in.s_vABC[1] -= gFoc_Ctrl.in.s_vDC/2.0f;
-	gFoc_Ctrl.in.s_vABC[2] -= gFoc_Ctrl.in.s_vDC/2.0f;
 
-	Clark(gFoc_Ctrl.in.s_vABC[0], gFoc_Ctrl.in.s_vABC[1], gFoc_Ctrl.in.s_vABC[2], &vAB);
-	
-	Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealVdq);
-#endif
 #ifdef PHASE_LFP
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
@@ -287,10 +283,13 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	gFoc_Ctrl.in.s_iABCFilter[2] = Fir_Filter(&phase2, gFoc_Ctrl.in.s_iABC[2]);
 	gFoc_Ctrl.in.s_iABCFilter[0] = -(gFoc_Ctrl.in.s_iABCFilter[1] + gFoc_Ctrl.in.s_iABCFilter[2]);
 #endif
-	Clark(iabc[0], iabc[1], iabc[2], &vAB);
+	Clark(iabc[0], iabc[1], iabc[2], &iAB);
 
-	Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
+	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
 
+#ifdef CONFIG_SMO_OBSERVER
+	smo_observer_update(gFoc_Ctrl.out.s_OutVAB.a, gFoc_Ctrl.out.s_OutVAB.b, iAB.a, iAB.b);
+#endif
 }
 
 static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
@@ -345,7 +344,6 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 }
 
 void PMSM_FOC_Schedule(void) {
-	AB_t vAB;
 
 	gFoc_Ctrl.ctrl_count++;
 
@@ -371,9 +369,9 @@ void PMSM_FOC_Schedule(void) {
 
 	gFoc_Ctrl.out.f_vdqRation = Circle_Limitation(&gFoc_Ctrl.in.s_targetVdq, gFoc_Ctrl.in.s_vDC, gFoc_Ctrl.params.n_modulation, &gFoc_Ctrl.out.s_OutVdq);
 	
-	RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &vAB);
+	RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVAB);
 	
-	SVM_Duty_Fix(&vAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);
+	SVM_Duty_Fix(&gFoc_Ctrl.out.s_OutVAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);
 
 	phase_current_point(&gFoc_Ctrl.out);
 	

Applications/foc/core/PMSM_FOC_Core.h

@@ -145,7 +145,7 @@ typedef struct {
 	u16   n_Sample2;
 	u8    n_RunMode;
 	DQ_t  s_OutVdq;
-	DQ_t  s_preOutVdq;
+	AB_t  s_OutVAB;
 	DQ_t  s_RealIdq;
 	DQ_t  s_RealVdq;
 	DQ_t  s_FilterIdq;

Applications/foc/foc_config.h

@@ -56,5 +56,12 @@
 
 #define CONFIG_MAX_NEG_CURRENT 2.0F
 
+#define CONFIG_SMO_OBSERVER 1
+#ifdef CONFIG_SMO_OBSERVER
+	#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   80.0F
+#endif
 #endif /* _FOC_CONFIG_H__ */