foc核心代码手写替代自动生成

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

Applications/app/app.c

@@ -5,7 +5,6 @@
 #include "libs/utils.h"
 #include "foc/motor/motor.h"
 #include "foc/samples.h"
-#include "foc/core/foc_core.h"
 #include "prot/can_foc_msg.h"
 #include "prot/can_message.h"
 #include "libs/time_measure.h"

Applications/bsp/bsp.h

@@ -54,7 +54,6 @@
 #define SPEED_SAMPLE_INVAL (100) //ת�Ѳɼ��ļ��,ms
 #define SPEED_RAMP_DURATION SPEED_SAMPLE_INVAL //�����ٶȵ�б��ʱ�䣬�ٶ�ƽ���������½�
 
-#define MAX_VBUS_VOLTAGE (54) 
 #define ADC_REFERENCE_VOLTAGE  (3.3F)
 
 #define pwm_timer TIMER0

Applications/bsp/mc_hall_gpio.c

@@ -55,10 +55,7 @@ u32 hall_get_hwcount(u8 *hall) {
 
 
 void hall_sensor_handler(void){
-	os_disable_irq();
-	g_hall.is_edged = 1;
-	g_hall.hall_time = timer_count32_get();
-	os_enable_irq();
+
 }
 
 

Applications/bsp/mc_irqs.c

@@ -81,6 +81,9 @@ __weak void ADC_IRQHandler(void) {
 
 }
 
+__weak void HALL_IRQHandler(void) {
+
+}
 
 void ADC0_1_IRQHandler(void)
 {
@@ -156,12 +159,12 @@ void EXTI5_9_IRQHandler(void){
 	}	
 	if(RESET != exti_interrupt_flag_get(EXTI_7)){
 		exti_interrupt_flag_clear(EXTI_7);
-		hall_sensor_handler();
+		HALL_IRQHandler();
 	}
 	if(RESET != exti_interrupt_flag_get(EXTI_8)){
 		exti_interrupt_flag_clear(EXTI_8);
 #ifndef GD32_FOC_DEMO		
-		hall_sensor_handler();
+		HALL_IRQHandler();
 #endif
 	}
 	if(RESET != exti_interrupt_flag_get(EXTI_9)){

Applications/bsp/timer_count32.c

@@ -86,3 +86,9 @@ u32 timer_count32_get(void) {
 	return _timer_count32_get();
 }
 
+u32 timer_count32_delta(u32 now, u32 prev) {
+	if (now >= prev) {
+		return (now - prev);
+	}
+	return (0xFFFFFFFF + prev - now + 1);
+}

Applications/bsp/timer_count32.h

@@ -5,6 +5,7 @@
 
 void timer_count32_init(void);
 u32  timer_count32_get(void);
+u32 timer_count32_delta(u32 now, u32 prev);
 
 #define COUNT_CLK (1000 * 1000)
 

Applications/foc/commands.c

@@ -6,8 +6,6 @@
 #include "bsp/bsp.h"
 #include "bsp/pwm.h"
 #include "bsp/adc.h"
-
-#include "foc/core/foc_core.h"
 #include "foc/motor/motor.h"
 #include "foc/commands.h"
 #include "prot/can_foc_msg.h"
@@ -39,15 +37,7 @@ static u32 foc_command_task(void *args) {
 }
 
 static void conf_foc_pid(pid_conf_t *pid) {
-	PMSM_FOC_SetIDPid((float)pid->kp/pid->gain, (float)pid->ki/pid->gain, (float)pid->kb/pid->gain);
-	pid += 1;
-	PMSM_FOC_SetIQPid((float)pid->kp/pid->gain, (float)pid->ki/pid->gain, (float)pid->kb/pid->gain);
-	pid += 1;
-	PMSM_FOC_SetSpdPid((float)pid->kp/pid->gain, (float)pid->ki/pid->gain, (float)pid->kb/pid->gain);
-	pid += 1;
-	PMSM_FOC_SetTrqPid((float)pid->kp/pid->gain, (float)pid->ki/pid->gain, (float)pid->kb/pid->gain);
-	pid += 1;
-	PMSM_FOC_SetFW_I((float)pid->ki/pid->gain, (float)pid->kb/pid->gain);	
+	
 }
 
 static void process_foc_command(foc_cmd_body_t *command) {
@@ -89,11 +79,6 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			PMSM_FOC_iBusLimit(current);
 		}		
 		case Foc_Cali_Hall_Phase:
-		{
-			PMSM_FOC_HallCalibrate(true, decode_s32((u8 *)command->data));
-			break;
-		}
-		case Foc_Cali_Hall_Offset:
 		{
 			mc_hall_calibrate(decode_s16((u8 *)command->data));
 			break;

Applications/foc/core/PMSM_FOC_Core.c

@@ -0,0 +1,316 @@
+#include "PMSM_FOC_Core.h"
+#include "PMSM_FOC_Params.h"
+#include "foc/core/e_ctrl.h"
+#include "math/fix_math.h"
+#include "math/fast_math.h"
+#include "foc/motor/current.h"
+#include "foc/motor/hall.h"
+#include "foc/core/svpwm.h"
+#include "bsp/pwm.h"
+
+static PMSM_FOC_Ctrl _gFOC_Ctrl;
+
+static void PMSM_FOC_idq_Assign(void);
+
+static __INLINE void RevPark(DQ_t *dq, s16q5_t angle, AB_t *alpha_beta) {
+	s16q14_t c,s;
+	SinCos_Lut(angle, &s, &c);
+
+	alpha_beta->a = S16_mul(dq->d, c, 14)  - S16_mul(dq->q, s, 14);
+	alpha_beta->b = S16_mul(dq->d, s, 14) + S16_mul(dq->q, c, 14);
+}
+
+static __INLINE void Clark(s16q5_t A, s16q5_t B, s16q5_t C, AB_t *alpha_beta){
+	alpha_beta->a = (2 * A - B - C) / 3;
+	alpha_beta->b = S16_mul(ONE_BY_SQRT3_Q14, (B - C), 14);
+}
+
+static __INLINE void Park(AB_t *alpha_beta, s16q5_t angle, DQ_t *dq) {
+	s16q14_t c,s;
+	SinCos_Lut(angle, &s, &c);
+
+	dq->d = S16_mul(alpha_beta->a, c, 14) + S16_mul(alpha_beta->b, s, 14);
+	dq->q = S16_mul(-alpha_beta->a, s, 14) + S16_mul(alpha_beta->b, c, 14);
+}
+
+static __INLINE s16q5_t Circle_Limitation(DQ_t *vdq, s16q5_t vDC, s16q14_t module, DQ_t *out) {
+	u32 sq_vdq = vdq->d * vdq->d + vdq->q * vdq->q;
+	s16q5_t vDC_m = S16_mul(vDC, module, 14);
+	u32 sq_vDC = vDC_m * vDC_m;
+	if (sq_vdq > sq_vDC) {
+		s16q5_t r = Sqrt_Fix((sq_vDC << 10) / (sq_vdq));
+		out->d = S16_mul(vdq->d, r, 5);
+		out->q = S16_mul(vdq->q, r, 5);
+		return r;
+	}
+	out->d = vdq->d;
+	out->q = vdq->q;
+	return 32; // s16q5 32 means int 1
+}
+
+static void PMSM_FOC_Reset_PID(void) {
+	PI_Controller_Reset(_gFOC_Ctrl.id_ctl, 0);
+	PI_Controller_Reset(_gFOC_Ctrl.iq_ctl, 0);
+	PI_Controller_Reset(_gFOC_Ctrl.spd_ctl, 0);
+	PI_Controller_Reset(_gFOC_Ctrl.fw_ctl, 0);
+}
+
+void PMSM_FOC_CoreInit(void) {
+	_gFOC_Ctrl.id_ctl = &PI_Ctrl_ID;
+	_gFOC_Ctrl.iq_ctl = &PI_Ctrl_IQ;
+	_gFOC_Ctrl.spd_ctl = &PI_Ctrl_Spd;
+	_gFOC_Ctrl.fw_ctl = &PI_Ctrl_fw;
+	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
+	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
+	_gFOC_Ctrl.in.n_poles = 4;
+	_gFOC_Ctrl.in.s_maxiDC = S16Q5(MAX_iDQ);
+	_gFOC_Ctrl.in.s_maxiDC = S16Q5(MAX_iDC);
+	_gFOC_Ctrl.in.s_maxRPM = S32Q14(MAX_SPEED);
+	_gFOC_Ctrl.in.s_vDC    = S16Q5(MAX_vDC);
+	_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
+	_gFOC_Ctrl.out.f_vdqRation = S16Q14(0.9f);
+	_gFOC_Ctrl.in.s_manualAngle = 0xFFFF;
+	
+	PMSM_FOC_Reset_PID();
+}
+
+static __INLINE void PMSM_FOC_Update_Encoder(void) {
+	if (_gFOC_Ctrl.in.s_manualAngle != 0xFFF0) {
+		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
+	}else {
+		_gFOC_Ctrl.in.s_motAngle = S16Q5(hall_sensor_get_theta());
+	}
+	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
+}
+
+
+void PMSM_FOC_Schedule(void) {
+	AB_t vAB;
+	s16q5_t *iabc = _gFOC_Ctrl.in.s_iABC;
+
+	PMSM_FOC_Update_Encoder();
+	//sample current
+	phase_current_get(iabc);
+	
+	if (_gFOC_Ctrl.out.n_RunMode != OPEN_MODE) {
+		Clark(iabc[0], iabc[1], iabc[2], &vAB);
+		Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealIdq);
+		s32q14_t err = _gFOC_Ctrl.in.s_targetIdq.d - _gFOC_Ctrl.out.s_RealIdq.d;
+		err = err << 9; //q5 to q14
+		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_run(_gFOC_Ctrl.id_ctl, err);
+		err = _gFOC_Ctrl.in.s_targetIdq.q - _gFOC_Ctrl.out.s_RealIdq.q;
+		err = err << 9; //q5 to q14
+		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_run(_gFOC_Ctrl.iq_ctl, err);
+	}
+	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, S16Q14(0.95f), &_gFOC_Ctrl.out.s_OutVdq);
+
+	RevPark(&_gFOC_Ctrl.out.s_OutVdq, _gFOC_Ctrl.in.s_motAngle, &vAB);
+
+	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);
+
+	u32 sp = phase_current_point(&_gFOC_Ctrl.out.n_Sector , _gFOC_Ctrl.out.n_Duty, _gFOC_Ctrl.out.n_lowDuty, _gFOC_Ctrl.out.n_midDuty);
+	pwm_update_sample(sp&0xFFFF, (sp>>16)&0xFFFF, _gFOC_Ctrl.out.n_Sector);
+}
+
+/*called in media task */
+void PMSM_FOC_CtrlMode(void) {
+	if (!_gFOC_Ctrl.in.b_motEnable) {
+		_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
+	}else if (!_gFOC_Ctrl.in.b_motEnable || _gFOC_Ctrl.in.n_ctlMode == OPEN_MODE) {
+		_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
+	}else if (_gFOC_Ctrl.in.n_ctlMode == SPD_MODE || _gFOC_Ctrl.in.b_cruiseEna){
+		_gFOC_Ctrl.out.n_RunMode = SPD_MODE;
+	}else {
+		_gFOC_Ctrl.out.n_RunMode = TRQ_MODE;
+	}
+}
+
+/*called in media task */
+void PMSM_FOC_idqCalc(void) {
+	if (!_gFOC_Ctrl.in.b_motEnable) {
+		return;
+	}
+	if (_gFOC_Ctrl.out.n_RunMode == OPEN_MODE) {
+		return;
+	}
+	if (_gFOC_Ctrl.out.n_RunMode == TRQ_MODE) {
+		_gFOC_Ctrl.in.s_targetTrq = eCtrl_get_RefTorque();
+	}else {
+		s32q14_t errRef = eCtrl_get_RefSpd() - _gFOC_Ctrl.in.s_motRPM;
+		_gFOC_Ctrl.in.s_targetTrq = PI_Controller_run(_gFOC_Ctrl.spd_ctl, errRef);
+		
+	}
+	PMSM_FOC_idq_Assign();
+}
+
+void PMSM_FOC_Start(u8 nCtrlMode) {
+	if (_gFOC_Ctrl.in.b_motEnable) {
+		return;
+	}
+	PMSM_FOC_CoreInit();
+	_gFOC_Ctrl.in.b_motEnable = true;
+}
+
+void PMSM_FOC_Stop(void) {
+	if (!_gFOC_Ctrl.in.b_motEnable) {
+		return;
+	}
+	_gFOC_Ctrl.in.b_motEnable = false;
+}
+
+bool PMSM_FOC_Is_Start(void) {
+	return _gFOC_Ctrl.in.b_motEnable;
+}
+
+void PMSM_FOC_iBusLimit(s16q5_t ibusLimit) {
+	_gFOC_Ctrl.in.s_maxiDC = (ibusLimit);
+}
+
+void PMSM_FOC_SpeedLimit(s32q4_t speedLimit) {
+	_gFOC_Ctrl.in.s_maxRPM = (speedLimit);
+}
+
+s32q4_t PMSM_FOC_GetSpeedLimit(void) {
+	return _gFOC_Ctrl.in.s_maxRPM;
+}
+
+void PMSM_FOC_VbusVoltage(s16q5_t vbusVol) {
+	_gFOC_Ctrl.in.s_vDC = vbusVol;
+}
+
+void PMSM_FOC_SetCtrlMode(u8 mode) {
+	_gFOC_Ctrl.in.n_ctlMode = mode;
+}
+
+void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
+	_gFOC_Ctrl.in.s_targetVdq.d = vd;
+	_gFOC_Ctrl.in.s_targetVdq.q = vq;
+}
+
+bool PMSM_FOC_EnableCruise(bool enable) {
+	if (enable != _gFOC_Ctrl.in.b_cruiseEna) {
+		s32q4_t motSpd = PMSM_FOC_GetSpeed();
+		if (motSpd < MIN_CRUISE_RPM) { //
+			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+			return false;
+		}
+		eCtrl_set_TargetSpeed(motSpd);
+		_gFOC_Ctrl.in.b_cruiseEna = enable;
+	}
+	
+	return true;
+}
+
+bool PMSM_FOC_Is_CruiseEnabled(void) {
+	return (_gFOC_Ctrl.in.b_cruiseEna && (_gFOC_Ctrl.out.n_RunMode == SPD_MODE));
+}
+
+bool PMSM_FOC_Set_Speed(s32q4_t rpm) {
+	if (_gFOC_Ctrl.in.b_cruiseEna) {
+		return false;
+	}
+	eCtrl_set_TargetSpeed(rpm);
+	return true;
+}
+
+bool PMSM_FOC_Set_Current(float current) {
+	eCtrl_set_TrqCurrent(current);
+	return true;
+}
+
+bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
+	if (PMSM_FOC_Is_CruiseEnabled()) {
+		eCtrl_set_TargetSpeed(rpm);
+		return true;
+	}
+	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
+	return false;
+}
+
+void PMSM_FOC_Set_Angle(s16 angle) {
+	_gFOC_Ctrl.in.s_manualAngle = S16Q14(angle);
+}
+
+s32q4_t PMSM_FOC_GetSpeed(void) {
+	return _gFOC_Ctrl.in.s_motRPM;
+}
+
+
+void PMSM_FOC_LockMotor(bool lock) {
+	_gFOC_Ctrl.in.b_motLock = lock;
+}
+
+void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min) {
+	_gFOC_Ctrl.spd_ctl->kp = S32Q14(kp);
+	_gFOC_Ctrl.spd_ctl->ki = S32Q14(ki);
+	_gFOC_Ctrl.spd_ctl->max = S32Q14(max);
+	_gFOC_Ctrl.spd_ctl->min = S32Q14(min);
+}
+
+void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min) {
+	_gFOC_Ctrl.id_ctl->kp = S32Q14(kp);
+	_gFOC_Ctrl.id_ctl->ki = S32Q14(ki);
+	_gFOC_Ctrl.id_ctl->max = S32Q14(max);
+	_gFOC_Ctrl.id_ctl->min = S32Q14(min);
+}
+
+void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min) {
+	_gFOC_Ctrl.iq_ctl->kp = S32Q14(kp);
+	_gFOC_Ctrl.iq_ctl->ki = S32Q14(ki);
+	_gFOC_Ctrl.iq_ctl->max = S32Q14(max);
+	_gFOC_Ctrl.iq_ctl->min = S32Q14(min);
+}
+
+void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min) {
+
+}
+
+void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min) {
+	_gFOC_Ctrl.fw_ctl->kp = S32Q14(kp);
+	_gFOC_Ctrl.fw_ctl->ki = S32Q14(ki);
+	_gFOC_Ctrl.fw_ctl->max = S32Q14(max);
+	_gFOC_Ctrl.fw_ctl->min = S32Q14(min);
+}
+
+void PMSM_FOC_SetErrCode(u8 error) {
+	if (_gFOC_Ctrl.out.n_Error != error) {
+		_gFOC_Ctrl.out.n_Error = error;
+	}
+}
+
+u8 PMSM_FOC_GetErrCode(void) {
+	return _gFOC_Ctrl.out.n_Error;
+}
+
+//获取母线电流
+s16q5_t PMSM_FOC_Get_iDC(void) {
+	s32 vd = _gFOC_Ctrl.out.s_OutVdq.d;
+	s32 vq = _gFOC_Ctrl.out.s_OutVdq.q;
+	s32 id = _gFOC_Ctrl.out.s_RealIdq.d;
+	s32 iq = _gFOC_Ctrl.out.s_RealIdq.q;
+    /*
+		根据公式（等幅值变换，功率不等）:
+		iDC x vDC = 2/3(iq x vq + id x vd);
+	*/
+	s32 m_pow = (vd * id + vq * iq); //s32q10
+	s16 iDC = m_pow / _gFOC_Ctrl.in.s_vDC; //s16q5
+	return S16Q5toF(iDC) * 0.667f;
+}
+
+void PMSM_FOC_Brake(bool brake) {
+	_gFOC_Ctrl.in.b_eBrake = brake;
+	if (!_gFOC_Ctrl.in.b_motEnable) {
+		return;
+	}
+	if (_gFOC_Ctrl.in.b_eBrake & _gFOC_Ctrl.in.b_cruiseEna) {
+		_gFOC_Ctrl.in.b_cruiseEna = false;
+	}
+	eCtrl_brake_signal(brake);
+}
+
+
+/* MPTA, 弱磁, 功率限制 */
+static void PMSM_FOC_idq_Assign(void) {
+	_gFOC_Ctrl.in.s_targetIdq.d = 0;
+	_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetTrq;
+}

