加入e_ctrl.c 控制加速，电子刹车，能量回收级别

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

Applications/app/app.c

@@ -29,7 +29,7 @@ void app_start(void){
 }
 
 static void _can_report_info(void) {
-	can_report_speed(0x45, mc_get_speed());
+	can_report_speed(0x45, PMSM_FOC_GetSpeed());
 }
 
 static void print_backtrace(void){

Applications/bsp/bsp.h

@@ -76,9 +76,6 @@
 #define ENABLE_AUX_TIMER 1
 
 
-#define THROTTLE_LOW_VALUE 0.2f
-#define THROTTLE_MAX_VALUE 4.9f
-#define THROTTLE_MIN_RPM   10
 void bsp_init(void);
 void wdog_reload(void);
 void system_reboot(void);

Applications/bsp/gpio.c

@@ -16,3 +16,7 @@ void gpio_pin_init(void){
 }
 
 
+bool gpio_get_brake(void) {
+	return 0;
+}
+

Applications/bsp/gpio.h

@@ -4,7 +4,7 @@
 #define _GPIO_PIN_H__
 
 #include "bsp.h"
-
+#include "os/os_types.h"
 typedef struct {
 	uint32_t group;
 	uint32_t pin;
@@ -14,6 +14,7 @@ typedef struct {
 }gpio_pin_config_t;
 
 void gpio_pin_init(void);
+bool gpio_get_brake(void) ;
 
 
 #endif /* _GPIO_PIN_H__ */

Applications/foc/core/e_ctrl.c

@@ -0,0 +1,133 @@
+#include "e_ctrl.h"
+#include "foc/foc_config.h"
+
+static e_Ctrl g_eCtrl;
+
+void eCtrl_init(u16 ebrk_time, u16 accl_time){
+	g_eCtrl.ebrk_time = ebrk_time;
+	g_eCtrl.accl_time = accl_time;
+	if (g_eCtrl.accl_time == 0) {
+		g_eCtrl.accl_time = DEFAULT_D_TIME;
+	}
+	if (g_eCtrl.ebrk_time == 0) {
+		g_eCtrl.ebrk_time = DEFAULT_D_TIME;
+	}
+	ramp_ctrl_init(&g_eCtrl.torque_ramp);
+	ramp_ctrl_init(&g_eCtrl.speed_ramp);
+
+	ramp_set_points(&g_eCtrl.torque_ramp, THROTTLE_MIN_IDQ, THROTTLE_MIN_IDQ);
+	ramp_set_step_value(&g_eCtrl.torque_ramp, TORQUE_STEP);
+
+	ramp_set_points(&g_eCtrl.speed_ramp, THROTTLE_MIN_RPM, THROTTLE_MIN_RPM);
+	ramp_set_step_value(&g_eCtrl.speed_ramp, SPEED_STEP);
+}
+
+
+void eCtrl_set_TrqCurrent(float c) {
+	ramp_t *ramp = &g_eCtrl.torque_ramp;
+	float c_now = ramp_get_interpolation(ramp);
+	float step_val = TORQUE_STEP;
+	int   sign = 1;
+
+	if (c < c_now) {
+		sign = -1;
+	}
+	u32 step_count = 1; 
+	u32 step_ms = eCTRL_STEP_TS;	
+	if (sign > 0) { //增加扭矩
+		step_count = (c - c_now)/TORQUE_STEP + 1;
+		step_ms = g_eCtrl.accl_time / step_count;	
+		if (step_ms < eCTRL_STEP_TS) {
+			step_ms = eCTRL_STEP_TS;
+			step_val = (c - c_now)/(g_eCtrl.accl_time / step_ms);
+		}
+	}else if (sign < 0) {
+		step_count = (c_now - c)/TORQUE_STEP + 1;
+		step_ms = g_eCtrl.ebrk_time / step_count;
+		if (step_ms < eCTRL_STEP_TS) {
+			step_ms = eCTRL_STEP_TS;
+			step_val = (c_now - c)/(g_eCtrl.ebrk_time / step_ms);
+		}		
+	}
+	ramp_set_step_value(ramp, sign * step_val);
+	ramp_set_step_time(ramp, step_ms);
+	ramp_set_target(ramp, c);
+	
+	ramp_exc(ramp);
+}
+
+
+void eCtrl_set_TargetSpeed(float s) {
+	ramp_t *ramp = &g_eCtrl.speed_ramp;
+	float s_now = ramp_get_interpolation(ramp);
+	float step_val = SPEED_STEP;
+	int   sign = 1;
+
+	if (s < s_now) {
+		sign = -1;
+	}
+	u32 step_count = 1; 
+	u32 step_ms = eCTRL_STEP_TS;	
+	if (sign > 0) { //加速
+		step_count = (s - s_now)/SPEED_STEP + 1;
+		step_ms = g_eCtrl.accl_time / step_count;	
+		if (step_ms < eCTRL_STEP_TS) {
+			step_ms = eCTRL_STEP_TS;
+			step_val = (s - s_now)/(g_eCtrl.accl_time / step_ms);
+		}
+	}else if (sign < 0) {
+		step_count = (s_now - s)/SPEED_STEP + 1;
+		step_ms = g_eCtrl.ebrk_time / step_count;
+		if (step_ms < eCTRL_STEP_TS) {
+			step_ms = eCTRL_STEP_TS;
+			step_val = (s_now - s)/(g_eCtrl.ebrk_time / step_ms);
+		}		
+	}
+	ramp_set_step_value(ramp, sign * step_val);
+	ramp_set_step_time(ramp, step_ms);
+	ramp_set_target(ramp, s);
+	
+	ramp_exc(ramp);
+}
+
+
+
+float eCtrl_get_RefSpd(void) {
+	return ramp_get_interpolation(&g_eCtrl.speed_ramp);
+}
+
+float eCtrl_get_RefTorque(void) {
+	return ramp_get_interpolation(&g_eCtrl.torque_ramp);
+}
+
+
+float eCtrl_get_FinalSpd(void) {
+	return ramp_get_target(&g_eCtrl.speed_ramp);
+}
+
+float eCtrl_get_FinalTorque(void) {
+	return ramp_get_target(&g_eCtrl.torque_ramp);
+}
+
+void eCtrl_brake_signal(bool hw_brake) {
+	if (hw_brake != g_eCtrl.hw_brake) {
+		g_eCtrl.hw_brake = hw_brake;
+		if (hw_brake) {
+			g_eCtrl.brake_ts = shark_get_mseconds();
+		}
+	}
+	
+	if (g_eCtrl.hw_brake) {
+		float ebrk_torque = 0.0f;
+		float ebrk_speed = 0.0f;
+		if (shark_get_mseconds() - g_eCtrl.brake_ts >= eCTRL_Brake_TIME) {
+			if (g_eCtrl.accl_time != DEFAULT_D_TIME) {
+				ebrk_torque = eCTRL_NEG_TORQUE;
+			}
+		}
+		eCtrl_set_TrqCurrent(ebrk_torque);
+		eCtrl_set_TargetSpeed(ebrk_speed);
+	}
+}
+
+

Applications/foc/core/e_ctrl.h

@@ -0,0 +1,26 @@
+#ifndef EBRAKE_CTRL_H__
+#define EBRAKE_CTRL_H__
+#include "os/os_types.h"
+#include "foc/core/ramp_ctrl.h"
+
+
+typedef struct {
+	u16  ebrk_time; //能量回收，时间越短，刹车性能或者回收越好
+	u16  accl_time; //加速时间（ms），时间越短，加速性能越好
+	bool hw_brake;
+	u64  brake_ts;//检测到刹车开始时间
+	ramp_t torque_ramp;
+	ramp_t speed_ramp;
+}e_Ctrl;
+
+void eCtrl_init(u16 ebrk_time, u16 accl_time);
+void eCtrl_brake_signal(bool hw_brake);
+void eCtrl_set_TrqCurrent(float c);
+void eCtrl_set_TargetSpeed(float s);
+float eCtrl_get_RefSpd(void);
+float eCtrl_get_RefTorque(void);
+float eCtrl_get_FinalSpd(void);
+float eCtrl_get_FinalTorque(void);
+
+#endif /* EBRAKE_CTRL_H__ */
+

Applications/foc/core/foc_core.c

@@ -3,7 +3,9 @@
 #include "bsp/adc.h"
 #include "bsp/mc_hall_gpio.h"
 #include "foc/core/foc_core.h"
+#include "foc/core/e_ctrl.h"
 #include "foc/motor/current.h"
+#include "foc/foc_config.h"
 #include "bsp/timer_count32.h"
 #include "libs/time_measure.h"
 #include "libs/logger.h"
@@ -17,11 +19,12 @@ extern void PMSM_FOC_Step(void);
 
 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_iBusLimit(Default_DC_iLimit);
-	PMSM_FOC_SpeedLimit(Default_Spd_Limit);
+	PMSM_FOC_iBusLimit(S16Q5(Default_DC_iLimit));
+	PMSM_FOC_SpeedLimit(S32Q4(Default_Spd_Limit));
 }
 
 static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
@@ -32,9 +35,6 @@ static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
 	pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
 }
 