Applications/foc/core/PMSM_FOC_Core.h

@@ -0,0 +1,127 @@
+#ifndef _PMSM_FOC_Core_H__
+#define _PMSM_FOC_Core_H__
+#include "math/fix_math.h"
+#include "foc/core/PI_Controller.h"
+typedef struct {
+	s16q5_t a;
+	s16q5_t b;
+}AB_t;
+
+typedef struct {
+	s16q5_t d;
+	s16q5_t q;
+}DQ_t;
+
+typedef struct {
+	u8 		n_poles;
+	s16q5_t s_maxIdq;
+	s16q5_t s_maxiDC;
+	s32q14_t  s_maxRPM;
+	s16q5_t s_iABC[3];
+	s16     s_motRPM;   //from hall or encoder
+	s16q5_t s_motAngle; //from hall or encoder
+	s16     s_targetRPM;
+	s16q5_t s_targetTrq;
+	DQ_t    s_targetIdq;
+	DQ_t    s_targetVdq;
+	s16q5_t s_vDC;
+	u8      n_ctlMode;
+	bool    b_motEnable;
+	bool    b_cruiseEna;
+	bool    b_motLock;
+	bool    b_eBrake;
+	s16q5_t s_manualAngle; //mainly used when calibrate hall
+}FOC_InP;
+
+typedef struct {
+	u16   n_Duty[3];
+	u16   n_lowDuty;
+	u16   n_midDuty;
+	u8    n_Sector;
+	u8    n_RunMode;
+	DQ_t  s_OutVdq;
+	DQ_t  s_RealIdq;
+	s16q14_t f_vdqRation;
+	u8    n_Error;
+}FOC_OutP;
+
+typedef struct {
+	PI_Controller *id_ctl;
+	PI_Controller *iq_ctl;
+	PI_Controller *spd_ctl;
+	PI_Controller *fw_ctl;
+	FOC_InP       in;
+	FOC_OutP      out;
+}PMSM_FOC_Ctrl;
+
+typedef enum {
+	FOC_Success = 0,
+	FOC_NotAllowed = 1,
+	FOC_Throttle_Err, //ready的时候检测到转把信号
+	FOC_NowAllowed_With_Speed,
+	FOC_Speed_TooLow,
+	FOC_NotCruiseMode,
+	FOC_Param_Err,
+	FOC_Phase_Err,
+	FOC_Hall_Err,
+	FOC_Brake_Err = 100,
+	FOC_Unknow_Cmd = 0xFF,
+}foc_fault_t;
+
+
+#define OPEN_MODE                      ((u8)0U)
+#define SPD_MODE                       ((u8)1U)
+#define TRQ_MODE                       ((u8)2U)
+
+#define FOC_CALIMOD_HALL               ((u8) 1U)
+#define FOC_CALIMOD_MTPA               ((u8) 2U)
+
+#if 1
+#define SECTOR_1  0u
+#define SECTOR_2  1u
+#define SECTOR_3  2u
+#define SECTOR_4  3u
+#define SECTOR_5  4u
+#define SECTOR_6  5u
+#define SECTOR_UKNOW 0xFF
+#else
+#define SECTOR_1  3u
+#define SECTOR_2  4u
+#define SECTOR_3  5u
+#define SECTOR_4  0u
+#define SECTOR_5  1u
+#define SECTOR_6  2u
+#endif
+
+
+void PMSM_FOC_CoreInit(void);
+void PMSM_FOC_Schedule(void);
+void PMSM_FOC_Start(u8 nCtrlMode);
+void PMSM_FOC_Stop(void);
+void PMSM_FOC_iBusLimit(s16q5_t ibusLimit);
+void PMSM_FOC_SpeedLimit(s32q4_t speedLimit);
+s32q4_t PMSM_FOC_GetSpeedLimit(void);
+void PMSM_FOC_VbusVoltage(s16q5_t vbusVol);
+void PMSM_FOC_SetCtrlMode(u8 mode);
+void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq);
+bool PMSM_FOC_EnableCruise(bool enable);
+bool PMSM_FOC_Set_Speed(s32q4_t rpm);
+bool PMSM_FOC_Set_Current(float current);
+bool PMSM_FOC_Set_CruiseSpeed(float rpm);
+s32q4_t PMSM_FOC_GetSpeed(void);
+bool PMSM_FOC_Lock_Motor(bool lock);
+void PMSM_FOC_Brake(bool brake);
+s16q5_t PMSM_FOC_Get_iDC(void);
+void PMSM_FOC_LockMotor(bool lock);
+void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min);
+void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min);
+void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min);
+void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min);
+void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min);
+void PMSM_FOC_Set_Angle(s16 angle);
+bool PMSM_FOC_Is_Start(void);
+void PMSM_FOC_SetErrCode(u8 error);
+u8 PMSM_FOC_GetErrCode(void);
+
+#endif /* _PMSM_FOC_Core_H__ */
+

Applications/foc/core/PMSM_FOC_Params.c → Applications/foc/core/PMSM_FOC_Params.h

@@ -1,11 +1,7 @@
+#include "foc/foc_config.h"
 #include "foc/core/PI_Controller.h"
 
-#define cf_iBand  (2.0f * PI * FOC_PWM_FS / 20.0f * 3.0f);
-#define Ld 0.0009262f 
-#define Lq 0.001024f
-#define Rs 0.0918f
-
-PI_Controller PI_Ctrl_ID = {
+static PI_Controller PI_Ctrl_ID = {
 	.kp = S32Q14(0.001f),
 	.ki = S32Q14(0.003f),
 	.max = S32Q14(MAX_iDQ),
@@ -13,7 +9,7 @@ PI_Controller PI_Ctrl_ID = {
 	.Ui = 0,
 };
 
-PI_Controller PI_Ctrl_IQ = {
+static PI_Controller PI_Ctrl_IQ = {
 	.kp = S32Q14(0.001f),
 	.ki = S32Q14(0.003f),
 	.max = S32Q14(MAX_iDQ),
@@ -21,12 +17,19 @@ PI_Controller PI_Ctrl_IQ = {
 	.Ui = 0,
 };
 
-PI_Controller PI_Ctrl_Spd = {
+static PI_Controller PI_Ctrl_Spd = {
 	.kp = S32Q14(0.001f),
 	.ki = S32Q14(0.003f),
-	.max = S32Q14(Default_Spd_Limit),
+	.max = S32Q14(MAX_SPEED),
 	.min = S32Q14(0),
 	.Ui = 0,
 };
 
+static PI_Controller PI_Ctrl_fw = {
+	.kp = S32Q14(0.001f),
+	.ki = S32Q14(0.003f),
+	.max = S32Q14(20),
+	.min = S32Q14(0),
+	.Ui = 0,
+};
 

Applications/foc/core/foc_core.c

@@ -1,259 +0,0 @@
-#include "os/os_task.h"
-#include "bsp/pwm.h"
-#include "bsp/adc.h"
-#include "bsp/mc_hall_gpio.h"
-#include "foc/core/foc_core.h"
-#include "foc/motor/current.h"
-#include "foc/core/e_ctrl.h"
-#include "foc/foc_config.h"
-#include "bsp/timer_count32.h"
-#include "libs/time_measure.h"
-#include "libs/logger.h"
-#include "foc/core/PI_Controller.h"
-pmsm_foc_t pmsm_foc = {0};
-
-extern void PMSM_FOC_Init(void);
-extern ExtU *PMSM_FOC_GetInputs(void);
-extern ExtY *PMSM_FOC_GetOutputs(void);
-extern void PMSM_FOC_Step(void);
-extern P *PMSM_FOC_GetParams(void);
-PI_Controller pi;
-
-void PMSM_FOC_CoreInit(void) {
-	mc_hall_init();
-	eCtrl_init(0, 0); //default
-	PMSM_FOC_Init();
-	pmsm_foc.FOC_In  = PMSM_FOC_GetInputs();
-	pmsm_foc.FOC_Out = PMSM_FOC_GetOutputs();
-	pmsm_foc.FOC_P   = PMSM_FOC_GetParams();
-	pmsm_foc.FOC_P->i_pwm_count = FOC_PWM_Half_Period;
-	pmsm_foc.FOC_P->n_polePairs = 4;
-	pmsm_foc.FOC_P->id_fieldWeakMax = S16Q5(-50);
-	pmsm_foc.FOC_P->i_dqMax = S16Q5(150);
-	pmsm_foc.FOC_P->V_modulation = S16Q14(0.95f);
-	pmsm_foc.FOC_Out->n_Sector = 1;
-	PMSM_FOC_iBusLimit(S16Q5(Default_iDC_Limit));
-	PMSM_FOC_SpeedLimit(S32Q4(Default_Spd_Limit));
-}
-
-static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
-	if (pmsm_foc.b_lockmot) {
-		pwm_update_duty(0, 0, 0);
-		pwm_update_sample(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period, 1);
-	}else {
-		current_samp_t *cs = get_phase_sample_point(pmsm_foc.FOC_Out->n_Sector-1);
-	
-		pwm_update_duty(pmsm_foc.FOC_Out->n_Duty[0], pmsm_foc.FOC_Out->n_Duty[1], pmsm_foc.FOC_Out->n_Duty[2]);
-	
-		//pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
-	}
-}
-
-
-void PMSM_FOC_Schedule(void) {
-
-	phase_current_sample(&pmsm_foc.FOC_In->adc_Phase[0], &pmsm_foc.FOC_In->adc_Phase[1], &pmsm_foc.FOC_In->adc_Phase[2]);
-	pmsm_foc.FOC_In->sys_ticks = hall_get_hwcount(pmsm_foc.FOC_In->hall_abc);
-
-	if (pmsm_foc.FOC_In->n_ctrlMod == SPD_MODE) {
-		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_RefSpd());
-		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_FinalTorque());
-	}else if(pmsm_foc.FOC_In->n_ctrlMod == TRQ_MODE){
-		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_FinalSpd());
-		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_RefTorque());
-	}
-	PI_Controller_run(&pi, 111);
-	
-	//PMSM_FOC_Step();
-
-	pmsm_foc.FOC_Out->n_Duty[0] = pmsm_foc.FOC_Out->n_Duty[1] = pmsm_foc.FOC_Out->n_Duty[2] = FOC_PWM_Half_Period;
-	
-	PMSM_FOC_PWMCurrent_Update();
-}
-
-void PMSM_FOC_Start(u8 nCtrlMode) {
-	if (pmsm_foc.b_FocEna) {
-		return;
-	}
-	pmsm_foc.b_FocEna = true;
-	pmsm_foc.FOC_In->b_motEna = true;
-	pmsm_foc.FOC_In->n_ctrlMod = nCtrlMode;
-	pmsm_foc.FOC_In->b_cruiseEna = 0;
-	pmsm_foc.FOC_In->spd_Target = 0;
-	pmsm_foc.FOC_In->idq_Target = 0;
-	pmsm_foc.FOC_In->vdq_Target[0] = 0;
-	pmsm_foc.FOC_In->vdq_Target[1] = 0;
-}
-
-void PMSM_FOC_Stop(void) {
-	if (!pmsm_foc.b_FocEna) {
-		return;
-	}
-	pmsm_foc.b_FocEna = false;
-	memset(pmsm_foc.FOC_In, 0, sizeof(ExtU));
-}
-
-bool PMSM_FOC_Is_Start(void) {
-	return pmsm_foc.FOC_In->b_motEna;
-}
-
-void PMSM_FOC_iBusLimit(s16q5_t ibusLimit) {
-	pmsm_foc.FOC_In->iDC_Limit = (ibusLimit);
-}
-
-void PMSM_FOC_SpeedLimit(s32q4_t speedLimit) {
-	pmsm_foc.FOC_In->spd_Limit = (speedLimit);
-}
-
-float PMSM_FOC_GetSpeedLimit(void) {
-	return S32Q4toF(pmsm_foc.FOC_In->spd_Limit);
-}
-
-void PMSM_FOC_VbusVoltage(s16q5_t vbusVol) {
-	pmsm_foc.FOC_In->vDC = (vbusVol);
-}
-
-void PMSM_FOC_SetCtrlMode(uint8_T mode) {
-	pmsm_foc.FOC_In->n_ctrlMod = mode;
-}
-
-void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
-	pmsm_foc.FOC_In->vdq_Target[0] = (vd);
-	pmsm_foc.FOC_In->vdq_Target[1] = (vq);
-}
-
-bool PMSM_FOC_EnableCruise(boolean_T enable) {
-	if (enable) {
-		float motSpd = PMSM_FOC_GetSpeed();
-		if (motSpd < MIN_CRUISE_RPM) { //
-			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
-			return false;
-		}
-		eCtrl_set_TargetSpeed(motSpd);
-	}
-	pmsm_foc.FOC_In->b_cruiseEna = enable;
-	return true;
-}
-
-bool PMSM_FOC_Is_CruiseEnabled(void) {
-	return (pmsm_foc.FOC_In->b_cruiseEna && (pmsm_foc.FOC_Out->n_FocMode == SPD_MODE));
-}
-
-bool PMSM_FOC_Set_Speed(float rpm) {
-	if (pmsm_foc.FOC_In->b_cruiseEna) {
-		return false;
-	}
-	eCtrl_set_TargetSpeed(rpm);
-	return true;
-}
-
-bool PMSM_FOC_Set_Current(float current) {
-	eCtrl_set_TrqCurrent(current);
-	return true;
-}
-
-bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
-	if (PMSM_FOC_Is_CruiseEnabled()) {
-		eCtrl_set_TargetSpeed(rpm);
-		return true;
-	}
-	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
-	return false;
-}
-
-void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, s16 open_vd) {
-
-	if (pmsm_foc.FOC_In->b_motEna == b_caliHall) { //motor must be stoped when start cali
-		return;
-	}
-	pmsm_foc.FOC_In->vdq_Target[0] = S16Q4(open_vd);
-	pmsm_foc.FOC_In->vdq_Target[1] = 0;
-	pmsm_foc.FOC_In->n_ctrlMod = OPEN_MODE;
-}
-
-void PMSM_FOC_Set_Vdq(s16 vd, s16 vq) {
-	pmsm_foc.FOC_In->vdq_Target[0] = S16Q4(vd);
-	pmsm_foc.FOC_In->vdq_Target[1] = S16Q4(vq);
-}
-
-void PMSM_FOC_Set_Angle(s16 angle) {
-	pmsm_foc.FOC_In->set_Angle = S16Q14(angle);
-}
-
-float PMSM_FOC_GetSpeed(void) {
-	return S32Q4toF(pmsm_foc.FOC_Out->f_MotRPM);
-}
-
-
-void PMSM_FOC_SetErrCode(u8 code) {
-	pmsm_foc.error_code = code;
-}
-
-
-u8 PMSM_FOC_GetErrCode(void) {
-	return pmsm_foc.error_code;
-}
-
-void PMSM_FOC_LockMotor(bool lock) {
-	pmsm_foc.b_lockmot = lock;
-}
-
-void PMSM_FOC_SetSpdPid(float kp, float ki, float kb) {
-	pmsm_foc.FOC_P->cf_nKp = S16Q10(kp);
-	pmsm_foc.FOC_P->cf_nKi = S16Q14(ki);
-	pmsm_foc.FOC_P->cf_nKb = S16Q14(kb);
-}
-
-void PMSM_FOC_SetIDPid(float kp, float ki, float kb) {
-	pmsm_foc.FOC_P->cf_idKp = S16Q10(kp);
-	pmsm_foc.FOC_P->cf_idKi = S16Q14(ki);
-	pmsm_foc.FOC_P->cf_idKb = S16Q14(kb);
-}
-
-void PMSM_FOC_SetIQPid(float kp, float ki, float kb) {
-	pmsm_foc.FOC_P->cf_iqKp = S16Q10(kp);
-	pmsm_foc.FOC_P->cf_iqKi = S16Q14(ki);
-	pmsm_foc.FOC_P->cf_iqKb = S16Q14(kb);
-}
-
-void PMSM_FOC_SetTrqPid(float kp, float ki, float kb) {
-
-}
-
-void PMSM_FOC_SetFW_I(float ki, float kb) {
-	pmsm_foc.FOC_P->cf_Fw_Ki = S16Q10(ki);
-	pmsm_foc.FOC_P->cf_Fw_Kb = S16Q10(kb);
-
-}
-
-s16 PMSM_FOC_Add_Hall_Offset(s16 offset) {
-	pmsm_foc.FOC_P->i_hall_offset = S16Q4(offset);
-	return (s16)S16Q4toF(pmsm_foc.FOC_P->i_hall_offset);
-}
-
-//获取母线电流
-float PMSM_FOC_Get_iDC(void) {
-	s32 vd = pmsm_foc.FOC_Out->f_Vdq[0];
-	s32 vq = pmsm_foc.FOC_Out->f_Vdq[1];
-	s32 id = pmsm_foc.FOC_Out->f_Idq[0];
-	s32 iq = pmsm_foc.FOC_Out->f_Idq[1];
-    /*
-		根据公式（等幅值变换，功率不等）:
-		iDC x vDC = 2/3(iq x vq + id x vd);
-	*/
-	s32 m_pow = (vd * id + vq * iq); //s32q10
-	s16 iDC = m_pow / pmsm_foc.FOC_In->vDC; //s16q5
-	return S16Q5toF(iDC) * 0.667f;
-}
-
-void PMSM_FOC_Brake(bool brake) {
-	pmsm_foc.b_brake_in = brake;
-	if (!pmsm_foc.FOC_In->b_motEna) {
-		return;
-	}
-	if (pmsm_foc.b_brake_in & pmsm_foc.FOC_In->b_cruiseEna) {
-		pmsm_foc.FOC_In->b_cruiseEna = false;
-	}
-	eCtrl_brake_signal(brake);
-}
-