-static __INLINE bool PMSM_FOC_BrakeCtrl(void) {
-	return false;
-}
 
 static __INLINE void PMSM_FOC_Controller(void) {
 	u8  hall[3];
@@ -44,18 +44,13 @@ static __INLINE void PMSM_FOC_Controller(void) {
 	pmsm_foc.FOC_In->hall_A = hall[0];
 	pmsm_foc.FOC_In->hall_B = hall[1];
 	pmsm_foc.FOC_In->hall_C = hall[2];
-	if (!PMSM_FOC_BrakeCtrl()) {
-		if (pmsm_foc.FOC_In->n_ctrlModReq == SPD_MODE) {
-			pmsm_foc.FOC_In->spd_Target = i2sFix32E4((s16)ramp_get_target(&pmsm_foc.speed_ramp));
-			pmsm_foc.FOC_In->idq_Target = 0;
-		}else if(pmsm_foc.FOC_In->n_ctrlModReq == TRQ_MODE){
-			pmsm_foc.FOC_In->spd_Target = i2sFix32E4((s16)pmsm_foc.speed_ramp.final_point);
-			pmsm_foc.FOC_In->idq_Target = i2sFix5((s16)ramp_get_target(&pmsm_foc.current_ramp));
-		}
-	}else {
-		/*PWM 3相输出50%, 停止3相供电*/
-		pmsm_foc.FOC_In->n_ctrlModReq = OPEN_MODE;
-		pmsm_foc.FOC_In->spd_Target = 0;
+
+	if (pmsm_foc.FOC_In->n_ctrlModReq == 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_ctrlModReq == TRQ_MODE){
+		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_FinalSpd());
+		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_RefTorque());
 	}
 	
 	PMSM_FOC_Step();
@@ -86,38 +81,39 @@ void PMSM_FOC_Stop(void) {
 	memset(pmsm_foc.FOC_In, 0, sizeof(ExtU));
 }
 
-void PMSM_FOC_iBusLimit(int16_T ibusLimit) {
-	pmsm_foc.FOC_In->iDC_Limit = i2sFix5(ibusLimit);
+void PMSM_FOC_iBusLimit(_s16q5_t ibusLimit) {
+	pmsm_foc.FOC_In->iDC_Limit = (ibusLimit);
 }
 
-void PMSM_FOC_SpeedLimit(int16_T speedLimit) {
-	pmsm_foc.FOC_In->spd_Limit = i2sFix32E4(speedLimit);
+void PMSM_FOC_SpeedLimit(_s32q4_t speedLimit) {
+	pmsm_foc.FOC_In->spd_Limit = (speedLimit);
 }
 
-s16 PMSM_FOC_GetSpeedLimit(void) {
-	return sFix32E4_s16(pmsm_foc.FOC_In->spd_Limit);
+float PMSM_FOC_GetSpeedLimit(void) {
+	return S32Q4toF(pmsm_foc.FOC_In->spd_Limit);
 }
 
-void PMSM_FOC_VbusVoltage(int16_T vbusVol) {
-	pmsm_foc.FOC_In->vDC = i2sFix5(vbusVol);
+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_ctrlModReq = mode;
 }
 
-void PMSM_FOC_SetOpenVdq(int16_T vd, int16_T vq) {
-	pmsm_foc.FOC_In->vdq_Open[0] = i2sFix5(vd);
-	pmsm_foc.FOC_In->vdq_Open[1] = i2sFix5(vq);
+void PMSM_FOC_SetOpenVdq(_s16q5_t vd, _s16q5_t vq) {
+	pmsm_foc.FOC_In->vdq_Open[0] = (vd);
+	pmsm_foc.FOC_In->vdq_Open[1] = (vq);
 }
 
 bool PMSM_FOC_EnableCruise(boolean_T enable) {
 	if (enable) {
-		if (sFix32E4_s16(pmsm_foc.FOC_Out->f_MotRPM) < 100) { //
+		float motSpd = PMSM_FOC_GetSpeed();
+		if (motSpd < MIN_CRUISE_RPM) { //
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), pmsm_foc.FOC_Out->f_MotRPM, 500);
+		eCtrl_set_TargetSpeed(motSpd);
 	}
 	pmsm_foc.FOC_In->b_cruiseEna = enable;
 	return true;
@@ -127,31 +123,29 @@ bool PMSM_FOC_Is_CruiseEnabled(void) {
 	return (pmsm_foc.FOC_In->b_cruiseEna && (pmsm_foc.FOC_Out->n_runingMode == SPD_MODE));
 }
 
-bool PMSM_FOC_Set_Speed(s16 rpm, u32 ramp) {
-	if (!pmsm_foc.FOC_In->b_cruiseEna) {
-		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, ramp);
+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(s16 current, u32 ramp) {
-	if (pmsm_foc.FOC_In->n_ctrlModReq != TRQ_MODE) {
-		return false;
-	}
-	ramp_set_target(&pmsm_foc.current_ramp, ramp_get_target(&pmsm_foc.current_ramp), current, ramp);
+bool PMSM_FOC_Set_Current(float current) {
+	eCtrl_set_TrqCurrent(current);
 	return true;
 }
 
-bool PMSM_FOC_Set_CruiseSpeed(s16 rpm) {
+bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	if (PMSM_FOC_Is_CruiseEnabled()) {
-		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, 0);
+		eCtrl_set_TargetSpeed(rpm);
 		return true;
 	}
 	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
 	return false;
 }
 
-void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, int16_T open_vd) {
+void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, _s16q5_t open_vd) {
 	uint16_T foc_cali = 0;
 	if (b_caliHall) {
 		foc_cali = FOC_CALIMOD_HALL;
@@ -165,13 +159,13 @@ void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, int16_T open_vd) {
 	pmsm_foc.FOC_In->FOC_Flags &= ~(FOC_CALIMOD_HALL);
 	pmsm_foc.FOC_In->b_motEna = b_caliHall;
 	pmsm_foc.FOC_In->FOC_Flags |= foc_cali;
-	pmsm_foc.FOC_In->vdq_Open[0] = i2sFix5(open_vd);
+	pmsm_foc.FOC_In->vdq_Open[0] = (open_vd);
 	pmsm_foc.FOC_In->vdq_Open[1] = 0;
 	pmsm_foc.FOC_In->n_ctrlModReq = OPEN_MODE;
 }
 
-s16 PMSM_FOC_GetSpeed(void) {
-	return sFix32E4_s16(pmsm_foc.FOC_Out->f_MotRPM);
+float PMSM_FOC_GetSpeed(void) {
+	return S32Q4toF(pmsm_foc.FOC_Out->f_MotRPM);
 }
 
 
@@ -184,13 +178,19 @@ u8 PMSM_FOC_GetErrCode(void) {
 	return pmsm_foc.error_code;
 }
 
-void foc_brake_handler(bool brake) {
+void PMSM_FOC_Brake(bool brake) {
 	pmsm_foc.b_brake_in = brake;
 	if (pmsm_foc.b_brake_in & pmsm_foc.FOC_In->b_cruiseEna) {
 		pmsm_foc.FOC_In->b_cruiseEna = false;
 	}
+	eCtrl_brake_signal(brake);
 }
 
+void foc_brake_handler(bool brake){
+	PMSM_FOC_Brake(brake);
+}
+
+
 void foc_pwm_up_handler(void){
 	phase_current_adc_triger();
 }

Applications/foc/core/foc_core.h

@@ -8,20 +8,22 @@
 void PMSM_FOC_CoreInit(void);
 void PMSM_FOC_Start(u8 nCtrlMode);
 void PMSM_FOC_Stop(void);
-void PMSM_FOC_iBusLimit(int16_T ibusLimit);
-void PMSM_FOC_SpeedLimit(int16_T speedLimit);
-s16 PMSM_FOC_GetSpeedLimit(void);
-void PMSM_FOC_VbusVoltage(int16_T vbusVol);
+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(int16_T vd, int16_T vq);
+void PMSM_FOC_SetOpenVdq(_s16q5_t vd, _s16q5_t vq);
 bool PMSM_FOC_EnableCruise(boolean_T enable);
-bool PMSM_FOC_Set_Speed(s16 rpm, u32 ramp);
-bool PMSM_FOC_Set_CruiseSpeed(s16 rpm);
-void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, int16_T open_vd);
-s16 PMSM_FOC_GetSpeed(void);
+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);
 
 #endif /* _FOC_CORE_H__ */
 

Applications/foc/core/foc_type.h

@@ -79,6 +79,7 @@ typedef struct _foc {
 	ramp_t 		speed_ramp;
 	ramp_t      current_ramp;
 	u8          error_code;
+	u8          ebrake_level;
 	bool   		b_brake_in;
 	bool        b_FocEna;
 	ExtU 		*FOC_In;                       /* External inputs */
@@ -98,9 +99,6 @@ typedef struct _foc {
 #define A_mA(a) ((a*1000))
 #define mA_A(ma) (((float)(ma))/1000.0f)
 
-#define RPM_FOR_CLOSE_LOOP 30.0F
-#define MAX_SPEED_RPM 3000
-
 #if 1
 #define SECTOR_1  0u
 #define SECTOR_2  1u

Applications/foc/core/ramp_ctrl.c

@@ -1,6 +1,8 @@
 #include "ramp_ctrl.h"
 
 #define RAMP_INTVAL 50 //ms
+#define min_step 0.5f
+
 void ramp_timer_handler(shark_timer_t *timer);
 
 void ramp_ctrl_init(ramp_t *ramp){
@@ -11,43 +13,89 @@ void ramp_ctrl_init(ramp_t *ramp){
 void ramp_clear(ramp_t *ramp) {
 	shark_timer_cancel(&ramp->timer);
 	ramp->start_point = 0;
-	ramp->target = 0;
+	ramp->step_ms = RAMP_INTVAL;
+	ramp->interpolation = 0;
 	ramp->final_point = 0;
 	ramp->duration_ms = 0;
-	ramp->steps = 0;
+	ramp->step_val = 0;
+}
+
+void ramp_set_points(ramp_t *ramp, float start, float target) {
+	ramp->start_point = start;
+	ramp->final_point = target;
+	ramp->interpolation = target;
+}
+
+
+void ramp_set_target(ramp_t *ramp, float target) {
+	ramp->final_point = target;
+}
+
+float ramp_get_target(ramp_t *ramp){
+	return ramp->final_point;
+}
+
+
+void ramp_set_step_value(ramp_t *ramp, float step) {
+	ramp->step_val = step;
+}
+
+void ramp_set_step_time(ramp_t *ramp, u32 ms) {
+	ramp->step_ms = ms;
+}
+
+float ramp_get_interpolation(ramp_t *ramp) {
+	return ramp->interpolation;
+}
+
+void ramp_calc_step(ramp_t *ramp) {
+	float delta = fabs(ramp->final_point - ramp->start_point);
+	float steps = delta/(ramp->duration_ms / ramp->step_ms);
+	if (steps < min_step) {
+		u32 step_ms = min_step * ramp->duration_ms / delta;
+		ramp->step_val = min_step;
+		ramp->step_ms = step_ms;
+	}else {
+		ramp->step_val = steps;
+	}
+	if (ramp->final_point < ramp->start_point) {
+		ramp->step_val = - ramp->step_val;
+	}
 }
 
-void ramp_set_target(ramp_t *ramp, float start, float final, u32 duration_ms) {
+
+void ramp_set_target_duration(ramp_t *ramp, float start, float final, u32 duration_ms) {
 	shark_timer_cancel(&ramp->timer);
 	ramp->start_point = start;
 	ramp->final_point = final;
 	ramp->duration_ms = duration_ms;
 	
 	if (duration_ms == 0) {
-		ramp->steps = (final - ramp->start_point);
-		ramp->target = final;
+		ramp->step_val = (final - ramp->start_point);
+		ramp->interpolation = final;
 	}else {
-		ramp->steps = (final - ramp->start_point) / (duration_ms / RAMP_INTVAL);
+		ramp_calc_step(ramp);		
 		ramp_exc(ramp);
 	}
 }
 
+
+
 void ramp_exc(ramp_t *ramp){
-	shark_timer_post(&ramp->timer, RAMP_INTVAL);
+	if (shark_timer_stopped(&ramp->timer)) {
+		shark_timer_post(&ramp->timer, ramp->step_ms);
+	}
 }
 
-float ramp_get_target(ramp_t *ramp){
-	return ramp->target;
-}
 
 bool ramp_complete(ramp_t *ramp) {
-	return ramp->target == ramp->final_point;
+	return ramp->interpolation == ramp->final_point;
 }
 
 void ramp_timer_handler(shark_timer_t *timer) {
 	ramp_t *ramp = (ramp_t *)timer;
-	float target = ramp->target + ramp->steps;
-	if (ramp->steps < 0) {
+	float target = ramp->interpolation + ramp->step_val;
+	if (ramp->step_val < 0) {
 		if (target < ramp->final_point) {
 			target = ramp->final_point;
 		}
@@ -56,9 +104,9 @@ void ramp_timer_handler(shark_timer_t *timer) {
 			target = ramp->final_point;
 		}
 	}
-	ramp->target = target;
+	ramp->interpolation = target;
 	if (target != ramp->final_point) {
-		shark_timer_post(&ramp->timer, RAMP_INTVAL);
+		shark_timer_post(&ramp->timer, ramp->step_ms);
 	}else {
 		shark_timer_cancel(&ramp->timer);
 	}

Applications/foc/core/ramp_ctrl.h

@@ -5,9 +5,10 @@ typedef struct {
 	shark_timer_t timer;
 	float start_point;
 	float final_point;
-	float target;
+	float interpolation;
 	u32   duration_ms;
-	float steps;
+	u32   step_ms;
+	float step_val;
 }ramp_t;
 
 void ramp_ctrl_init(ramp_t *ramp);
@@ -15,7 +16,12 @@ void ramp_clear(ramp_t *ramp);
 void ramp_exc(ramp_t *ramp);
 float ramp_get_target(ramp_t *ramp);
 bool ramp_complete(ramp_t *ramp);
-void ramp_set_target(ramp_t *ramp, float start, float final, u32 duration_ms);
+void ramp_set_target_duration(ramp_t *ramp, float start, float final, u32 duration_ms);
+void ramp_set_points(ramp_t *ramp, float start, float target);
+void ramp_set_target(ramp_t *ramp, float target) ;
+void ramp_set_step_value(ramp_t *ramp, float step);
+void ramp_set_step_time(ramp_t *ramp, u32 ms) ;
+float ramp_get_interpolation(ramp_t *ramp);
 
 #endif /* _RAMP_CTRL_H__ */
 

Applications/foc/foc_config.h

@@ -0,0 +1,22 @@
+#ifndef _FOC_CONFIG_H__
+#define _FOC_CONFIG_H__
+
+/* 转把 */
+#define THROTTLE_LOW_VALUE 0.2f /* 转把最小值 */
+#define THROTTLE_MAX_VALUE 4.9f /* 转把最大值 */
+#define THROTTLE_MIN_RPM   10   /* 转把对应最小的速度 */
+#define THROTTLE_MIN_IDQ   20   /* 转把对应最小的扭矩电流 Q轴 */
+#define MAX_iDQ            150  /* 最大DQ轴电流*/
+
+
+#define MIN_CRUISE_RPM 1000     /* 能启动定速巡航的最小速度 */
+
+/* 电子刹车，动能回收，加速 */
+#define TORQUE_STEP 1.0f          /* 扭矩斜率给定的step值，单位 A */
+#define SPEED_STEP  20.0f         /* 速度斜率给定的step值，单位 RPM */
+#define DEFAULT_D_TIME 3000       /* 默认的斜率给定时间，越大，越慢到给定值*/
+#define eCTRL_STEP_TS 10          /* 斜率给定的step的时间值，单位 ms */
+#define eCTRL_Brake_TIME 1500     /* 捏住刹车的时间，超过这个时间启动ebrake，单位 ms */
+#define eCTRL_NEG_TORQUE (-100)   /* ebrake 的最大方向DQ电流，单位 ACKED_KEY*/
+#endif /* _FOC_CONFIG_H__ */
+

Applications/foc/motor/motor.c

@@ -1,6 +1,7 @@
 #include "foc/motor/motor.h"
 #include "foc/motor/current.h"
 #include "foc/core/foc_core.h"
+#include "foc/foc_config.h"
 #include "foc/samples.h"
 #include "math/fast_math.h"
 #include "bsp/delay.h"
@@ -9,7 +10,8 @@
 #include "bsp/pwm.h"
 
 static bool _motor_started = false;
-static sfix16En6_t _motor_throttle = 0;
+static float _motor_throttle = 0;
+
 bool mc_start(u8 mode) {
 	if (_motor_started) {
 		return true;
@@ -18,7 +20,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Param_Err);
 		return false;
 	}
-	if (mc_get_speed() > 10) {
+	if (PMSM_FOC_GetSpeed() > 10) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -41,7 +43,7 @@ bool mc_stop(void) {
 	if (!_motor_started) {
 		return true;
 	}
-	if (mc_get_speed() > 10) {
+	if (PMSM_FOC_GetSpeed() > 10) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -56,28 +58,28 @@ bool mc_stop(void) {
 	return true;
 }
 
-bool mc_target_speed(s16 rpm) {
-	return PMSM_FOC_Set_Speed(rpm, 1000);
-}
-
-bool mc_cruise_speed(s16 rpm) {
-	return PMSM_FOC_Set_CruiseSpeed(rpm);
-}
-
-bool mc_enable_cruise(boolean_T enable) {
-	return PMSM_FOC_EnableCruise(enable);
+bool mc_lock_motor(bool lock) {
+	if (lock && (PMSM_FOC_GetSpeed() > 10)) {
+		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+		return false;
+	}
+	if (lock) {
+		adc_stop_convert();
+		pwm_stop();
+	}else {
+		pwm_start();
+		adc_start_convert();
+	}
+	return true;
 }
 
-s16 mc_get_speed(void) {
-	return PMSM_FOC_GetSpeed();
-}
 
 bool mc_throttle_released(void) {
 	return get_throttle_float() < THROTTLE_LOW_VALUE;
 }
 
-s16 _get_speed_from_throttle(sfix16En6_t throttle) {
-	float f_throttle = sfix6toF(throttle);
+static float _get_speed_from_throttle(float throttle) {
+	float f_throttle = (throttle);
 	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
 		return 0;
 	}
@@ -88,13 +90,38 @@ s16 _get_speed_from_throttle(sfix16En6_t throttle) {
 	return (THROTTLE_MIN_RPM + PMSM_FOC_GetSpeedLimit() * ration);
 }
 
+static float _get_idq_from_throttle(float throttle) {
+	float f_throttle = (throttle);
+	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
+		return 0;
+	}
+	float delta = f_throttle - (THROTTLE_LOW_VALUE);
+
+	float ration = delta / (THROTTLE_MAX_VALUE - THROTTLE_LOW_VALUE);
+
+	return (THROTTLE_MIN_IDQ + MAX_iDQ * ration);
+}
+
+static void _brake_io_process(void) {
+	bool brake = gpio_get_brake();
+	for (int i = 0; i < 10; i++) {
+		if (brake != gpio_get_brake()) {
+			return;
+		}
+	}
+	PMSM_FOC_Brake(brake);
+}
+
 u32 mc_main_task(void *param) {
 	if (_motor_started) {
-		if (get_throttle_sfix6() != _motor_throttle) {
-			_motor_throttle = get_throttle_sfix6();
-			s16 speed_Ref = _get_speed_from_throttle(_motor_throttle);
-			mc_target_speed(speed_Ref);
+		if (get_throttle_float() != _motor_throttle) {
+			_motor_throttle = get_throttle_float();
+			float speed_Ref = _get_speed_from_throttle(_motor_throttle);
+			PMSM_FOC_Set_Speed(speed_Ref);
+			float torque_idq = _get_idq_from_throttle(_motor_throttle);
+			PMSM_FOC_Set_Current(torque_idq);
 		}
+		_brake_io_process();
 	}
 	return 0;
 }

Applications/foc/motor/motor.h

@@ -4,12 +4,8 @@
 #include "foc/core/foc_type.h"
 bool mc_start(u8 mode);
 bool mc_stop(void);
-bool mc_target_speed(s16 rpm);
-bool mc_cruise_speed(s16 rpm);
-bool mc_enable_cruise(boolean_T enable);
-s16 mc_get_speed(void);
 bool mc_throttle_released(void);
-
+bool mc_lock_motor(bool lock);
 
 #endif /* _MOTOR_H__ */
 

Applications/foc/samples.c

@@ -2,13 +2,15 @@
 #include "bsp/bsp.h"
 #include "bsp/adc.h"
 #include "math/fast_math.h"
-#include "os/os_task.h"
 #include "math/fix_math.h"
+#include "os/os_task.h"
+
 
 typedef struct {
-	sfix16En6_t value;
-	sfix16En6_t filted_value;
-	sfix16En10_t lowpass;
+	float value;
+	float filted_value;
+	float lowpass;
+	s16   fix_value;
 }samples_t;
 
 static void sample_vbus(void);
@@ -19,13 +21,13 @@ static samples_t _vbus;
 static samples_t _throttle;
 
 void samples_init(void){
-	_vbus.filted_value = i2sFix5(MAX_VBUS_VOLTAGE);
-	_vbus.value = i2sFix5(MAX_VBUS_VOLTAGE);
-	_vbus.lowpass = ftoSfix10(0.2f); 
+	_vbus.filted_value = (MAX_VBUS_VOLTAGE);
+	_vbus.value = (MAX_VBUS_VOLTAGE);
+	_vbus.lowpass = (0.2f); 
 
-	_throttle.filted_value = i2sFix5(0);
-	_throttle.value = i2sFix5(0);
-	_throttle.lowpass = ftoSfix10(0.2f); 
+	_throttle.filted_value = (0);
+	_throttle.value = (0);
+	_throttle.lowpass = (0.2f); 
 
 	sample_throttle();
 	sample_vbus();
@@ -33,20 +35,19 @@ void samples_init(void){
 	shark_task_create(sample_task, NULL);
 }
 
-s16 get_vbus_sfix6(void) {
-	return _vbus.filted_value;
-}
+
 
 float get_vbus_float(void) {
-	return sfix6toF(_vbus.filted_value);
+	return (float)((s32)(_vbus.filted_value * 100.0f)/100.0f);
 }
 
-float get_throttle_sfix6(void) {
-	return (_throttle.filted_value);
+s16 get_vbus_sfix5(void){
+	return _vbus.fix_value;
 }
 
+
 float get_throttle_float(void) {
-	return sfix6toF(_throttle.filted_value);
+	return (float)((s32)(_throttle.filted_value * 100.0f)/100.0f);
 }
 static u32 sample_task(void *param) {
 	sample_vbus();
@@ -56,18 +57,16 @@ static u32 sample_task(void *param) {
 
 static void sample_vbus(void){
 	s32 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 16);
-	s32 fix_vbus = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
-	s32 fix_filter = _vbus.filted_value;
-	LowPass_Filter(fix_filter, ftoSfix6(fix_vbus), _vbus.lowpass); //sfix6 * sfix10
-	_vbus.filted_value = fix_filter >> 10; // to sfix6
+	float fix_vbus = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
+	LowPass_Filter(_vbus.filted_value, (fix_vbus), _vbus.lowpass);
+	_vbus.fix_value = S16Q5toF(_vbus.filted_value);
 }
 
 static void sample_throttle(void){
 	s32 vadc = adc_sample_regular_channel(THROTTLE_CHAN, 16);
-	s32 fix_V = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
-	s32 V_filter = _throttle.filted_value;
-	LowPass_Filter(V_filter, ftoSfix6(fix_V), _vbus.lowpass); //sfix6 * sfix10
-	_throttle.filted_value = V_filter >> 10; // to sfix6
+	float fix_V = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
+	LowPass_Filter(_throttle.filted_value, (fix_V), _throttle.lowpass);
+	_throttle.fix_value = S16Q5toF(_throttle.filted_value);
 }
 
 

Applications/foc/samples.h

@@ -5,9 +5,9 @@
 #define MAX_GAS_VALUE 5000.0F
 
 void samples_init(void);
-s16 get_vbus_sfix6(void);
+s16 get_vbus_sfix5(void);
 float get_vbus_float(void);
-float get_throttle_sfix6(void);
+float get_throttle_sfix5(void);
 float get_throttle_float(void);
 
 #endif /* _SAMPLES_H__ */

Applications/math/fast_math.h

@@ -27,7 +27,7 @@ void normal_sincosf(float angle, float *sin, float *cos);
  * @param filter_constant
  * Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
  */
-#define LowPass_Filter(value, sample, filter_constant)	(value = value * (i2sFix10(1) - filter_constant) + sample * filter_constant)
+#define LowPass_Filter(value, sample, filter_constant)	(value = value * (1.0f - filter_constant) + sample * filter_constant)
 
 #endif /* _Fast_Math_H__ */
 

Applications/math/fix_math.h

@@ -2,46 +2,45 @@
 #define _FIX_MATH_H__
 #include "bsp/bsp.h"
 #include "os/os_types.h"
-typedef signed short sfix16En10_t;
-typedef signed short sfix16En6_t;
-typedef signed short sfix16En5_t;
-typedef signed short sfix16En4_t;
+typedef signed short _s16q10_t;
+typedef signed short _s16q5_t;
+typedef signed short _s16q4_t;
+typedef signed int   _s32q4_t;
 
-typedef int          sfix32En4_t;
+#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 S32Q4(A) (signed int)((A) * 16.0F)
 
-#define Fix10_6(x) (x >> 4)
-#define i2sFix6(x) ((sfix16En6_t)(x <<6))
-#define i2sFix5(x) ((sfix16En6_t)(x <<5))
-#define i2sFix4(x) ((sfix16En6_t)(x <<4))
-#define i2sFix10(x) ((sfix16En6_t)(x <<10))
 
-#define i2sFix32E4(x) ((sfix32En4_t)(x <<4))
-#define sFix32E4_s16(x) ((s16)(x >> 4))
 
-static __INLINE sfix16En10_t ftoSfix10(float v) {
-	s16 num = (s16)v;
-	float tail_f = v - num;
-	u8 tail_u8 = (u8)(tail_f / (1.0f / 1024.0f)); 
-
-	return (s16) ((num << 10) | (tail_u8 & 0x03FF));
-}
-
-
-static __INLINE float sfix6toF(sfix16En6_t v) {
-	s16 num = (v >> 6) & 0xFFFF;
-	u16 tail = v & 0x003F;
-	float f = num + (float)tail / 64.0f;
+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 sfix16En6_t ftoSfix6(float v) {
-	s16 num = (s16)v;
-	float tail_f = v - num;
-	u8 tail_u8 = (u8)(tail_f / (1.0f / 64.0f)); 
+static __INLINE float S16Q5toF(_s16q5_t v) {
+	s16 num = (v >> 5) & 0xFFFF;
+	u16 tail = v & 0x001F;
+	float f = num + (float)tail / 32.0f;
+	return f;
+} 
 
-	return (s16) ((num << 6) | (tail_u8 & 0x003F));
+static __INLINE float S16Q10toF(_s16q5_t v) {
+	s16 num = (v >> 10) & 0xFFFF;
+	u16 tail = v & 0x03FF;
+	float f = num + (float)tail / 1024.0f;
+	return f;
 }
 
+static __INLINE float S32Q4toF(_s32q4_t v) {
+	s32 num = (v >> 4) & 0xFFFFFFFF;
+	u16 tail = v & 0x000F;
+	float f = num + (float)tail / 16.0f;
+	return f;
+} 
 
 
 #endif /* _FIX_MATH_H__ */

Applications/os/os_task.h

@@ -21,6 +21,16 @@ typedef struct shark_timer {
 	u64 time;
 } shark_timer_t;
 
+static inline bool shark_timer_started(shark_timer_t *timer)
+{
+	return timer->next != timer;
+}
+
+static inline bool shark_timer_stopped(shark_timer_t *timer)
+{
+	return timer->next == timer;
+}
+
 void os_enable_irq(void);
 void os_disable_irq(void);
 

Applications/os/os_types.h

@@ -5,6 +5,7 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <stdbool.h>
+#include <math.h>
 
 #ifndef TRUE
 #define TRUE 1

Project/MC100.uvoptx

@@ -215,7 +215,7 @@
 
   <Group>
     <GroupName>Foc</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -279,6 +279,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>9</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\foc\core\e_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>e_ctrl.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -289,7 +301,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>9</FileNumber>
+      <FileNumber>10</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -301,7 +313,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>10</FileNumber>
+      <FileNumber>11</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -321,7 +333,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>11</FileNumber>
+      <FileNumber>12</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -333,7 +345,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>12</FileNumber>
+      <FileNumber>13</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -353,7 +365,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>13</FileNumber>
+      <FileNumber>14</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -365,7 +377,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>14</FileNumber>
+      <FileNumber>15</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -377,7 +389,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -389,7 +401,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -409,7 +421,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -429,7 +441,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -441,7 +453,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -453,7 +465,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -465,7 +477,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -477,7 +489,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -489,7 +501,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -501,7 +513,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -513,7 +525,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -525,7 +537,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -537,7 +549,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -549,7 +561,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -561,7 +573,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -573,7 +585,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -585,7 +597,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -605,7 +617,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -617,7 +629,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -629,7 +641,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -641,7 +653,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -653,7 +665,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -665,7 +677,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -685,7 +697,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -697,7 +709,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -709,7 +721,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -729,7 +741,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -741,7 +753,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -753,7 +765,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -765,7 +777,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -777,7 +789,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -789,7 +801,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -801,7 +813,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -813,7 +825,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -825,7 +837,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -837,7 +849,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -849,7 +861,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -861,7 +873,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -873,7 +885,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -885,7 +897,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -897,7 +909,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -909,7 +921,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -929,7 +941,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -941,7 +953,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/MC100.uvprojx

@@ -428,6 +428,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\foc_wapper.c</FilePath>
             </File>
+            <File>
+              <FileName>e_ctrl.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\e_ctrl.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.c

@@ -3,9 +3,9 @@
  *
  * Code generated for Simulink model 'PMSM_Controller'.
  *
- * Model version                  : 1.1445
+ * Model version                  : 1.1447
  * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
- * C/C++ source code generated on : Sat May 21 17:50:12 2022
+ * C/C++ source code generated on : Mon May 23 16:15:38 2022
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: ARM Compatible->ARM Cortex-M

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.h

@@ -3,9 +3,9 @@
  *
  * Code generated for Simulink model 'PMSM_Controller'.
  *
- * Model version                  : 1.1445
+ * Model version                  : 1.1447
  * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
- * C/C++ source code generated on : Sat May 21 17:50:12 2022
+ * C/C++ source code generated on : Mon May 23 16:15:38 2022
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: ARM Compatible->ARM Cortex-M

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller_data.c

@@ -3,9 +3,9 @@
  *
  * Code generated for Simulink model 'PMSM_Controller'.
  *
- * Model version                  : 1.1445
+ * Model version                  : 1.1447
  * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
- * C/C++ source code generated on : Sat May 21 17:50:12 2022
+ * C/C++ source code generated on : Mon May 23 16:15:38 2022
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: ARM Compatible->ARM Cortex-M

Simulink/PMSM_Controller_ert_rtw/ert_main.c

@@ -3,9 +3,9 @@
  *
  * Code generated for Simulink model 'PMSM_Controller'.
  *
- * Model version                  : 1.1445
+ * Model version                  : 1.1447
  * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
- * C/C++ source code generated on : Sat May 21 17:50:12 2022
+ * C/C++ source code generated on : Mon May 23 16:15:38 2022
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: ARM Compatible->ARM Cortex-M

Simulink/PMSM_Controller_ert_rtw/html/PMSM_Controller_c.html

@@ -21,11 +21,11 @@
 <tr name="5" id="5">
 <td><a id="l5" class='ln'>5</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="6" id="6">
-<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1445</span></td></tr>
+<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1447</span></td></tr>
 <tr name="7" id="7">
 <td><a id="l7" class='ln'>7</a></td><td><span class="ct"> * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020</span></td></tr>
 <tr name="8" id="8">
-<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Sat May 21 17:50:12 2022</span></td></tr>
+<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Mon May 23 16:15:38 2022</span></td></tr>
 <tr name="9" id="9">
 <td><a id="l9" class='ln'>9</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="10" id="10">

Simulink/PMSM_Controller_ert_rtw/html/PMSM_Controller_data_c.html

@@ -21,11 +21,11 @@
 <tr name="5" id="5">
 <td><a id="l5" class='ln'>5</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="6" id="6">
-<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1445</span></td></tr>
+<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1447</span></td></tr>
 <tr name="7" id="7">
 <td><a id="l7" class='ln'>7</a></td><td><span class="ct"> * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020</span></td></tr>
 <tr name="8" id="8">
-<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Sat May 21 17:50:12 2022</span></td></tr>
+<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Mon May 23 16:15:38 2022</span></td></tr>
 <tr name="9" id="9">
 <td><a id="l9" class='ln'>9</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="10" id="10">

Simulink/PMSM_Controller_ert_rtw/html/PMSM_Controller_h.html

@@ -21,11 +21,11 @@
 <tr name="5" id="5">
 <td><a id="l5" class='ln'>5</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="6" id="6">
-<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1445</span></td></tr>
+<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1447</span></td></tr>
 <tr name="7" id="7">
 <td><a id="l7" class='ln'>7</a></td><td><span class="ct"> * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020</span></td></tr>
 <tr name="8" id="8">
-<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Sat May 21 17:50:12 2022</span></td></tr>
+<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Mon May 23 16:15:38 2022</span></td></tr>
 <tr name="9" id="9">
 <td><a id="l9" class='ln'>9</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="10" id="10">

Simulink/PMSM_Controller_ert_rtw/html/PMSM_Controller_metrics.html

@@ -171,7 +171,7 @@ PMSM_Controller.c
 3,097
 </td>
 <td width="22%" align="right" valign="top" style="border-style: none">
-05/21/2022  5:50 PM
+05/23/2022  4:15 PM
 </td>
 
 </tr>
@@ -186,7 +186,7 @@ PMSM_Controller.h
 542
 </td>
 <td width="22%" align="right" valign="top" style="border-style: none">
-05/21/2022  5:50 PM
+05/23/2022  4:15 PM
 </td>
 
 </tr>
@@ -201,7 +201,7 @@ PMSM_Controller_data.c
 199
 </td>
 <td width="22%" align="right" valign="top" style="border-style: none">
-05/21/2022  5:50 PM
+05/23/2022  4:15 PM
 </td>
 
 </tr>
@@ -216,7 +216,7 @@ rtwtypes.h
 103
 </td>
 <td width="22%" align="right" valign="top" style="border-style: none">
-05/21/2022  5:50 PM
+05/23/2022  4:15 PM
 </td>
 
 </tr>
@@ -231,7 +231,7 @@ zero_crossing_types.h
 52
 </td>
 <td width="22%" align="right" valign="top" style="border-style: none">
-05/21/2022  5:50 PM
+05/23/2022  4:15 PM
 </td>
 
 </tr>

Simulink/PMSM_Controller_ert_rtw/html/PMSM_Controller_survey.html

@@ -39,7 +39,7 @@ kevin
 Model Version
 </td>
 <td align="left" valign="top">
-1.1445
+1.1447
 </td>
 
 </tr>
@@ -108,7 +108,7 @@ Simulink Coder Version
 Timestamp of Generated Source Code
 </td>
 <td align="left" valign="top">
-Sat May 21 17:50:12 2022
+Mon May 23 16:15:38 2022
 </td>
 
 </tr>

Simulink/PMSM_Controller_ert_rtw/html/ert_main_c.html

@@ -21,11 +21,11 @@
 <tr name="5" id="5">
 <td><a id="l5" class='ln'>5</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="6" id="6">
-<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1445</span></td></tr>
+<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1447</span></td></tr>
 <tr name="7" id="7">
 <td><a id="l7" class='ln'>7</a></td><td><span class="ct"> * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020</span></td></tr>
 <tr name="8" id="8">
-<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Sat May 21 17:50:12 2022</span></td></tr>
+<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Mon May 23 16:15:38 2022</span></td></tr>
 <tr name="9" id="9">
 <td><a id="l9" class='ln'>9</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="10" id="10">

Simulink/PMSM_Controller_ert_rtw/html/rtwtypes_h.html

@@ -21,11 +21,11 @@
 <tr name="5" id="5">
 <td><a id="l5" class='ln'>5</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="6" id="6">
-<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1445</span></td></tr>
+<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1447</span></td></tr>
 <tr name="7" id="7">
 <td><a id="l7" class='ln'>7</a></td><td><span class="ct"> * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020</span></td></tr>
 <tr name="8" id="8">
-<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Sat May 21 17:50:12 2022</span></td></tr>
+<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Mon May 23 16:15:38 2022</span></td></tr>
 <tr name="9" id="9">
 <td><a id="l9" class='ln'>9</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="10" id="10">

Simulink/PMSM_Controller_ert_rtw/html/zero_crossing_types_h.html

@@ -21,11 +21,11 @@
 <tr name="5" id="5">
 <td><a id="l5" class='ln'>5</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="6" id="6">
-<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1445</span></td></tr>
+<td><a id="l6" class='ln'>6</a></td><td><span class="ct"> * Model version                  : 1.1447</span></td></tr>
 <tr name="7" id="7">
 <td><a id="l7" class='ln'>7</a></td><td><span class="ct"> * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020</span></td></tr>
 <tr name="8" id="8">
-<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Sat May 21 17:50:12 2022</span></td></tr>
+<td><a id="l8" class='ln'>8</a></td><td><span class="ct"> * C/C++ source code generated on : Mon May 23 16:15:38 2022</span></td></tr>
 <tr name="9" id="9">
 <td><a id="l9" class='ln'>9</a></td><td><span class="ct"> *</span></td></tr>
 <tr name="10" id="10">

Simulink/PMSM_Controller_ert_rtw/rtwtypes.h

@@ -3,9 +3,9 @@
  *
  * Code generated for Simulink model 'PMSM_Controller'.
  *
- * Model version                  : 1.1445
+ * Model version                  : 1.1447
  * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
- * C/C++ source code generated on : Sat May 21 17:50:12 2022
+ * C/C++ source code generated on : Mon May 23 16:15:38 2022
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: ARM Compatible->ARM Cortex-M

Simulink/PMSM_Controller_ert_rtw/zero_crossing_types.h

@@ -3,9 +3,9 @@
  *
  * Code generated for Simulink model 'PMSM_Controller'.
  *
- * Model version                  : 1.1445
+ * Model version                  : 1.1447
  * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
- * C/C++ source code generated on : Sat May 21 17:50:12 2022
+ * C/C++ source code generated on : Mon May 23 16:15:38 2022
  *
  * Target selection: ert.tlc
  * Embedded hardware selection: ARM Compatible->ARM Cortex-M