Applications/foc/core/foc_core.h

@@ -1,71 +0,0 @@
-#ifndef _FOC_CORE_H__
-#define _FOC_CORE_H__
-#include "math/fix_math.h"
-
-typedef struct {
-	u8 		n_poles;
-	s16q5_t s_maxIdq;
-	s16q5_t s_maxiDC;
-	s16     s_maxRPM;
-	s16q5_t s_iABC[3];
-	s16     s_motRPM;   //from hall or encoder
-	s16q5_t s_motAngle; //from hall or encoder
-	s16     s_targetRPM;
-	s16q5_t s_targetIdq;
-	s16q5_t s_targetVdq;
-	u8      n_ctlMode;
-	bool    b_motEnable;
-	s16q5_t s_manualAngle; //mainly used when calibrate hall
-}FOC_InP;
-
-typedef struct {
-	u16 n_Duty[3];
-	u8  n_Sector;
-	u8  n_RunMode;
-	u8  s_RealVdq;
-	u8  s_RealIdq;
-}FOC_OutP;
-
-typedef struct {
-	PI_Controller id_ctl;
-	PI_Controller iq_ctl;
-	PI_Controller spd_ctl;
-	PI_Controller fw_crl;
-	FOC_InP       in;
-	FOC_OutP      out;
-}PMSM_FOC_Ctrl;
-
-void PMSM_FOC_CoreInit(void);
-void PMSM_FOC_Schedule(void);
-void PMSM_FOC_Start(u8 nCtrlMode);
-void PMSM_FOC_Stop(void);
-void PMSM_FOC_iBusLimit(s16q5_t ibusLimit);
-void PMSM_FOC_SpeedLimit(s32q4_t speedLimit);
-float PMSM_FOC_GetSpeedLimit(void);
-void PMSM_FOC_VbusVoltage(s16q5_t vbusVol);
-void PMSM_FOC_SetCtrlMode(uint8_T mode);
-void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq);
-bool PMSM_FOC_EnableCruise(boolean_T enable);
-bool PMSM_FOC_Set_Speed(float rpm);
-bool PMSM_FOC_Set_Current(float current);
-bool PMSM_FOC_Set_CruiseSpeed(float rpm);
-void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, s16q5_t open_vd);
-float PMSM_FOC_GetSpeed(void);
-void PMSM_FOC_SetErrCode(u8 code);
-u8 PMSM_FOC_GetErrCode(void);
-bool PMSM_FOC_Lock_Motor(bool lock);
-void PMSM_FOC_Brake(bool brake);
-float PMSM_FOC_Get_iDC(void);
-void PMSM_FOC_LockMotor(bool lock);
-void PMSM_FOC_SetSpdPid(float kp, float ki, float kb);
-void PMSM_FOC_SetIDPid(float kp, float ki, float kb);
-void PMSM_FOC_SetIQPid(float kp, float ki, float kb);
-void PMSM_FOC_SetTrqPid(float kp, float ki, float kb);
-void PMSM_FOC_SetFW_I(float ki, float kb);
-s16 PMSM_FOC_Add_Hall_Offset(s16 offset);
-void PMSM_FOC_Set_Angle(s16 angle);
-bool PMSM_FOC_Is_Start(void);
-void PMSM_FOC_Set_Vdq(s16 vd, s16 vq);
-
-#endif /* _FOC_CORE_H__ */
-

Applications/foc/core/foc_type.h

@@ -1,118 +0,0 @@
-#ifndef _FOC_TYPE_H__
-#define _FOC_TYPE_H__
-#include <math.h>
-#include "os/os_types.h"
-#include "ramp_ctrl.h"
-#include "PMSM_Controller.h"
-#include "math/fix_math.h"
-
-typedef struct _alphabeta {
-	float alpha;
-	float beta;
-}alpha_beta_t;
-
-typedef struct _dqaix {
-	union {
-		float Id;
-		float Vd;
-	};
-	union {
-		float Iq;
-		float Vq;
-	};
-}dq_t;
-
-
-typedef struct _phase_time {
-	s32 A;
-	s32 B;
-	s32 C;
-	s32 A_next;
-	s32 B_next;
-	s32 C_next;
-	u32 Samp_p1; //单电阻方案两个采样时刻可能不是连续的
-	u32 Samp_p2;
-	u8  sampe_phase_1;
-	u8  sampe_phase_2;
-	u8  boundary3_flags;
-	u32 low;
-	u32 midle;
-	u32 high;
-	u8  three_shunts_flags;
-}phase_time_t; //����pwn��duty cnt
-
-typedef enum {
-	IDLE = 0,
-	ANY_STOP,
-	START,
-	CURRENT_CALIBRATE,
-	CHARGER_BOOT_CAP,
-	READY_TO_RUN,
-	RAMPING_START,
-	RUNNING
-}foc_state_t;
-
-typedef enum {
-	INVERT_NO,
-	INVERT_A,
-	INVERT_B,
-	INVERT_C
-}phase_invert_t;
-
-
-typedef enum {
-	FOC_Success = 0,
-	FOC_NotAllowed = 1,
-	FOC_Throttle_Err, //ready的时候检测到转把信号
-	FOC_NowAllowed_With_Speed,
-	FOC_Speed_TooLow,
-	FOC_NotCruiseMode,
-	FOC_Param_Err,
-	FOC_Phase_Err,
-	FOC_Hall_Err,
-	FOC_Brake_Err = 100,
-	FOC_Unknow_Cmd = 0xFF,
-}foc_fault_t;
-
-
-typedef struct _foc {
-	u8          error_code;
-	bool   		b_brake_in;
-	bool        b_FocEna;
-	bool        b_lockmot;
-	ExtU 		*FOC_In;                       /* External inputs */
-	ExtY 		*FOC_Out;                       /* External outputs */
-	P           *FOC_P;
-}pmsm_foc_t;
-
-
-#define OPEN_MODE                      ((uint8_T)0U)
-#define SPD_MODE                       ((uint8_T)1U)
-#define TRQ_MODE                       ((uint8_T)2U)
-
-#define FOC_CALIMOD_HALL               ((uint16_T) 1U)
-#define FOC_CALIMOD_MTPA               ((uint16_T) 2U)
-
-#define degree_2_pi(d) ((float)d * M_PI / 180.0f)
-#define pi_2_degree(d) ((float)d * 180.0f / M_PI)
-#define A_mA(a) ((a*1000))
-#define mA_A(ma) (((float)(ma))/1000.0f)
-
-#if 1
-#define SECTOR_1  0u
-#define SECTOR_2  1u
-#define SECTOR_3  2u
-#define SECTOR_4  3u
-#define SECTOR_5  4u
-#define SECTOR_6  5u
-#define SECTOR_UKNOW 0xFF
-#else
-#define SECTOR_1  3u
-#define SECTOR_2  4u
-#define SECTOR_3  5u
-#define SECTOR_4  0u
-#define SECTOR_5  1u
-#define SECTOR_6  2u
-#endif
-#endif /* _FOC_TYPE_H__ */
-

Applications/foc/core/svpwm.c

@@ -1,7 +1,8 @@
-#include "foc/svpwm.h"
+#include "foc/core/svpwm.h"
 #include "math/fast_math.h"
 
 
+#if 0
 static void __inline ModuleTime(u32 *T4, u32 *T6, u32 PWM_Period) {
 	u32 period = PWM_Period * 95 / 100; //95%�ĵ���
 	if (*T4 + *T6 > period){
@@ -373,6 +374,138 @@ void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_perio
 	phase_out->high = high;
 	*sector_out = sector;
 }
+#endif
 
 
+void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
+	s16q5_t alpha = S16_mul(alb->a, SQRT3_BY_2_Q14, 14);
+	s16q5_t beta  = S16_mul(alb->b, SQRT3_BY_2_Q14, 14);
+	u32   PWM_Period = PWM_half_period * 2;
+	u8 sector = 0xFF;
+	u32 tA, tB, tC;
+	u32 low, midle;
+	u32 X, Y, Z;
+	s16q5_t mod = ((PWM_Period << 10) / vbus) >> 5;
+
+	if (beta >= 0) {
+		if (alpha >= 0) {
+			//quadrant I
+			if (S16_mul(ONE_BY_SQRT3_Q14, beta, 14) > alpha) {
+				sector = SECTOR_2;
+			} else {
+				sector = SECTOR_1;
+			}
+		} else {
+			//quadrant II
+			if (-S16_mul(ONE_BY_SQRT3_Q14, beta, 14) > alpha) {
+				sector = SECTOR_3;
+			} else {
+				sector = SECTOR_2;
+			}
+		}
+	} else {
+		if (alpha >= 0.0f) {
+			//quadrant IV5
+			if (-S16_mul(ONE_BY_SQRT3_Q14 ,beta, 14) > alpha) {
+				sector = SECTOR_5;
+			} else {
+				sector = SECTOR_6;
+			}
+		} else {
+			//quadrant III
+			if (S16_mul(ONE_BY_SQRT3_Q14, beta, 14) > alpha) {
+				sector = SECTOR_4;
+			} else {
+				sector = SECTOR_5;
+			}
+		}
+	}
+	s16q5_t temp = S16_mul(ONE_BY_SQRT3_Q14, beta, 14);
+	X = S16_mul(TWO_BY_SQRT3_Q14, S16_mul(beta, mod, 5), 14);
+	Y = S16_mul((alpha + temp), mod, 5);
+	Z = S16_mul((-alpha + temp) ,mod, 5);
+	switch(sector) {
+		case SECTOR_1: // 3
+		{	u32 T4 = -Z;
+			u32 T6 = X;
+			tC = (PWM_Period - T4 - T6)/4;
+        	tB = tC + T6/2;
+        	tA = tB + T4/2;
+			low = tA;
+			midle = tB;
+			//high = tC;
+			break;
+		}
+		case SECTOR_2: // 1
+		{
+			u32 T6 = Y;
+			u32 T2 = Z;
+			tC = (PWM_Period - T6 - T2)/4;
+        	tA = tC + T6/2;
+        	tB = tA + T2/2;
+			low = tB;
+			midle = tA;
+			//high = tC;			
+			break;
+		}
+		case SECTOR_3: // 5
+		{
+			u32 T2 = X;
+			u32 T3 = -Y;
+			tA = (PWM_Period - T2 - T3)/4;
+			tC = tA + T3/2;
+			tB = tC + T2/2;
+			low = tB;
+			midle = tC;
+			//high = tA;			
+			break;
+		}
+		case SECTOR_4: // 4
+		{
+			u32 T1 = -X;
+			u32 T3 = Z;
+			tA = (PWM_Period - T1 - T3)/4;
+			tB = tA + T3/2;
+			tC = tB + T1/2;
+			low = tC;
+			midle = tB;
+			//high = tA;			
+			break;
+		}
+		case SECTOR_5: // 6
+		{
+			u32 T1 = -Y;
+			u32 T5 = -Z;
+			tB = (PWM_Period - T1 - T5)/4;
+			tA = tB + T5/2;
+			tC = tA + T1/2;
+			low = tC;
+			midle = tA;
+			//high = tB;			
+			break;
+		}					
+		case SECTOR_6: // 2
+		{
+			u32 T4 = Y;
+			u32 T5 = -X;
+			tB = (PWM_Period - T4 - T5)/4;
+			tC = tB + T5/2;
+			tA = tC + T4/2;
+			low = tA;
+			midle = tC;
+			//high = tB;			
+			break;
+		}
+		default:
+			break;
+	}	
+	out->n_Duty[0] = tA;
+	out->n_Duty[1] = tB;
+	out->n_Duty[2] = tC;
+	out->n_lowDuty = low;
+	out->n_midDuty = midle;
+	//out->n_highDuty = high;
+	out->n_Sector = sector;
+}
+
 

Applications/foc/core/svpwm.h

@@ -1,10 +1,13 @@
 #ifndef _SVPWM_H__
 #define _SVPWM_H__
-#include "foc/foc_type.h"
-
+#include "foc/core/PMSM_FOC_Core.h"
+#if 0
 void svpwm(alpha_beta_t *alpha_beta, float vbus, u32 PWW_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
+#endif
+void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out);
+
 #endif /* _SVPWM_H__ */
 

Applications/foc/foc_config.h

@@ -9,10 +9,11 @@
 #define THROTTLE_MAX_VALUE 4.9f /* 转把最大值 */
 #define THROTTLE_MIN_RPM   10   /* 转把对应最小的速度 */
 #define THROTTLE_MIN_IDQ   20   /* 转把对应最小的扭矩电流 Q轴 */
-#define MAX_iDQ            150  /* 最大DQ轴电流*/
-
-
+#define MAX_iDQ            150  /* 最大DQ轴电流， A*/
+#define MAX_SPEED          8200 /* 最大转速 RPM*/
+#define MAX_iDC            45   /* 最大母线电流 A*/
 #define MIN_CRUISE_RPM 1000     /* 能启动定速巡航的最小速度 */
+#define MAX_vDC (50)   /* 母线最大电压 V*/
 
 /* 电子刹车，动能回收，加速 */
 #define TORQUE_STEP 1.0f          /* 扭矩斜率给定的step值，单位 A */

Applications/foc/motor/current.c

@@ -2,6 +2,7 @@
 #include "bsp/adc.h"
 #include "bsp/pwm.h"
 #include "foc/motor/current.h"
+#include "foc/core/PMSM_FOC_Core.h"
 #include "libs/utils.h"
 #include "libs/logger.h"
 
@@ -84,14 +85,15 @@ bool phase_current_offset(void) {
 
 
 #define LowPass_filter 1.0f
-void phase_current_sample(s16 *ia, s16 *ib, s16 *ic){
+void phase_current_get(s16 *iABC){
 	current_samp_t *cs = &g_cs;
 	s32 phase_current1, phase_current2;
 	phase_time_t *time = &cs->time;
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
 	if (time->three_shunts_flags == 1) {
-		*ia = cs->adc_ia;
-		*ib = cs->adc_ib;		
+		iABC[0] = cs->adc_ia;
+		iABC[1] = cs->adc_ib;
+		iABC[2] = cs->adc_ic;
 		time->three_shunts_flags = 0;
 		return; //use old current;
 	}
@@ -114,18 +116,18 @@ void phase_current_sample(s16 *ia, s16 *ib, s16 *ic){
 		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
 		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
 	}
-	*ia = cs->adc_ia;
-	*ib = cs->adc_ib;
-	*ic = cs->adc_ic;
+	iABC[0] = cs->adc_ia;
+	iABC[1] = cs->adc_ib;
+	iABC[2] = cs->adc_ic;
 }
 
 
-current_samp_t *get_phase_sample_point(u8 sector){
+u32 phase_current_point(u8 *sector, u16 *duty, u16 low, u16 midle){
 	current_samp_t *cs = &g_cs;
 	phase_time_t *time = &cs->time;
-	u32 low_side_low_duty = FOC_PWM_Half_Period - time->low;
-	u32 low_side_mid_duty = FOC_PWM_Half_Period - time->midle;
-	cs->sector = sector;
+	u32 low_side_low_duty = FOC_PWM_Half_Period - low;
+	u32 low_side_mid_duty = FOC_PWM_Half_Period - midle;
+	cs->sector = *sector;
 	time->Samp_p1 = FOC_PWM_Half_Period + 1;
 	time->Samp_p2 = FOC_PWM_Half_Period + 1;
 	/*底边开mos的时间是2倍的 low_side_low_duty(一个周期)*/
@@ -134,7 +136,7 @@ current_samp_t *get_phase_sample_point(u8 sector){
 			time->Samp_p1 = FOC_PWM_Half_Period - 1;
 			cs->sector = SECTOR_1;
 		}else {
-			u32 Samp_p = time->low + TSampleBefore;
+			u32 Samp_p = low + TSampleBefore;
 			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
 				time->Samp_p2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
 			}else {
@@ -145,7 +147,7 @@ current_samp_t *get_phase_sample_point(u8 sector){
 		if (low_side_mid_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
 			time->Samp_p1 = FOC_PWM_Half_Period - 1;
 		}else {
-			u32 Samp_p = time->midle + TSampleBefore;
+			u32 Samp_p = midle + TSampleBefore;
 			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
 				time->Samp_p2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
 			}else {
@@ -156,7 +158,8 @@ current_samp_t *get_phase_sample_point(u8 sector){
 		time->three_shunts_flags = 1; //means do'nt use the sample current
 		time->Samp_p1 = FOC_PWM_Half_Period - 1;//dumy trigger
 	}
-	return cs;
+	*sector = cs->sector;
+	return (time->Samp_p1 | time->Samp_p2>>16);
 }
 
 

Applications/foc/motor/current.h

@@ -1,6 +1,6 @@
 #ifndef _PHASE_CURRENT_H__
 #define _PHASE_CURRENT_H__
-#include "foc/core/foc_type.h"
+#include "math/fix_math.h"
 /* boundary zone definition */
 #define REGULAR         ((uint8_t)0u)
 #define BOUNDARY_1      ((uint8_t)1u)  /* Two small, one big */
@@ -17,6 +17,20 @@
 #define SAMP_OLDA 7u
 #define SAMP_OLDB 8u
 #define SAMP_OLDC 9u
+typedef struct _phase_time {
+	s32 A;
+	s32 B;
+	s32 C;
+	s32 A_next;
+	s32 B_next;
+	s32 C_next;
+	u32 Samp_p1; //单电阻方案两个采样时刻可能不是连续的
+	u32 Samp_p2;
+	u8  sampe_phase_1;
+	u8  sampe_phase_2;
+	u8  boundary3_flags;
+	u8  three_shunts_flags;
+}phase_time_t; //����pwn��duty cnt
 
 typedef struct current_sample {
 	s32   adc_offset_a;
@@ -32,9 +46,9 @@ typedef struct current_sample {
 }current_samp_t;
 
 void phase_current_init(void);
-void phase_current_sample(s16 *ia, s16 *ib, s16 *ic);
+void phase_current_get(s16 *iABC);
 bool phase_current_offset(void);
-current_samp_t * get_phase_sample_point(u8 sector);
+u32 phase_current_point(u8 *sector, u16 *duty, u16 low, u16 midle);
 void phase_current_adc_triger(void);
 void phase_current_start_cali(void);
 #endif /* _PHASE_CURRENT_H__ */

Applications/foc/motor/hall.c

@@ -0,0 +1,379 @@
+#include <string.h>
+#include "bsp/bsp.h"
+#include "bsp/mc_hall_gpio.h"
+#include "os/os_task.h"
+#include "libs/utils.h"
+#include "libs/logger.h"
+#include "math/fast_math.h"
+#include "foc/motor/hall.h"
+#include "app/nv_storage.h"
+#include "bsp/timer_count32.h"
+#include "libs/time_measure.h"
+
+//#define USE_DETECTED_ANGLE 1
+
+#define HALL_READ_TIMES 3
+
+static u32 _hall_detect_task(void *args);
+static void _hall_init_el_angle(void);
+
+
+#define HALL_PLACE_OFFSET (315 << 19) //(345) //315
+/* 
+4,5,1,3,2,6,4
+*/
+
+static hall_sensor_t _sensor_hander;
+
+measure_time_t g_meas_hall = {.exec_max_time = 6,};
+
+//#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
+#define us_2_s(tick) S32Q14((float)tick / 1000000.0f) //s32q14
+
+#define rand_angle(a) {if (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;else if (a < 0) a +=PHASE_360_DEGREE;};
+
+static void __inline _hall_put_sample(u32 ticks, s32q19_t angle) {
+	hall_sample_t *s = &_sensor_hander.samples;
+	s->ticks_sum -= s->ticks[s->index];
+	s->angles_sum -= s->angles[s->index];
+	s->ticks[s->index] = ticks;
+	s->angles[s->index] = angle;
+	s->ticks_sum += s->ticks[s->index];
+	s->angles_sum += s->angles[s->index];
+	s->index += 1;
+	if (s->index >= SAMPLE_MAX_COUNT) {
+		s->full = true;
+		s->index = 0;
+	}
+}
+
+static s32q5_t __inline _hall_angle_speed(void){
+	hall_sample_t *s = &_sensor_hander.samples;
+	if (s->ticks_sum == 0) {
+		return 0.0f;
+	}
+	
+	if (!s->full) {
+		return s->angles[s->index - 1] / us_2_s(s->ticks[s->index-1]);
+	}else {
+		return s->angles_sum / us_2_s(s->ticks_sum);
+	}
+}
+
+void hall_debug_log(void) {
+	sys_debug("angle dir %d\n", _sensor_hander.direction);
+}
+
+/*
+static bool __inline _hall_data_empty(void) {
+	hall_sample_t *s = &_sensor_hander.samples;
+	if ((!s->full) && (s->index == 0)){
+		return true;
+	}
+	return false;
+}
+*/
+
+static void hall_sensor_default(void) {
+	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
+	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+	memcpy((char *)_sensor_hander.angle_table, (char *)mc_config_get()->hall_table, sizeof(_sensor_hander.angle_table));
+	_hall_init_el_angle();
+	hall_debug_log();
+}
+
+void hall_sensor_init(void) {
+	mc_hall_init();
+	hall_sensor_default();
+	
+	shark_task_create(_hall_detect_task, NULL);
+}
+
+void hall_sensor_clear(void) {
+	hall_sensor_default();
+}
+
+
+static u32 _hall_detect_task(void *args) {
+	if (_sensor_hander.el_speed != 0) {
+		u32 ticks_now = timer_count32_get();
+		u32 delta_us = timer_count32_delta(ticks_now, _sensor_hander.hall_ticks);
+		if (delta_us >= (1200*1000)) {
+			hall_sensor_clear();
+		}
+	}
+	return 0;
+}
+
+s16q5_t hall_sensor_get_theta(void){
+	u32 us_now = timer_count32_get();
+	u32 us_delta = timer_count32_delta(us_now, _sensor_hander.estimate_time_ticks);
+	_sensor_hander.estimate_time_ticks = us_now;
+	s32q19_t angle_step = _sensor_hander.estimate_el_speed * us_2_s(us_delta);
+	_sensor_hander.estimate_delta_angle += angle_step;
+
+	if (_sensor_hander.direction == POSITIVE) {
+		_sensor_hander.estimate_el_angle += angle_step;
+	}else {
+		_sensor_hander.estimate_el_angle -= angle_step;
+	}
+
+	rand_angle(_sensor_hander.estimate_el_angle);
+
+	return (_sensor_hander.estimate_el_angle >> 14);
+}
+
+
+s32q5_t hall_sensor_get_speed(void) {
+	return _sensor_hander.rpm;
+}
+
+int hall_offset_increase(int inc) {
+	inc = inc << 19;
+	if (_sensor_hander.phase_offset + inc >= PHASE_360_DEGREE) {
+		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - PHASE_360_DEGREE;
+	}else {
+		_sensor_hander.phase_offset += inc;
+	}
+	return _sensor_hander.phase_offset;
+}
+
+s32 *hall_get_table(void) {
+	return _sensor_hander.angle_table;
+}
+
+
+static void _hall_init_el_angle(void) {
+	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
+#ifdef USE_DETECTED_ANGLE
+	if (_sensor_hander.hall_stat == 0 || _sensor_hander.hall_stat == 7) {
+		_sensor_hander.sensor_error ++;
+		return;
+	}
+	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + _sensor_hander.angle_table[_sensor_hander.hall_stat];
+#else
+	s32 sector_center = PHASE_60_DEGREE/2;
+  	switch ( _sensor_hander.hall_stat )
+  	{
+    case STATE_5:
+      	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + sector_center;
+      	break;
+    case STATE_1:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + sector_center;
+      	break;
+    case STATE_3:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + sector_center;
+      	break;
+    case STATE_2:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_180_DEGREE + sector_center;
+      	break;
+    case STATE_6:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + sector_center;
+      	break;
+    case STATE_4:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + sector_center;
+      	break;
+    default:
+      	/* Bad hall sensor configutarion so update the speed reliability */
+      	_sensor_hander.sensor_error ++;
+      	return;
+ 	}
+#endif
+	_sensor_hander.sensor_error = 0;
+  	/* Initialize the measured angle */
+	rand_angle(_sensor_hander.measured_el_angle);
+  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
+	_sensor_hander.hall_ticks = timer_count32_get();
+	_sensor_hander.estimate_time_ticks = timer_count32_get();
+}
+
+static __inline__ s32 _get_angle(u8 state, s32 added) {
+#ifdef USE_DETECTED_ANGLE
+	return _sensor_hander.phase_offset + _sensor_hander.angle_table[state];
+#else
+	return _sensor_hander.phase_offset + added;
+#endif
+}
+/* 4,5,1,3,2,6,4 */
+static s32 _hall_position(u8 state_now, u8 state_prev) {
+	s32 theta_now =  0xFFFFFFFF;
+	switch (state_now) {
+		case STATE_1:
+			if (state_prev == STATE_5) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
+			}else if (state_prev == STATE_3) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
+			}
+			break;
+		case STATE_2:
+			if (state_prev == STATE_3) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
+			}else if (state_prev == STATE_6) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
+			}
+			break;
+		case STATE_3:
+			if (state_prev == STATE_1) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
+			}else if (state_prev == STATE_2) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
+			}
+			break;
+		case STATE_4:
+			if (state_prev == STATE_6) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
+			}else if (state_prev == STATE_5) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
+			}
+			break;
+		case STATE_5:
+			if (state_prev == STATE_4) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
+			}else if (state_prev == STATE_1) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
+			}
+			break;
+		case STATE_6:
+			if (state_prev == STATE_2) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
+			}else if (state_prev == STATE_4) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
+			}
+			break;
+		default:
+			_sensor_hander.sensor_error ++;
+			return 0xFFFFFFFF;
+	}
+	rand_angle(theta_now);
+	return theta_now;
+}
+
+#ifdef USE_DETECTED_ANGLE
+static __inline u8 _next_hall(u8 hall_now) {
+	switch (hall_now) {
+		case STATE_1:
+			if (_sensor_hander.direction == POSITIVE) {
+				return STATE_3;
+			}else {
+				return STATE_5;
+			}
+		case STATE_2:
+			if (_sensor_hander.direction == POSITIVE) {
+				return STATE_6;
+			}else {
+				return STATE_3;
+			}
+		case STATE_3:
+			if (_sensor_hander.direction == POSITIVE) {
+				return STATE_2;
+			}else {
+				return STATE_1;
+			}
+		case STATE_4:
+			if (_sensor_hander.direction == POSITIVE) {
+				return STATE_5;
+			}else {
+				return STATE_6;
+			}
+		case STATE_5:
+			if (_sensor_hander.direction == POSITIVE) {
+				return STATE_1;
+			}else {
+				return STATE_4;
+			}
+		case STATE_6:
+			if (_sensor_hander.direction == POSITIVE) {
+				return STATE_4;
+			}else {
+				return STATE_2;
+			}
+		default: //not reached here
+			return STATE_1;
+	}
+
+} 
+
+
+static __inline__ s32 _get_delta_angle(u8 now, u8 next) {
+	s32 delta_angle = _sensor_hander.angle_table[next] - _sensor_hander.angle_table[now];
+	if (_sensor_hander.direction == POSITIVE) {
+		if (delta_angle < 0) { //process cross 360 degree
+			delta_angle += PHASE_360_DEGREE;
+		}
+	}else if (_sensor_hander.direction == NEGATIVE) {
+		if (delta_angle > 0) { //process cross 360 degree
+			delta_angle -= PHASE_360_DEGREE;
+		}
+		delta_angle = -delta_angle;
+	}
+	return delta_angle;
+}
+#endif
+
+void HALL_IRQHandler(void) {
+	time_measure_start(&g_meas_hall);
+	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
+	u8 hall_stat_prev = _sensor_hander.hall_stat;
+	u32 hall_ticks_now = timer_count32_get();	
+
+	/*获取当前转子角度*/
+	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
+	if (theta_now == 0xFFFFFFFF) {
+		return;
+	}
+	//获取两次中断的时间间隔，估计速度
+	u32 delta_us = timer_count32_delta(hall_ticks_now, _sensor_hander.hall_ticks);
+	if (delta_us == 0) {
+		return;
+	}
+	//获取两次中断之间转子转过的角度,获取预期的下次hall状态变换转过的角度
+#ifdef USE_DETECTED_ANGLE
+	s32 delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
+	s32 next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
+#else
+	s32 delta_angle = PHASE_60_DEGREE;
+	s32 next_delta_angle = delta_angle;
+#endif
+	s32 delta_time = us_2_s(delta_us);
+	s32 prev_imme_el_speed = _sensor_hander.immediately_el_speed;
+	_sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
+	s32 delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
+	if (delta_el_speed*100/prev_imme_el_speed >= 20) { //即时速度增加10%,认为不稳定，需要使用即时速度估计转子位置
+		_sensor_hander.trns_detect = true;
+	}else {
+		_sensor_hander.trns_detect = false;
+	}
+	_hall_put_sample(delta_us, delta_angle);
+	os_disable_irq();
+	_sensor_hander.el_speed = _hall_angle_speed();	//s32q5
+	_sensor_hander.estimate_delta_angle = _sensor_hander.estimate_delta_angle - delta_angle;
+	/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
+	s32q14_t est_el_speed = ((next_delta_angle - _sensor_hander.estimate_delta_angle)<<9)/delta_angle * _sensor_hander.el_speed;
+	if (_sensor_hander.trns_detect) { //s32q14
+		est_el_speed = ((next_delta_angle - _sensor_hander.estimate_delta_angle)<<9)/delta_angle * _sensor_hander.immediately_el_speed;
+	}
+	//s32q5
+	_sensor_hander.estimate_el_speed = est_el_speed >> 9;
+	_sensor_hander.next_delta_angle = next_delta_angle;
+	//_sensor_hander.measured_el_angle = theta_now;
+	os_enable_irq();
+	_sensor_hander.hall_stat = hall_stat_now;
+	_sensor_hander.hall_ticks = hall_ticks_now;
+	_sensor_hander.rpm = _sensor_hander.el_speed / 360 * 60; //s32q5
+	
+	time_measure_end(&g_meas_hall);
+}
+
+

Applications/foc/motor/hall.h

@@ -0,0 +1,68 @@
+#ifndef _HALL_SENSOR_H__
+#define _HALL_SENSOR_H__
+#include "os/os_types.h"
+#include "bsp/bsp.h"
+#include "math/fix_math.h"
+
+#define NEGATIVE          (int8_t)-1
+#define POSITIVE          (int8_t)1
+
+//S32Q19 格式
+#define PHASE_0_DEGREE (0)
+#define PHASE_60_DEGREE (983040*32)
+#define PHASE_120_DEGREE (1966080*32)
+#define PHASE_180_DEGREE (2949120*32)
+#define PHASE_240_DEGREE (3932160*32)
+#define PHASE_300_DEGREE (4915200*32)
+#define PHASE_360_DEGREE (5898240*32)
+
+#define STATE_0 (uint8_t)0
+#define STATE_1 (uint8_t)1
+#define STATE_2 (uint8_t)2
+#define STATE_3 (uint8_t)3
+#define STATE_4 (uint8_t)4
+#define STATE_5 (uint8_t)5
+#define STATE_6 (uint8_t)6
+#define STATE_7 (uint8_t)7
+
+#define THETA_NONE        (float)0xFFFF
+#define SAMPLE_MAX_COUNT 3
+
+typedef struct {
+	u32        	ticks[SAMPLE_MAX_COUNT];
+	s32q19_t   	angles[SAMPLE_MAX_COUNT];
+	u32   		ticks_sum;
+	s32q19_t   	angles_sum;
+	u32   		index;
+	bool  		full;
+}hall_sample_t;
+
+typedef struct {
+	s32q19_t estimate_el_angle; //60度区间内的估计电角度
+	s32q19_t estimate_delta_angle;	
+	u32   estimate_time_ticks;	
+	s32q19_t   measured_el_angle; //hall测量到的电角度
+	s32q19_t next_delta_angle;
+	s32q5_t estimate_el_speed;
+	s32q5_t immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
+	s32q5_t el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
+	s32q5_t  rpm;        		//当前的电速度, 单位：RPM
+	bool  trns_detect;     //速度变化超过阈值
+	u8    hall_stat;
+	u32   hall_ticks;
+	s8    direction;
+	s32q19_t   phase_offset;
+	hall_sample_t samples;
+	u32  sensor_error;
+	s32q19_t  angle_table[8];
+}hall_sensor_t;
+
+void hall_sensor_init(void);
+void hall_sensor_clear(void);
+s16q5_t hall_sensor_get_theta(void); //return degree
+s32q5_t hall_sensor_get_speed(void); //return rpm;
+int hall_offset_increase(int inc);
+s32 *hall_get_table(void);
+
+#endif /* _HALL_SENSOR_H__ */
+

Applications/foc/motor/motor.c

@@ -1,6 +1,6 @@
 #include "foc/motor/motor.h"
 #include "foc/motor/current.h"
-#include "foc/core/foc_core.h"
+#include "foc/core/PMSM_FOC_Core.h"
 #include "foc/foc_config.h"
 #include "foc/samples.h"
 #include "math/fast_math.h"
@@ -82,7 +82,7 @@ void mc_hall_calibrate(s16 vd) {
 	PMSM_FOC_Start(OPEN_MODE);
 	pwm_start();
 	adc_start_convert();
-	PMSM_FOC_Set_Vdq(vd, 0);
+	PMSM_FOC_SetOpenVdq(vd, 0);
 	delay_ms(1000);
 
 	for (int i = 0; i < 5; i++) {

Applications/foc/motor/motor.h

@@ -1,7 +1,7 @@
 #ifndef _MOTOR_H__
 #define _MOTOR_H__
 #include "os/os_types.h"
-#include "foc/core/foc_type.h"
+#include "foc/core/PMSM_FOC_Core.h"
 
 void mc_init(void);
 bool mc_start(u8 mode);

Applications/foc/samples.c

@@ -4,7 +4,7 @@
 #include "math/fast_math.h"
 #include "math/fix_math.h"
 #include "os/os_task.h"
-
+#include "foc/foc_config.h"
 
 typedef struct {
 	float value;
@@ -21,8 +21,8 @@ static samples_t _vbus;
 static samples_t _throttle;
 
 void samples_init(void){
-	_vbus.filted_value = (MAX_VBUS_VOLTAGE);
-	_vbus.value = (MAX_VBUS_VOLTAGE);
+	_vbus.filted_value = (MAX_vDC);
+	_vbus.value = (MAX_vDC);
 	_vbus.lowpass = (0.2f); 
 
 	_throttle.filted_value = (0);

Applications/libs/time_measure.c

@@ -3,7 +3,7 @@
 #include "os/os_task.h"
 #include "libs/time_measure.h"
 
-#define COUNT_2_US(c) (c/120)
+#define COUNT_2_US(c) (c)
 
 u32 time_delta_us(u32 count, u32 *p_update) {
 	u32 now = task_ticks_abs();

Applications/math/fast_math.h

@@ -8,6 +8,11 @@
 #define SQRT3                   (1.73205080757f)
 #define SQRT2_BY_SQRT3          (0.8164966f)
 #define M_PI (3.14159265f)
+
+#define ONE_BY_SQRT3_Q14  (9459) //0.57735026919 * 16384.0F
+#define SQRT3_BY_2_Q14    (14189)//0.86602540378 * 16384.0F
+#define TWO_BY_SQRT3_Q14  (28378)
+
 #ifndef SQ
 #define SQ(x) ((x)*(x))
 #endif

Applications/math/fix_math.h

@@ -3,30 +3,32 @@
 #include "bsp/bsp.h"
 #include "os/os_types.h"
 #include "libs/utils.h"
+
 typedef signed short s16q14_t;
 typedef signed short s16q10_t;
 typedef signed short s16q5_t;
-typedef signed short s16q4_t;
-typedef signed int   s32q4_t;
-typedef signed int   s32q14_t;
-
+//typedef signed short s16q4_t;
+typedef int32_t   s32q4_t;
+typedef int32_t   s32q14_t;
+typedef int32_t   s32q5_t;
+typedef int32_t   s32q19_t;
 
-#define S16Q4(A) (signed short)((A) * 16.0F)
+//#define S16Q4(A) (signed short)((A) * 16.0F)
 #define S16Q5(A) (signed short)((A) * 32.0F)
 #define S16Q10(A) (signed short)((A) * 1024.0F)
 #define S16Q14(A) (signed short)((A) * 16384.0F)
 #define S32Q4(A) (signed int)((A) * 16.0F)
 #define S32Q14(A) (signed int)((A) * 16384.0F)
 #define S32Q14_MUL(a, b) (((a)*(b)) >>14)
-
-
+#define S16_mul(a, b, q) (((u32)(a)*(b)) >> q) 
+/*
 static __INLINE float S16Q4toF(s16q4_t v) {
 	s16 num = (v >> 4) & 0xFFFF;
 	u16 tail = v & 0x000F;
 	float f = num + (float)tail / 16.0f;
 	return f;
 } 
-
+*/
 static __INLINE float S16Q5toF(s16q5_t v) {
 	s16 num = (v >> 5) & 0xFFFF;
 	u16 tail = v & 0x001F;
@@ -50,4 +52,30 @@ static __INLINE float S32Q4toF(s32q4_t v) {
 
 #define MATH_sat(in, minOut, maxOut) (min((in), MAX((in), (minOut))))
 
+static __INLINE u16 Sqrt_Fix( u32 wInput )
+{
+	s32 wtemprootnew;
+
+	u8 biter = 0u;
+	s32 wtemproot;
+	if ( wInput <= ( s32 )2097152 ) {
+      	wtemproot = ( s32 )128;
+    } else {
+      	wtemproot = ( s32 )8192;
+    }
+
+    do {
+      	wtemprootnew = ( wtemproot + wInput / wtemproot ) / ( s32 )2;
+      	if ( wtemprootnew == wtemproot ) {
+        	biter = 6u;
+      	} else {
+        	biter ++;
+        	wtemproot = wtemprootnew;
+      	}
+    } while ( biter < 6u );
+  	return ( wtemprootnew );
+}
+
+void SinCos_Lut(s16q5_t angle, s16q14_t *s, s16q14_t *c);
+
 #endif /* _FIX_MATH_H__ */

Applications/math/sin_table.c

@@ -0,0 +1,63 @@
+#include "os/os_types.h"
+#include "math/fix_math.h"
+/*
+1.cos和sin转换公式一
+
+sin[(pi/2)-x]=cosx；
+
+cos[(pi/2)-x]=sinx；
+
+2.cos和sin转换公式二
+
+cos[(pi/2)+x]=-sinx；
+
+sin[(pi/2)+x]=cosx；
+*/
+static s16 sinTable[] = 
+{ 0, 286, 572, 857, 1143, 1428, 1713, 1997, 2280, 2563, 2845, 3126, 3406, 3686,
+  3964, 4240, 4516, 4790, 5063, 5334, 5604, 5872, 6138, 6402, 6664, 6924, 7182,
+  7438, 7692, 7943, 8192, 8438, 8682, 8923, 9162, 9397, 9630, 9860, 10087,
+  10311, 10531, 10749, 10963, 11174, 11381, 11585, 11786, 11982, 12176, 12365,
+  12551, 12733, 12911, 13085, 13255, 13421, 13583, 13741, 13894, 14044, 14189,
+  14330, 14466, 14598, 14726, 14849, 14968, 15082, 15191, 15296, 15396, 15491,
+  15582, 15668, 15749, 15826, 15897, 15964, 16026, 16083, 16135, 16182, 16225,
+  16262, 16294, 16322, 16344, 16362, 16374, 16382, 16384, 16382, 16374, 16362,
+  16344, 16322, 16294, 16262, 16225, 16182, 16135, 16083, 16026, 15964, 15897,
+  15826, 15749, 15668, 15582, 15491, 15396, 15296, 15191, 15082, 14968, 14849,
+  14726, 14598, 14466, 14330, 14189, 14044, 13894, 13741, 13583, 13421, 13255,
+  13085, 12911, 12733, 12551, 12365, 12176, 11982, 11786, 11585, 11381, 11174,
+  10963, 10749, 10531, 10311, 10087, 9860, 9630, 9397, 9162, 8923, 8682, 8438,
+  8192, 7943, 7692, 7438, 7182, 6924, 6664, 6402, 6138, 5872, 5604, 5334, 5063,
+  4790, 4516, 4240, 3964, 3686, 3406, 3126, 2845, 2563, 2280, 1997, 1713, 1428,
+  1143, 857, 572, 286, 0, -286, -572, -857, -1143, -1428, -1713, -1997, -2280,
+  -2563, -2845, -3126, -3406, -3686, -3964, -4240, -4516, -4790, -5063, -5334,
+  -5604, -5872, -6138, -6402, -6664, -6924, -7182, -7438, -7692, -7943, -8192,
+  -8438, -8682, -8923, -9162, -9397, -9630, -9860, -10087, -10311, -10531,
+  -10749, -10963, -11174, -11381, -11585, -11786, -11982, -12176, -12365,
+  -12551, -12733, -12911, -13085, -13255, -13421, -13583, -13741, -13894,
+  -14044, -14189, -14330, -14466, -14598, -14726, -14849, -14968, -15082,
+  -15191, -15296, -15396, -15491, -15582, -15668, -15749, -15826, -15897,
+  -15964, -16026, -16083, -16135, -16182, -16225, -16262, -16294, -16322,
+  -16344, -16362, -16374, -16382, -16384, -16382, -16374, -16362, -16344,
+  -16322, -16294, -16262, -16225, -16182, -16135, -16083, -16026, -15964,
+  -15897, -15826, -15749, -15668, -15582, -15491, -15396, -15296, -15191,
+  -15082, -14968, -14849, -14726, -14598, -14466, -14330, -14189, -14044,
+  -13894, -13741, -13583, -13421, -13255, -13085, -12911, -12733, -12551,
+  -12365, -12176, -11982, -11786, -11585, -11381, -11174, -10963, -10749,
+  -10531, -10311, -10087, -9860, -9630, -9397, -9162, -8923, -8682, -8438,
+  -8192, -7943, -7692, -7438, -7182, -6924, -6664, -6402, -6138, -5872, -5604,
+  -5334, -5063, -4790, -4516, -4240, -3964, -3686, -3406, -3126, -2845, -2563,
+  -2280, -1997, -1713, -1428, -1143, -857, -572, -286, 0 };
+
+void SinCos_Lut(s16q5_t angle, s16q14_t *s, s16q14_t *c) {
+	u16 angle_degree = angle >> 5; //去掉小数部分
+	if ((angle & 0x1F) > 16) {
+		angle_degree += 1;
+	}
+	angle_degree = angle_degree % 360;
+	*s = sinTable[angle_degree];
+	//cosx = sin[(pi/2)+x]
+	angle_degree = (angle_degree + 90) % 360;
+	*c = sinTable[angle_degree];
+}
+

Project/MC100.uvoptx

@@ -251,8 +251,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_core.c</PathWithFileName>
-      <FilenameWithoutPath>foc_core.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\ramp_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -263,8 +263,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\ramp_ctrl.c</PathWithFileName>
-      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\commands.c</PathWithFileName>
+      <FilenameWithoutPath>commands.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -275,8 +275,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\commands.c</PathWithFileName>
-      <FilenameWithoutPath>commands.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\samples.c</PathWithFileName>
+      <FilenameWithoutPath>samples.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -287,8 +287,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\samples.c</PathWithFileName>
-      <FilenameWithoutPath>samples.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\e_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>e_ctrl.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -299,8 +299,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_wapper.c</PathWithFileName>
-      <FilenameWithoutPath>foc_wapper.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\svpwm.c</PathWithFileName>
+      <FilenameWithoutPath>svpwm.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -311,8 +311,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\e_ctrl.c</PathWithFileName>
-      <FilenameWithoutPath>e_ctrl.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\PMSM_FOC_Core.c</PathWithFileName>
+      <FilenameWithoutPath>PMSM_FOC_Core.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -320,7 +320,7 @@
 
   <Group>
     <GroupName>Motor</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -348,35 +348,15 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
-  </Group>
-
-  <Group>
-    <GroupName>Simulink</GroupName>
-    <tvExp>0</tvExp>
-    <tvExpOptDlg>0</tvExpOptDlg>
-    <cbSel>0</cbSel>
-    <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>4</GroupNumber>
+      <GroupNumber>3</GroupNumber>
       <FileNumber>12</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller.c</PathWithFileName>
-      <FilenameWithoutPath>PMSM_Controller.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>4</GroupNumber>
-      <FileNumber>13</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller_data.c</PathWithFileName>
-      <FilenameWithoutPath>PMSM_Controller_data.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\motor\hall.c</PathWithFileName>
+      <FilenameWithoutPath>hall.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -389,8 +369,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>5</GroupNumber>
-      <FileNumber>14</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>13</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -401,8 +381,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>5</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>14</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -413,8 +393,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>5</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>15</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -425,8 +405,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>5</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -440,13 +420,13 @@
 
   <Group>
     <GroupName>Math</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -456,6 +436,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>18</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\math\sin_table.c</PathWithFileName>
+      <FilenameWithoutPath>sin_table.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -465,7 +457,7 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -477,7 +469,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -489,7 +481,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -501,7 +493,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -513,7 +505,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -525,7 +517,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -537,7 +529,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -549,7 +541,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -561,7 +553,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -573,7 +565,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -585,7 +577,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -597,7 +589,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -609,7 +601,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -621,7 +613,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
+      <GroupNumber>6</GroupNumber>
       <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -641,7 +633,7 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>8</GroupNumber>
+      <GroupNumber>7</GroupNumber>
       <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -653,7 +645,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
+      <GroupNumber>7</GroupNumber>
       <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -665,7 +657,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
+      <GroupNumber>7</GroupNumber>
       <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -677,7 +669,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
+      <GroupNumber>7</GroupNumber>
       <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -689,7 +681,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
+      <GroupNumber>7</GroupNumber>
       <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -701,7 +693,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
+      <GroupNumber>7</GroupNumber>
       <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -721,7 +713,7 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>9</GroupNumber>
+      <GroupNumber>8</GroupNumber>
       <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -733,7 +725,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
+      <GroupNumber>8</GroupNumber>
       <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -745,7 +737,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
+      <GroupNumber>8</GroupNumber>
       <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -765,7 +757,7 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -777,7 +769,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -789,7 +781,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -801,7 +793,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -813,7 +805,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -825,7 +817,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -837,7 +829,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -849,7 +841,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -861,7 +853,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -873,7 +865,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -885,7 +877,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -897,7 +889,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -909,7 +901,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -921,7 +913,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -933,7 +925,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -945,7 +937,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
+      <GroupNumber>9</GroupNumber>
       <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -965,7 +957,7 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>11</GroupNumber>
+      <GroupNumber>10</GroupNumber>
       <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
@@ -977,7 +969,7 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>11</GroupNumber>
+      <GroupNumber>10</GroupNumber>
       <FileNumber>59</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>

Project/MC100.uvprojx

@@ -403,11 +403,6 @@
         <Group>
           <GroupName>Foc</GroupName>
           <Files>
-            <File>
-              <FileName>foc_core.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\foc_core.c</FilePath>
-            </File>
             <File>
               <FileName>ramp_ctrl.c</FileName>
               <FileType>1</FileType>
@@ -424,14 +419,19 @@
               <FilePath>..\Applications\foc\samples.c</FilePath>
             </File>
             <File>
-              <FileName>foc_wapper.c</FileName>
+              <FileName>e_ctrl.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\e_ctrl.c</FilePath>
+            </File>
+            <File>
+              <FileName>svpwm.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\foc_wapper.c</FilePath>
+              <FilePath>..\Applications\foc\core\svpwm.c</FilePath>
             </File>
             <File>
-              <FileName>e_ctrl.c</FileName>
+              <FileName>PMSM_FOC_Core.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\e_ctrl.c</FilePath>
+              <FilePath>..\Applications\foc\core\PMSM_FOC_Core.c</FilePath>
             </File>
           </Files>
         </Group>
@@ -448,20 +448,10 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\motor\motor.c</FilePath>
             </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Simulink</GroupName>
-          <Files>
-            <File>
-              <FileName>PMSM_Controller.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller.c</FilePath>
-            </File>
             <File>
-              <FileName>PMSM_Controller_data.c</FileName>
+              <FileName>hall.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller_data.c</FilePath>
+              <FilePath>..\Applications\foc\motor\hall.c</FilePath>
             </File>
           </Files>
         </Group>
@@ -498,6 +488,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\math\fast_math.c</FilePath>
             </File>
+            <File>
+              <FileName>sin_table.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\math\sin_table.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>