更新代码

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

Application/App/app.c

@@ -1,14 +1,14 @@
 #include "app/app.h"
-#include "foc/foc.h"
+#include "foc/foc_stm.h"
 
 void app_init(void){
 
 }
 
 void start_stop_handler(void) {
-	FOCState s = FOC_STM_State();
+	FOCState s = foc_stm_state();
 	if (s == IDLE) {
-		Foc_Set_StartRamp(23.0f, 200);
+		foc_set_current_ramp(-20.0f);
 		foc_start_motor();
 	}else {
 		foc_stop_motor();

Application/App/app_api.c

@@ -0,0 +1,30 @@
+#include "app_api.h"
+
+foc_fault_t api_set_ready(bool ready) {
+	if (ready == foc_is_ready()) {
+		return foc_success;
+	}
+	normal_task_enable(false);
+	if (foc_stm_nextstate(ready?START:ANY_STOP) == NoError) {
+		foc_set_ready(ready);
+		normal_task_enable(true);
+		return foc_success;
+	}	
+	normal_task_enable(true);
+	return foc_not_allowed;
+}
+
+foc_fault_t api_set_speed(u16 rpm) {
+	normal_task_enable(false);
+	FOCState focs = foc_stm_state();
+	if (focs == IDLE || focs == ANY_STOP) {
+		normal_task_enable(true);
+		return foc_not_allowed;
+	}
+
+	foc_set_ref_speed(rpm);
+	normal_task_enable(true);
+	return foc_success;
+}
+
+

Application/App/app_api.h

@@ -0,0 +1,7 @@
+#ifndef _APP_API_H__
+#define _APP_API_H__
+
+
+
+#endif /*_APP_API_H__ */
+

Application/App/communication.c

@@ -1 +1,25 @@
+#include "communication.h"
+
+void protocol_recv_frame(u8 *data, u16 rx_len) {
+	if (rx_len < sizeof(cmd_frame_t)) {
+		return;
+	}
+	cmd_frame_t *head = (cmd_frame_t *)data;
+	switch (head->key) {
+		case Cmd_Start:
+			break;
+		case Cmd_Ready:
+			break;
+		case Cmd_Hall_Cali:
+			break;
+		case Cmd_Limit_Speed:
+			break;
+		case Cmd_Limit_Current:
+			break;
+		case Cmd_PI_Param:
+			break;
+		default:
+			break;		
+	}
+}
 

Application/App/communication.h

@@ -1,11 +1,29 @@
 #ifndef _COMMUNICATION_H__
 #define _COMMUNICATION_H__
+#include "libs/types.h"
 
 typedef enum {
 	Cmd_Start = 0x1, //start/stop
 	Cmd_Ready,       //ready / unready
 	Cmd_Hall_Cali,
+	Cmd_Limit_Speed,
+	Cmd_Limit_Current,
+	Cmd_PI_Param
 }motor_cmd_t;
+#define MY_CAN_ADDR 0x30
+#pragma  pack (push,1)  
+typedef struct
+{
+	uint8_t src:7;
+	uint8_t dir:1; //0->out, 1 ->in，如果发现收到的是0,可能是红外那边自环过来的，需要丢弃
+}cmd_head_t;
+
+typedef struct {
+	cmd_head_t head;
+	uint8_t key;
+	uint8_t data[0];
+}cmd_frame_t;
+#pragma pack(pop)
 
 #pragma  pack (push,1) 
 typedef struct {

Application/App/main.c

@@ -1,6 +1,6 @@
 #include "bsp/bsp.h"
 #include "libs/task.h"
-#include "foc/foc.h"
+#include "foc/foc_api.h"
 #include "app/app.h"
 int main(void){
 	system_init();

Application/Bsp/bsp.c

@@ -1,5 +1,5 @@
 #include "bsp/bsp.h"
-#include "foc/foc.h"
+#include "foc/foc_api.h"
 
 void system_init(void){
 	HAL_Init();
@@ -31,6 +31,10 @@ int get_hall_stat(int samples) {
 	return (h1 > tres) | ((h2 > tres) << 1) | ((h3 > tres) << 2);
 }
 
+void normal_task_enable(bool enable) {
+	TIM4_Enable_IRQ(enable);
+}
+
 void wdog_reload(void){
 #if CONFIG_DEBUG == 0
    

Application/Bsp/bsp.h

@@ -12,5 +12,6 @@ void system_reboot(void);
 int get_hall_stat(int samples);
 void wdog_reload(void);
 void wdog_enable(void);
+void normal_task_enable(bool enable);
 
 #endif /* __BSP_H__ */

Application/Bsp/serial.c

@@ -1,30 +1,170 @@
 #include "hal/uart2.h"
-#include "libs/circle_buffer.h"
+#include "libs/utils.h"
+#include "libs/task.h"
 #include "serial.h"
+static uart_t uart2;
 
-static c_buffer_t c_buff;
-static u8 _tx_buf[512];
-static u8 _rx_buf[256];
+static u32 uart_poll_task(void);
 
 void serial_init(void){
+	circle_buffer_init(&uart2.c_tx_buff , uart2.tx_buf, sizeof(uart2.tx_buf));
+	circle_buffer_init(&uart2.c_rx_buff , uart2.rx_buf, sizeof(uart2.rx_buf));
 	UART2_Init();
-	circle_buffer_init(&c_buff , _tx_buf, sizeof(_tx_buf));
+	task_start(uart_poll_task, 0);
 }
 
 void serial_write(u8 *data, int len){
-	circle_put_data(&c_buff, data, len);
+	circle_put_data(&uart2.c_tx_buff, data, len);
 	UART2_EnableTx();
 }
+__weak void protocol_recv_frame(u8 *data, u16 rx_len) {}
+
+static bool uart_on_rx_frame(uart_t *uart)
+{
+	u16 crc0 = shark_decode_u16(uart->rx_frame + uart->rx_length);
+	u16 crc1 = shark_crc16_check(uart->rx_frame, uart->rx_length);
+
+	if (crc0 != crc1) {
+		return false;
+	}
+	protocol_recv_frame((u8 *)uart->rx_frame, uart->rx_length);
+	return true;
+}
+
+
+static void uart_rx_poll(uart_t *uart) {
+	u8 data;
+	while(1) {
+		if (circle_get_one_data(&uart->c_rx_buff, &data) != 1) {
+			break;
+		}
+		
+		switch(data){
+			case CH_START:
+				uart->rx_length = 0;
+				uart->escape = false;
+				break;
+			case CH_END:
+				if (uart->rx_length > 2 && uart->rx_length != 0xFFFF){
+					uart->rx_length -= 2; //skip crc
+					uart_on_rx_frame(uart);
+				}
+				uart->rx_length = 0xFFFF;
+				break;
+			case CH_ESC:
+				uart->escape = true;
+				break;
+			default:
+				if (uart->escape) {
+					uart->escape = false;
+					switch (data) {
+						case CH_ESC_START:
+							data = CH_START;
+							break;
+
+						case CH_ESC_END:
+							data = CH_END;
+							break;
+
+						case CH_ESC_ESC:
+							data = CH_ESC;
+							break;
+
+						default:
+							data = 0xFF;
+					}
+				}
+
+				if (uart->rx_length < sizeof(uart->rx_frame)) {
+					uart->rx_frame[uart->rx_length] = data;
+					uart->rx_length++;
+				} else {
+					uart->rx_length = 0xFFFF;
+				}			
+		}
+	}
+}
+
+static u32 uart_poll_task(void){
+	uart_rx_poll(&uart2);
+	return 0;
+}
+
+
+static void uart_write_byte(uart_t *uart, u8 value)
+{
+	circle_put_data(&uart->c_tx_buff, &value, 1);
+}
+
+static void uart_write_byte_esc(uart_t *uart, u8 value)
+{
+	switch (value) {
+	case CH_START:
+		uart_write_byte(uart, CH_ESC);
+		value = CH_ESC_START;
+		break;
+
+	case CH_END:
+		uart_write_byte(uart, CH_ESC);
+		value = CH_ESC_END;
+		break;
+
+	case CH_ESC:
+		uart_write_byte(uart, CH_ESC);
+		value = CH_ESC_ESC;
+		break;
+	}
+
+	uart_write_byte(uart, value);
+}
+
+static void uart_write_esc(uart_t *uart, const u8 *buff, u16 length)
+{
+	const u8 *buff_end;
+
+	for (buff_end = buff + length; buff < buff_end; buff++) {
+		uart_write_byte_esc(uart, *buff);
+	}
+}
+
+static void uart_tx_start(uart_t *uart)
+{
+	uart_write_byte(uart, CH_START);
+	uart->tx_crc16 = 0;
+}
+
+static void uart_tx_continue(uart_t *uart, const void *buff, u16 length)
+{
+	uart_write_esc(uart, (const u8 *) buff, length);
+	uart->tx_crc16 = shark_crc16_update(uart->tx_crc16, (const u8 *) buff, length);
+}
+
+static void uart_tx_end(uart_t *uart)
+{
+	uart_write_esc(uart, (u8 *)&uart->tx_crc16, sizeof(uart->tx_crc16));
+	uart_write_byte(uart, CH_END);
+	UART2_EnableTx();
+}
+
+void uart_write_frame(uint8_t *bytes, int len){
+	uart_t *uart = &uart2;
+	uart_tx_start(uart);
+	uart_tx_continue(uart, bytes, len);
+	uart_tx_end(uart);
+}
 
 bool uart_tx_finished(void) {
 	u8 data;
-	if (circle_get_one_data(&c_buff, &data) <= 0){
+	if (circle_get_one_data(&uart2.c_tx_buff, &data) <= 0){
 		return true;
 	}
 	UART2_TxData(data);
 	return false;
 }
 
+void uart_rx_put(u8 ch) {
+	circle_put_one_data(&uart2.c_rx_buff, ch);
+}
 
 int fputc(int c, FILE *fp){
 	u8 data = c;

Application/Bsp/serial.h

@@ -1,8 +1,33 @@
 #ifndef _SERIAL_H__
 #define _SERIAL_H__
 #include "libs/types.h"
+#include "libs/circle_buffer.h"
+
+#define CH_START						0xF5
+#define CH_END							0xF6
+#define CH_ESC							0xF7
+#define CH_ESC_START					0x05
+#define CH_ESC_END						0x06
+#define CH_ESC_ESC						0x07
+
+#define RX_FRAME_MAX_LEN 130
+typedef struct {
+	u8 tx_buf[128];
+	u8 rx_buf[128];
+	c_buffer_t c_tx_buff;
+	c_buffer_t c_rx_buff;
+	c_buffer_t rx_queue;
+	uint16_t tx_length;
+	uint16_t tx_crc16;
+	uint8_t rx_frame[RX_FRAME_MAX_LEN];
+	uint16_t rx_length;
+	bool escape;
+}uart_t;
+
 
 void serial_init(void);
 void serial_write(u8 *data, int len);
+void uart_write_frame(uint8_t *bytes, int len);
+
 #endif /* _SERIAL_H__ */
 

Application/FOC/PI_Controller.c

@@ -1,6 +1,29 @@
 #include "pi_controller.h"
 
 float pi_control(PI_ctrl_t *pi, float error){
-	return 0.0f;
+	float output = 0.0f;
+	/* »ý·Ö¿¹±¥ºÍ */
+	if (pi->output > pi->max_output) {
+		if (error < 0) {
+			pi->i_errors += error;
+		}
+	}else if (pi->output < pi->min_output) {
+		if (error > 0) {
+			pi->i_errors += error;
+		}
+	}else {
+		pi->i_errors += error;
+	}
+	output = pi->output = pi->Kp_gain * error + pi->Ki_gain * pi->i_errors;
+	if (output > pi->max_output) {
+		output = pi->max_output;
+	}else if (output < pi->min_output) {
+		output = pi->min_output;
+	}
+	return output;
 }
 
+void pi_clear(PI_ctrl_t *pi) {
+	pi->i_errors = 0.0f;
+	pi->output = 0.0f;
+}

Application/FOC/PI_Controller.h

@@ -3,11 +3,14 @@
 typedef struct _pi {
 	float Kp_gain;
 	float Ki_gain;
-	float limit_high_output;
-	float limit_low_output;
+	float i_errors;
+	float output;
+	float max_output;
+	float min_output;
 }PI_ctrl_t;
 
 float pi_control(PI_ctrl_t *pi, float error);
+void pi_clear(PI_ctrl_t *pi);
 
 #endif  /* _PI_CONTROLLER_H__ */
 

Application/FOC/circle_limitation.c

@@ -0,0 +1,13 @@
+#include "circle_limitation.h"
+
+void CirCle_Limitation_Process(dq_t *inout_vdq, float in_vbus, float svm_ration){
+	float svd = SQ(inout_vdq->Vd);
+	float svq = SQ(inout_vdq->Vq);
+	float svBus = SQ(in_vbus*svm_ration);
+	if (svd + svq > svBus) {
+		float r = sqrtf(svBus/(svd+svq));
+		inout_vdq->Vd *= r;
+		inout_vdq->Vq *= r;
+	}
+}
+

Application/FOC/circle_limitation.h

@@ -0,0 +1,7 @@
+#ifndef _CIRCLE_LIMITATION_H__
+#define _CIRCLE_LIMITATION_H__
+#include "foc_type.h"
+
+void CirCle_Limitation_Process(dq_t *inout_vdq, float in_vbus, float svm_ration);
+#endif /*_CIRCLE_LIMITATION_H__ */
+

Application/FOC/foc.c

@@ -1,215 +0,0 @@
-#include <string.h>
-#include "libs/task.h"
-#include "bsp/bsp.h"
-#include "foc/foc.h"
-#include "foc/park_clark.h"
-#include "foc/svpwm.h"
-#include "foc/foc_task.h"
-#include "foc/phase_current.h"
-#include "foc/hall_sensor.h"
-#include "foc/vbus_sensor.h"
-#include "foc/ntc_sensor.h"
-
-static u32 foc_measure_task(void);
-static void foc_defulat_value(void);
-
-static motor_foc_t mFOC = {
-	.motor_p = {
-		.poles = 2,
-		.ld = 0.578477f,
-		.lq = 5.78477f,
-		.rs = 1.088f,
-		.inertia = 3.319367f,
-		.b_emf = 4.332566f,
-	},
-};
-
-void foc_init(void) {
-	foc_defulat_value();	
-	HAL_ADC1_Enable();
-	/* init pwm hardware timer */
-	PWM_TimerEnable();
-	/* enable tim4 to run the foc normal task */
-	TIM4_Enable();
-
-	hall_sensor_init();
-	vbus_sensor_init();
-	ntc_sensor_init();
-	
-	task_start(foc_measure_task, 0);
-
-	///hall_sensor_calibrate(8.0f, mFOC.hall_table);
-}
-
-static void foc_defulat_value(void){
-	memset(&mFOC, 0, sizeof(mFOC));
-	mFOC.state = IDLE;
-	mFOC.mosGate = false;
-	mFOC.vbus = 12.0f;
-	phase_current_init(&mFOC.current_samp);
-}
-
-
-void foc_clear(void) {
-	PWM_Stop();
-	mFOC.mosGate = false;
-	foc_defulat_value();
-	hall_sensor_init();
-}
-
-FOCState FOC_STM_State(void){
-	return mFOC.state;
-}
-
-u32 foc_get_speed(void) {
-	return abs(hall_sensor_get_speed()/mFOC.motor_p.poles);
-}
-
-FError FOC_STM_NextState(FOCState state) {
-	bool changed = false;
-	if (state == mFOC.state) {
-		return NoError;
-	}
-	if (state == START) {
-		if (mFOC.state == IDLE) {
-			changed = true;
-		}
-	}else if (state == IDLE) {
-		if (mFOC.state == ANY_STOP) {
-			changed = true;
-		}
-	}else if (state == ANY_STOP) {
-		if (mFOC.state != IDLE) {
-			changed = true;
-		}
-	}else if (state == CURRENT_CALIBRATE) {
-		if (mFOC.state == START) {
-			changed = true;
-		}
-	}else if (state == READY_TO_RUN) {
-		if (mFOC.state == CURRENT_CALIBRATE) {
-			changed = true;
-		}
-	}else if (state == RAMPING_START) {
-		if (mFOC.state == READY_TO_RUN) {
-			changed = true;
-		}
-	}else if (state == RUNNING) {
-		if (mFOC.state == RAMPING_START) {
-			changed = true;
-		}
-	}
-	if (changed) {
-		mFOC.state = state;
-		return NoError;
-	}
-	return STMNotAllow;
-}
-
-/* ÉèÖÃÆô¶¯rampµçÁ÷ºÍʱ¼ä */
-void Foc_Set_StartRamp(float final, u32 duration_ms){
-	ramp_ctrl_init(&mFOC.start_ramp, 0.0f, final, duration_ms);
-}
-
-FError foc_start_motor(void){
-	return FOC_STM_NextState(START);
-}
-
-FError foc_stop_motor(void) {
-	return FOC_STM_NextState(ANY_STOP);
-}
-
-void foc_current_calibrate(void){
-	mFOC.current_samp.adc_offset_a = 0;
-	mFOC.current_samp.adc_offset_b = 0;
-	mFOC.current_samp.adc_offset_c = 0;
-
-	PWM_Disable_Channels();
-	//foc_pwm_start(false);
-
-	task_udelay(10);
-	
-	phase_current_init(&mFOC.current_samp);
-	mFOC.current_samp.is_calibrating = true;
-	mFOC.current_samp.sector = SECTOR_5;
-	foc_pwm_start(true);
-	HAL_ADC1_InJ_StartConvert();
-	while(mFOC.current_samp.offset_sample_count != 0){};
-	
-	foc_pwm_start(false);
-	task_udelay(100);
-	phase_current_init(&mFOC.current_samp);	
-	mFOC.current_samp.sector = SECTOR_1;
-	foc_pwm_start(true);
-	while(mFOC.current_samp.offset_sample_count != 0){};
-	mFOC.current_samp.is_calibrating = false;
-	foc_pwm_start(false);
-	PWM_Enable_Channels();
-}
-
-void foc_overide_theta(bool enable){
-	mFOC.override.is_theta = enable;
-}
-
-void foc_overide_vdq(bool enable){
-	mFOC.override.is_vdq = enable;
-}
-
-void foc_overide_set_theta(float theta){
-	mFOC.override.theta = theta;
-}
-
-void foc_overide_set_vdq(float d, float q){
-	mFOC.override.vdq.d = d;
-	mFOC.override.vdq.q = q;
-}
-
-
-static u32 foc_measure_task(void){
-	vbus_sample_voltage();
-	ntc_sensor_sample();
-	LowPass_Filter(mFOC.vbus, vbus_get_voltage(), 0.1f);
-	wdog_reload();
-	return 1;
-}
-
-void foc_brake_handler(void) {
-
-}
-
-void foc_pwm_up_handler(void){
-	phase_current_adc_triger(&mFOC.current_samp);
-}
-
-#if defined (CCMRAM)
-#if defined (__ICCARM__)
-#pragma location = ".ccmram"
-#elif defined (__CC_ARM)
-__attribute__( ( section ( ".ccmram" ) ) )
-#endif
-#endif
-
-void current_sample_handler(void) {
-	if (mFOC.current_samp.is_calibrating) {
-		phase_current_offset(&mFOC.current_samp);
-	}else {
-		FOC_Fast_Task(&mFOC);
-	}
-}
-
-void foc_slow_task_handler(void) {
-	FOC_Normal_Task(&mFOC);
-}
-
-void foc_pwm_start(bool start) {
-	if (start == mFOC.mosGate) {
-		return;
-	}
-	if (start) {
-		PWM_Start();
-	}else {
-		PWM_Stop();
-	}
-	mFOC.mosGate = start;	
-}
-

Application/FOC/foc.h

@@ -1,24 +0,0 @@
-#ifndef _FOC_H__
-#define _FOC_H__
-#include "foc/foc_type.h"
-
-
-void foc_init(void);
-void foc_clear(void);
-void set_dq_voltage(float d_v, float q_v);
-void foc_pwm_start(bool start);
-FError foc_start_motor(void);
-FError foc_stop_motor(void);
-int foc_hall_detect(float current, u16 *hall_table);
-void foc_overide_theta(bool enable);
-void foc_overide_vdq(bool enable);
-void foc_overide_set_theta(float theta);
-void foc_overide_set_vdq(float d, float q);
-FError FOC_STM_NextState(FOCState s);
-void Foc_Set_StartRamp(float final, u32 duration_ms);
-FOCState FOC_STM_State(void);
-void foc_current_calibrate(void);
-u32 foc_get_speed(void);
-
-#endif /* _FOC_H__ */
-

Application/FOC/foc_api.c

@@ -0,0 +1,183 @@
+#include <string.h>
+#include "libs/task.h"
+#include "bsp/bsp.h"
+#include "foc/foc_api.h"
+#include "foc/park_clark.h"
+#include "foc/svpwm.h"
+#include "foc/foc_core.h"
+#include "foc/foc_stm.h"
+#include "foc/phase_current.h"
+#include "foc/hall_sensor.h"
+#include "foc/vbus_sensor.h"
+#include "foc/ntc_sensor.h"
+#include "foc/gas_sensor.h"
+
+extern motor_foc_t mFOC;
+static u32 foc_measure_task(void);
+static void foc_defulat_value(void);
+
+void foc_init(void) {
+	foc_defulat_value();
+	
+	HAL_ADC1_Enable();
+	/* init pwm hardware timer */
+	PWM_TimerEnable();
+	/* enable tim4 to run the foc normal task */
+	TIM4_Enable();
+
+	hall_sensor_init();
+	vbus_sensor_init();
+	ntc_sensor_init();
+
+	task_start(foc_measure_task, 0);
+}
+
+static void foc_defulat_value(void){
+	memset(&mFOC, 0, sizeof(mFOC));
+	mFOC.state = IDLE;
+	mFOC.mosGate = false;
+	mFOC.vbus = MAX_VBUS_VOLTAGE;
+	mFOC.state = IDLE;
+	mFOC.mode = FOC_MODE_OPEN_LOOP;
+	phase_current_init(&mFOC.current_samp);
+	ramp_ctrl_init(&mFOC.start_ramp);
+	ramp_ctrl_init(&mFOC.current_ramp);
+	ramp_ctrl_init(&mFOC.speed_ramp);
+	pi_clear(&mFOC.PI_id);
+	pi_clear(&mFOC.PI_iq);
+	pi_clear(&mFOC.PI_speed);	
+}
+
+float speed_to_voltage(u16 rpm) {
+	return foc_get_vbus_voltage() * rpm / MAX_SPEED_RPM;
+}
+
+float speed_to_current(u16 rpm) {
+	return MAX_CURRENT * rpm / MAX_SPEED_RPM;
+}
+
+void foc_clear(void) {
+	PWM_Stop();
+	mFOC.mosGate = false;
+	foc_defulat_value();
+	hall_sensor_init();
+}
+
+u32 foc_get_speed(void) {
+	return abs(hall_sensor_get_speed()/mFOC.motor_p.poles);
+}
+
+bool foc_is_ready(void){
+	return mFOC.is_ready;
+}
+
+void foc_set_ready(bool ready) {
+	mFOC.is_ready = ready;
+}
+
+
+void foc_set_current_ramp(float final){
+	ramp_set_target(&mFOC.current_ramp, mFOC.dq_v.Vq, final, START_RAMP_DURATION);
+}
+
+void foc_set_speed_ramp(u16 rpm){
+	ramp_set_target(&mFOC.speed_ramp, foc_get_speed(), rpm, SPEED_RAMP_DURATION);
+}
+
+void foc_set_start_ramp(float v) {
+	ramp_set_target(&mFOC.start_ramp, mFOC.dq_v.Vq, v, START_RAMP_DURATION);
+}
+
+void foc_set_ref_speed(u16 rpm) {
+	normal_task_enable(false);
+	mFOC.rpm_ref = rpm;
+	if (mFOC.mode == FOC_MODE_OPEN_LOOP) {
+		foc_set_start_ramp(speed_to_voltage(rpm));
+		ramp_exc(&mFOC.start_ramp);
+	}else if (mFOC.mode == FOC_MODE_PI_CURRENT) {
+		foc_set_current_ramp(speed_to_current(rpm));
+		ramp_exc(&mFOC.current_ramp);
+	}
+	normal_task_enable(true);
+}
+
+foc_fault_t api_set_ready(bool ready) {
+	if (ready == foc_is_ready()) {
+		return foc_success;
+	}
+	normal_task_enable(false);
+	if (foc_stm_nextstate(ready?START:ANY_STOP) == NoError) {
+		foc_set_ready(ready);
+		normal_task_enable(true);
+		return foc_success;
+	}	
+	normal_task_enable(true);
+	return foc_not_allowed;
+}
+
+foc_fault_t foc_start_motor(void){
+	return foc_stm_nextstate(START);
+}
+
+foc_fault_t foc_stop_motor(void) {
+	return foc_stm_nextstate(ANY_STOP);
+}
+
+void foc_current_calibrate(void){
+	mFOC.current_samp.adc_offset_a = 0;
+	mFOC.current_samp.adc_offset_b = 0;
+	mFOC.current_samp.adc_offset_c = 0;
+
+	PWM_Disable_Channels();
+	//foc_pwm_start(false);
+
+	task_udelay(10);
+	
+	phase_current_init(&mFOC.current_samp);
+	mFOC.current_samp.is_calibrating = true;
+	mFOC.current_samp.sector = SECTOR_5;
+	foc_pwm_start(true);
+	HAL_ADC1_InJ_StartConvert();
+	while(mFOC.current_samp.offset_sample_count != 0){};
+	
+	foc_pwm_start(false);
+	task_udelay(100);
+	phase_current_init(&mFOC.current_samp);	
+	mFOC.current_samp.sector = SECTOR_1;
+	foc_pwm_start(true);
+	while(mFOC.current_samp.offset_sample_count != 0){};
+	mFOC.current_samp.is_calibrating = false;
+	foc_pwm_start(false);
+	PWM_Enable_Channels();
+}
+
+void foc_overide_theta(bool enable){
+	mFOC.override.is_theta = enable;
+}
+
+void foc_overide_vdq(bool enable){
+	mFOC.override.is_vdq = enable;
+}
+
+void foc_overide_set_theta(float theta){
+	mFOC.override.theta = theta;
+}
+
+
+void foc_overide_set_vdq(float d, float q){
+	mFOC.override.vdq.Vd = d;
+	mFOC.override.vdq.Vq = q;
+}
+
+float foc_get_vbus_voltage(void){
+	return mFOC.vbus;
+}
+static u32 foc_measure_task(void){
+	gas_sample_voltage();
+	vbus_sample_voltage();
+	ntc_sensor_sample();
+	LowPass_Filter(mFOC.vbus, vbus_get_voltage(), 0.1f);
+	wdog_reload();
+	return 1;
+}
+

Application/FOC/foc_api.h

@@ -0,0 +1,29 @@
+#ifndef _FOC_API_H__
+#define _FOC_API_H__
+#include "foc/foc_type.h"
+
+
+void foc_init(void);
+void foc_clear(void);
+void set_dq_voltage(float d_v, float q_v);
+void foc_pwm_start(bool start);
+foc_fault_t foc_start_motor(void);
+foc_fault_t foc_stop_motor(void);
+int foc_hall_detect(float current, u16 *hall_table);
+void foc_overide_theta(bool enable);
+void foc_overide_vdq(bool enable);
+void foc_overide_set_theta(float theta);
+void foc_overide_set_vdq(float d, float q);
+void foc_set_current_ramp(float final);
+void foc_set_speed_ramp(u16 rpm);
+void foc_set_start_ramp(float v);
+void foc_current_calibrate(void);
+u32 foc_get_speed(void);
+float foc_get_vbus_voltage(void);
+bool foc_is_ready(void);
+void foc_set_ready(bool ready);
+void foc_set_ref_speed(u16 rpm);
+float speed_to_voltage(u16 rpm);
+float speed_to_current(u16 rpm);
+#endif /* _FOC_API_H__ */
+

Application/FOC/foc_core.c

@@ -0,0 +1,249 @@
+#include "hal/hal.h"
+#include "hal/pwm.h"
+#include "libs/task.h"
+#include "foc_core.h"
+#include "foc_api.h"
+#include "foc_stm.h"
+#include "phase_current.h"
+#include "park_clark.h"
+#include "hall_sensor.h"
+#include "circle_limitation.h"
+#include "svpwm.h"
+
+
+motor_foc_t mFOC = {
+	.motor_p = {
+		.poles = 2,
+		.ld = 0.578477f,
+		.lq = 5.78477f,
+		.rs = 1.088f,
+		.inertia = 3.319367f,
+		.b_emf = 4.332566f,
+	},
+	.PI_id = {
+		.Kp_gain = 5,
+		.Ki_gain = 100,
+		.max_output = MAX_VBUS_VOLTAGE,
+		.min_output = -MAX_VBUS_VOLTAGE,		
+	},
+	.PI_iq = {
+		.Kp_gain = 5,
+		.Ki_gain = 100,
+		.max_output = MAX_VBUS_VOLTAGE,
+		.min_output = -MAX_VBUS_VOLTAGE,
+	},
+	.PI_speed = {
+		.Kp_gain = 5,
+		.Ki_gain = 100,
+		.max_output = MAX_CURRENT,
+		.min_output = -MAX_CURRENT,
+	},	
+};
+#if 1
+static void __inline foc_update_theta(motor_foc_t *foc) {
+	float angle = 0.0f;
+	if (foc->override.is_theta) {
+		angle = foc->override.theta;
+	}else {
+		angle = hall_sensor_get_theta();
+	}
+	foc->motor_s.angle = angle;
+	foc->motor_s.theta = degree_2_pi(foc->motor_s.angle);
+}
+
+#else
+static void __inline foc_update_theta(motor_foc_t *foc) {
+	static float angle = 0.0f;
+	static bool first_s = false;
+	if (!first_s) {
+		first_s = true;
+		angle = hall_sensor_get_theta();
+	}else {
+		angle += 0.5f;
+	}
+	fast_norm_angle(&angle);
+	foc->motor_s.angle = angle;
+	foc->motor_s.theta = degree_2_pi(angle);
+}
+#endif
+
+
+static void __inline Foc_Current_PI_Contrl(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
+	if (foc->mode == FOC_MODE_PI_CURRENT || foc->mode == FOC_MODE_PI_FULL) {
+		ref_out->Vd = pi_control(&foc->PI_id, foc->dq_ref.Id - sampled->Id);
+		ref_out->Vq = pi_control(&foc->PI_iq, foc->dq_ref.Iq - sampled->Iq);
+	}else {
+		ref_out->Vd = foc->dq_ref.Vd;
+		ref_out->Vq = foc->dq_ref.Vq;
+	}
+	if (foc->override.is_vdq) {
+		ref_out->Vd = foc->override.vdq.Vd;
+		ref_out->Vq = foc->override.vdq.Vq;
+	}
+	foc->dq_v.Vd = ref_out->Vd;
+	foc->dq_v.Vq = ref_out->Vq;
+}
+
+static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_t *time) {
+#if 0
+    /* Dead time compensation */
+    if ( c_sample->Ia > 0)
+    {
+      time->A += TDead;
+    }
+    else
+    {
+      time->A -= TDead;
+    }
+
+    if ( c_sample->Ib > 0 )
+    {
+      time->B += TDead;
+    }
+    else
+    {
+      time->B -= TDead;
+    }
+
+    if ( c_sample->Ic > 0 )
+    {
+      time->C += TDead;
+    }
+    else
+    {
+      time->C -= TDead;
+    }
+#endif
+}
+
+static void __inline Debug_Log(motor_foc_t *foc){
+#if 0
+	static int count;
+	if (count++ % 10 == 0) {
+		//printf("$%d %d %d %d %d;",(int)(foc->current_samp.Ia * 1000.0f), (int)(foc->current_samp.Ib * 1000.0f),
+		//	(int)(foc->current_samp.Ic * 1000.0f), (int)foc->sector * 100, (int)foc->motor_s.angle);
+		printf("$%d;", (int)hall_sensor_get_speed());
+	}
+#endif	
+}
+
+static void __inline Debug_dq(dq_t *dq){
+#if 0
+	static int count;
+	if (count++ % 10 == 0) {
+		printf("$%d %d;",(int)(dq->d * 1000.0f), (int)(dq->q * 1000.0f));
+	}
+#endif	
+}
+
+#if defined (CCMRAM)
+#if defined (__ICCARM__)
+#pragma location = ".ccmram"
+#elif defined (__CC_ARM)
+__attribute__( ( section ( ".ccmram" ) ) )
+#endif
+#endif
+/* FOC 主控制任务 */
+void FOC_Fast_Task(motor_foc_t *foc){
+	current_samp_t *c_sample = &foc->current_samp;
+	alpha_beta_t sample_ab, pwm_ab;
+	dq_t         sample_dq, v_dq;
+	phase_time_t         phase_time;
+	u32 sample_point;
+
+	/* 更新电角度 */
+	foc_update_theta(foc);
+	/* 采集相电流 */
+	phase_current_sample(c_sample);
+	/* ABC三相坐标到alpha-beta坐标 */
+	Clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
+	/* alpha-beta坐标系到D-Q旋转坐标系 */
+	Park(&sample_ab, foc->motor_s.theta, &sample_dq);
+	/* 电流环，输出电压给SVPWM */
+	Foc_Current_PI_Contrl(foc, &sample_dq, &v_dq);
+	/* 确保电压在6个扇区的内切圆中 */
+	CirCle_Limitation_Process(&v_dq, foc->vbus, 0.95f);
+	/* d-q坐标系到alpha-beta坐标系，输出给svpwm */
+	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
+	/* SVPWM，获取三相逆变器的开关时间，用的是pwm1模式，如果是pwm2模式，这个函数需要修改 */
+	SVM_Get_Phase_Time(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
+	/* 计算三相电流的采样点 */
+	sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
+	/* 死区补偿 */
+	DeadTime_Compensation(c_sample, &phase_time);
+	/* 更新 TIM1的CCR0-2，生成互补pwm */
+	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);
+
+	Debug_Log(foc);
+
+	Debug_dq(&sample_dq);
+}
+
+/* 计算电流环的参考输入 */
+void Foc_Calc_Current_Ref(motor_foc_t *foc) {
+	if (foc->mode == FOC_MODE_PI_SPEED || foc->mode == FOC_MODE_PI_FULL){
+		float speed_ref = ramp_get_target(&foc->speed_ramp);
+		float speed_feedback = foc_get_speed();
+		float vq_out = pi_control(&foc->PI_speed, speed_ref - speed_feedback);
+		foc->dq_ref.Iq = vq_out;
+		foc->dq_ref.Id = 0.0f; //if MTPA used, d is not 0
+	}else {
+		foc->dq_ref.Iq = ramp_get_target(&foc->current_ramp);
+		foc->dq_ref.Id = 0.0f; //if MTPA used, d is not 0
+	}
+}
+
+void Foc_Speed_Ramp(motor_foc_t *foc){
+	if (foc->rpm_ref >= 0 && foc->mode != FOC_MODE_PI_CURRENT){
+		u16 current_rpm = foc_get_speed();
+		u16 ref_rpm = foc->rpm_ref;
+		foc->rpm_ref = -1;
+		
+		if (ref_rpm + 60 < current_rpm){
+			ramp_set_target(&foc->current_ramp, foc->dq_ref.Iq, speed_to_current(ref_rpm), SPEED_RAMP_DURATION);
+			ramp_exc(&foc->current_ramp);
+			foc->mode = FOC_MODE_PI_CURRENT;
+		}
+	}
+}
+
+void foc_brake_handler(void) {
+	mFOC.foc_fault = foc_brake_error;
+}
+
+void foc_pwm_up_handler(void){
+	phase_current_adc_triger(&mFOC.current_samp);
+}
+
+#if defined (CCMRAM)
+#if defined (__ICCARM__)
+#pragma location = ".ccmram"
+#elif defined (__CC_ARM)
+__attribute__( ( section ( ".ccmram" ) ) )
+#endif
+#endif
+
+void current_sample_handler(void) {
+	if (mFOC.current_samp.is_calibrating) {
+		phase_current_offset(&mFOC.current_samp);
+	}else {
+		FOC_Fast_Task(&mFOC);
+	}
+}
+
+void foc_slow_task_handler(void) {
+	FOC_Normal_Task(&mFOC);
+}
+
+void foc_pwm_start(bool start) {
+	if (start == mFOC.mosGate) {
+		return;
+	}
+	if (start) {
+		PWM_Start();
+	}else {
+		PWM_Stop();
+	}
+	mFOC.mosGate = start;	
+}
+

Application/FOC/foc_core.h

@@ -0,0 +1,8 @@
+#ifndef _FOC_CORE_H__
+#define _FOC_CORE_H__
+#include "foc_type.h"
+void FOC_Fast_Task(motor_foc_t *foc);
+void Foc_Calc_Current_Ref(motor_foc_t *foc);
+void Foc_Speed_Ramp(motor_foc_t *foc);
+#endif /* _FOC_CORE_H__ */
+

Application/FOC/foc_stm.c

@@ -0,0 +1,110 @@
+#include <string.h>
+#include "bsp/bsp.h"
+#include "foc/foc_api.h"
+#include "foc/foc_core.h"
+
+extern motor_foc_t mFOC;
+
+FOCState foc_stm_state(void){
+	return mFOC.state;
+}
+
+foc_fault_t foc_stm_nextstate(FOCState state) {
+	bool changed = false;
+	if (state == mFOC.state) {
+		return foc_success;
+	}
+	if (state == START) {
+		if (mFOC.state == IDLE) {
+			changed = true;
+		}
+	}else if (state == IDLE) {
+		if (mFOC.state == ANY_STOP) {
+			changed = true;
+		}
+	}else if (state == ANY_STOP) {
+		if (mFOC.state != IDLE) {
+			changed = true;
+		}
+	}else if (state == CURRENT_CALIBRATE) {
+		if (mFOC.state == START) {
+			changed = true;
+		}
+	}else if (state == READY_TO_RUN) {
+		if (mFOC.state == CURRENT_CALIBRATE) {
+			changed = true;
+		}
+	}else if (state == RAMPING_START) {
+		if (mFOC.state == READY_TO_RUN || mFOC.state == RUNNING) {
+			changed = true;
+		}
+	}else if (state == CLOSED_LOOP) {
+		if (mFOC.state == RAMPING_START) {
+			changed = true;
+		}		
+	}else if (state == RUNNING) {
+		if (mFOC.state == CLOSED_LOOP) {
+			changed = true;
+		}
+	}
+	if (changed) {
+		mFOC.state = state;
+		return foc_success;
+	}
+	return foc_not_allowed;
+}
+
+void FOC_Normal_Task(motor_foc_t *foc) {
+	switch (foc->state) {
+		case START:
+			PWM_TurnOnLowSides();
+			foc_stm_nextstate(CURRENT_CALIBRATE);
+			break;
+		case CURRENT_CALIBRATE:
+			foc_current_calibrate();
+			foc_stm_nextstate(READY_TO_RUN);
+			break;
+		case READY_TO_RUN:
+			foc_pwm_start(true);
+			foc_stm_nextstate(RAMPING_START);
+			ramp_exc(&foc->current_ramp);
+			foc_overide_vdq(true);
+			break;
+		case RAMPING_START:
+			foc_overide_set_vdq(0.0f, ramp_get_target(&foc->current_ramp));
+			if (foc_get_speed() >= RPM_FOR_CLOSE_LOOP){
+				foc_stm_nextstate(CLOSED_LOOP);
+				ramp_clear(&foc->current_ramp);
+				foc_overide_vdq(false);
+			}
+			break;
+		case CLOSED_LOOP:
+			Foc_Speed_Ramp(foc);
+			foc->mode = FOC_MODE_PI_FULL;
+			foc_stm_nextstate(RUNNING);
+			break;
+		case RUNNING:
+			Foc_Speed_Ramp(foc);
+			Foc_Calc_Current_Ref(foc);
+			if (foc->foc_fault == foc_brake_error){
+				foc_stm_nextstate(ANY_STOP);
+				break;
+			}
+			if (foc_get_speed() <= RPM_FOR_CLOSE_LOOP){
+				foc_set_current_ramp(speed_to_current(foc_get_speed()));
+				ramp_exc(&foc->current_ramp);
+				foc_stm_nextstate(RAMPING_START);
+				foc->mode = FOC_MODE_OPEN_LOOP;
+				foc_overide_vdq(true);
+			}
+			break;
+		case ANY_STOP:
+			ramp_clear(&foc->current_ramp);
+			foc_clear();
+			foc_stm_nextstate(IDLE);
+			break;
+		default:
+			break;
+	}
+
+}

Application/FOC/foc_stm.h

@@ -0,0 +1,9 @@
+#ifndef _FOC_STM_H__
+#define _FOC_STM_H__
+#include "foc/foc_api.h"
+#include "foc/foc_core.h"
+
+FOCState foc_stm_state(void);
+foc_fault_t foc_stm_nextstate(FOCState state);
+void FOC_Normal_Task(motor_foc_t *foc);
+#endif /* _FOC_STM_H__ */

Application/FOC/foc_task.c

@@ -1,191 +0,0 @@
-#include "hal/hal.h"
-#include "hal/pwm.h"
-#include "libs/task.h"
-#include "foc_task.h"
-#include "foc.h"
-#include "phase_current.h"
-#include "park_clark.h"
-#include "hall_sensor.h"
-#include "svpwm.h"
-
-#if 1
-static void __inline foc_update_theta(motor_foc_t *foc) {
-	float angle = 0.0f;
-	if (foc->override.is_theta) {
-		angle = foc->override.theta;
-	}else {
-		angle = hall_sensor_get_theta();
-	}
-	foc->motor_s.angle = angle;
-	foc->motor_s.theta = degree_2_pi(foc->motor_s.angle);
-}
-
-#else
-static void __inline foc_update_theta(motor_foc_t *foc) {
-	static float angle = 0.0f;
-	static bool first_s = false;
-	if (!first_s) {
-		first_s = true;
-		angle = hall_sensor_get_theta();
-	}else {
-		angle += 0.5f;
-	}
-	fast_norm_angle(&angle);
-	foc->motor_s.angle = angle;
-	foc->motor_s.theta = degree_2_pi(angle);
-}
-#endif
-
-/* 输出dq电压给反park,最后给svpwm */
-static void __inline Foc_Dq_PI_Contrl(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
-	if (foc->mode == FOC_MODE_PI_DQ || foc->mode == FOC_MODE_PI_FULL) {
-		ref_out->d = pi_control(&foc->PI_id, foc->dq_ref.d - sampled->d);
-		ref_out->q = pi_control(&foc->PI_iq, foc->dq_ref.q - sampled->q);
-	}else {
-		ref_out->d = foc->dq_ref.d;
-		ref_out->q = foc->dq_ref.q;
-	}
-	if (foc->override.is_vdq) {
-		ref_out->d = foc->override.vdq.d;
-		ref_out->q = foc->override.vdq.q;
-	}
-}
-
-static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_t *time) {
-#if 0
-    /* Dead time compensation */
-    if ( c_sample->Ia > 0)
-    {
-      time->A += TDead;
-    }
-    else
-    {
-      time->A -= TDead;
-    }
-
-    if ( c_sample->Ib > 0 )
-    {
-      time->B += TDead;
-    }
-    else
-    {
-      time->B -= TDead;
-    }
-
-    if ( c_sample->Ic > 0 )
-    {
-      time->C += TDead;
-    }
-    else
-    {
-      time->C -= TDead;
-    }
-#endif
-}
-
-static void __inline Debug_Log(motor_foc_t *foc){
-#if 0
-	static int count;
-	if (count++ % 10 == 0) {
-		//printf("$%d %d %d %d %d;",(int)(foc->current_samp.Ia * 1000.0f), (int)(foc->current_samp.Ib * 1000.0f),
-		//	(int)(foc->current_samp.Ic * 1000.0f), (int)foc->sector * 100, (int)foc->motor_s.angle);
-		printf("$%d;", (int)hall_sensor_get_speed());
-	}
-#endif	
-}
-
-static void __inline Debug_dq(dq_t *dq){
-#if 0
-	static int count;
-	if (count++ % 10 == 0) {
-		printf("$%d %d;",(int)(dq->d * 1000.0f), (int)(dq->q * 1000.0f));
-	}
-#endif	
-}
-
-#if defined (CCMRAM)
-#if defined (__ICCARM__)
-#pragma location = ".ccmram"
-#elif defined (__CC_ARM)
-__attribute__( ( section ( ".ccmram" ) ) )
-#endif
-#endif
-
-/* FOC 主控制逻辑 */
-void FOC_Fast_Task(motor_foc_t *foc){
-	current_samp_t *c_sample = &foc->current_samp;
-	alpha_beta_t sample_ab, pwm_ab;
-	dq_t         sample_dq, v_dq;
-	phase_time_t         phase_time;
-	u32 sample_point;
-
-	/* 更新电角度 */
-	foc_update_theta(foc);
-	/* 采集3相电流 */
-	phase_current_sample(c_sample);
-	/* ABC坐标转alpha-beta坐标 */
-	Clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
-	/* alpha-beta坐标转旋转坐标系D-Q */
-	Park(&sample_ab, foc->motor_s.theta, &sample_dq);
-	/* 处理D,Q电流环，速度环低频运行，不在此处处理*/
-	Foc_Dq_PI_Contrl(foc, &sample_dq, &v_dq);
-	/* d-q坐标转 alpha-beta坐标，输入给pwm */
-	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
-	/* 生成 pwm，模拟正弦波,此处vbus需要动态采集 */
-	SVPWM_7(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
-	/* 通过扇区和pwm duty 选择合适的3相电流采样点 */
-	sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
-	/* 死区补偿 */
-	DeadTime_Compensation(c_sample, &phase_time);
-	/* 更新duty和采样点到硬件TIM1中 */
-	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);
-
-	Debug_Log(foc);
-
-	Debug_dq(&sample_dq);
-}
-
-//输出dq电流，给电流环
-static void __inline Foc_Speed_PI_Control(motor_foc_t *foc) {
-	if (foc->mode == FOC_MODE_PI_SPEED || foc->mode == FOC_MODE_PI_FULL){
-		float speed_ref = ramp_get_target(&foc->speed_ramp);
-		float vq_out = pi_control(&foc->PI_speed, speed_ref - foc->motor_s.rpm);
-		foc->dq_ref.q = vq_out;
-	}
-}
-
-void FOC_Normal_Task(motor_foc_t *foc) {
-	switch (foc->state) {
-		case START:
-			PWM_TurnOnLowSides();
-			FOC_STM_NextState(CURRENT_CALIBRATE);
-			break;
-		case CURRENT_CALIBRATE:
-			foc_current_calibrate();
-			FOC_STM_NextState(READY_TO_RUN);
-			break;
-		case READY_TO_RUN:
-			foc_pwm_start(true);
-			FOC_STM_NextState(RAMPING_START);
-			ramp_exc(&foc->start_ramp);
-			foc_overide_vdq(true);
-			break;
-		case RAMPING_START:
-			foc_overide_set_vdq(0.0f, ramp_get_target(&foc->start_ramp));
-			if (ramp_complete(&foc->start_ramp)) {
-				FOC_STM_NextState(RUNNING);
-			}
-			break;
-		case RUNNING:
-			Foc_Speed_PI_Control(foc);
-			break;
-		case ANY_STOP:
-			ramp_clear(&foc->start_ramp);
-			foc_clear();
-			FOC_STM_NextState(IDLE);
-			break;
-		default:
-			break;
-	}
-
-}

Application/FOC/foc_task.h

@@ -1,7 +0,0 @@
-#ifndef _FOC_TASK_H__
-#define _FOC_TASK_H__
-#include "foc_type.h"
-void FOC_Fast_Task(motor_foc_t *foc);
-void FOC_Normal_Task(motor_foc_t *foc);
-#endif /* _FOC_TASK_H__ */
-

Application/FOC/foc_type.h

@@ -10,24 +10,30 @@ typedef struct _alphabeta {
 }alpha_beta_t;
 
 typedef struct _dqaix {
-	float d;
-	float q;
+	union {
+		float Id;
+		float Vd;
+	};
+	union {
+		float Iq;
+		float Vq;
+	};
 }dq_t;
 
 typedef struct _motor_p {
-	u8   poles; //极对数
-	float  ld;    //q 轴电感
-	float  lq;    //d 轴电感
-	float  rs;     //内阻
-	float  flux_linkage; //磁链
-	float  inertia; //转动惯量
+	u8   poles; //锟斤拷锟斤拷锟斤拷
+	float  ld;    //q 锟斤拷锟斤拷
+	float  lq;    //d 锟斤拷锟斤拷
+	float  rs;     //锟斤拷锟斤拷
+	float  flux_linkage; //锟斤拷锟斤拷
+	float  inertia; //转锟斤拷锟斤拷锟斤拷
 	float  b_emf;
 }motor_param_t;
 
 typedef struct _motor_s {
-	float theta;//转子电角度, PI
-	int angle;//转子电角度, 度
-	float rpm; //转速
+	float theta;//转锟接碉拷嵌锟�, PI
+	int angle;//转锟接碉拷嵌锟�, 锟斤拷
+	float rpm; //转锟斤拷
 }motor_stat_t;
 
 typedef struct _phase_time {
@@ -37,25 +43,26 @@ typedef struct _phase_time {
 	u32 low;
 	u32 midle;
 	u32 high;
-}phase_time_t; //三相pwn的duty cnt
+}phase_time_t; //锟斤拷锟斤拷pwn锟斤拷duty cnt
 
 typedef enum {
 	IDLE = 0,
-	START = 1,
-	CURRENT_CALIBRATE = 2,
-	CHARGER_BOOT_CAP = 3,
-	READY_TO_RUN = 4,
-	RAMPING_START = 5,
-	RUNNING = 6,
-	ANY_STOP = 7
+	START,
+	CURRENT_CALIBRATE,
+	CHARGER_BOOT_CAP,
+	READY_TO_RUN,
+	RAMPING_START,
+	CLOSED_LOOP,
+	RUNNING,
+	ANY_STOP
 }FOCState;
 
 typedef enum {
-	FOC_MODE_OPEN_LOOP, //开环
-	FOC_MODE_PI_DQ,      //只电流环
-	FOC_MODE_PI_SPEED,     //只速度环
-	FOC_MODE_PI_FULL,         //速度，电流环都有
-	FOC_MODE_CURRENT_CALI,    //校准电流读取
+	FOC_MODE_OPEN_LOOP, //锟斤拷锟斤拷
+	FOC_MODE_PI_CURRENT,      //只锟斤拷锟斤拷锟斤拷
+	FOC_MODE_PI_SPEED,     //只锟劫度伙拷
+	FOC_MODE_PI_FULL,         //锟劫度ｏ拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+	FOC_MODE_CURRENT_CALI,    //校准锟斤拷锟斤拷锟斤拷取
 }foc_mode_t;
 
 typedef struct current_sample {
@@ -78,14 +85,22 @@ typedef struct _override {
 	dq_t vdq;
 }override_param_t;
 
+typedef enum {
+	foc_success = 0,
+	foc_not_allowed = 1,
+	foc_brake_error = 100,
+}foc_fault_t;
+
+
 typedef struct foc_s {
+	bool is_ready;
 	alpha_beta_t alpha_beta;
-	current_samp_t current_samp; /* 三相电流采样 */
-	dq_t dq_ref; //如果是正常模式下，由速度环输出
-	int  rpm_ref; //用户设置的速度
+	current_samp_t current_samp; /* 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟� */
+	dq_t dq_ref; //锟斤拷锟斤拷锟斤拷锟斤拷锟侥Ｊ斤拷拢锟斤拷锟斤拷俣然锟斤拷锟斤拷
+	int  rpm_ref; //锟矫伙拷锟斤拷锟矫碉拷锟劫讹拷
 	dq_t dq_v;
-	u8   sector; //svpwm 扇区
-	float vbus; //母线电压
+	u8   sector; //svpwm 锟斤拷锟斤拷
+	float vbus; //母锟竭碉拷压
 	motor_param_t motor_p;
 	motor_stat_t  motor_s;
 	phase_time_t phase_time;
@@ -96,9 +111,10 @@ typedef struct foc_s {
 	volatile foc_mode_t mode;
 	override_param_t override;
 	bool mosGate;
-	ramp_t start_ramp; //电机启动的电流ramp
-	ramp_t speed_ramp; //改变速度的ramp，比如油门变化需要设置speed_ramp
-	u16  hall_table[8];
+	ramp_t start_ramp; //锟斤拷锟斤拷锟斤拷锟斤拷牡锟窖箁amp,锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷通锟斤拷锟斤拷锟矫碉拷压锟斤拷锟斤拷锟狡碉拷锟斤拷
+	ramp_t current_ramp; //锟斤拷锟劫碉拷锟斤拷斜锟斤拷
+	ramp_t speed_ramp; //锟侥憋拷锟劫度碉拷ramp锟斤拷锟斤拷锟斤拷锟斤拷锟脚变化锟斤拷要锟斤拷锟斤拷speed_ramp
+	foc_fault_t foc_fault;
 }motor_foc_t;
 
 typedef enum {
@@ -109,6 +125,12 @@ typedef enum {
 
 #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)
+
+#define RPM_FOR_CLOSE_LOOP 60.0F
+#define MAX_SPEED_RPM 3000
+
 #if 1
 #define SECTOR_1  0u
 #define SECTOR_2  1u

Application/FOC/gas_sensor.c

@@ -0,0 +1,36 @@
+#include "gas_sensor.h"
+#include "bsp/bsp.h"
+
+static gas_t _gas;
+void gas_sensor_init(void){
+	_gas.voltage = 0;
+	_gas.lowpass = 5;
+	HAL_ADC1_ChanConfig(SPEED_SENSOR_ADC_CHANNEL);
+	gas_sample_voltage();
+}
+
+void gas_sample_voltage(void){
+	u32 vadc = 0;
+	u32 max = 0, min = 0xFFFFFFF;
+	int count = 8;
+	for (int i = 0; i < count; i++) {
+		u32 one = HAL_ADC1_ReadValue(SPEED_SENSOR_ADC_CHANNEL);
+		vadc += one;
+		if (min > one) {
+			min = one;
+		}
+		if (max < one) {
+			max = one;
+		}
+	}
+	vadc -= (max + min);
+	vadc = vadc / (count - 2);
+	_gas.voltage = ((float)vadc)/(65536.0f) * ADC_REFERENCE_VOLTAGE;
+}
+
+
+float gas_get_value(void){
+
+	return _gas.voltage/ADC_REFERENCE_VOLTAGE;
+}
+

Application/FOC/gas_sensor.h

@@ -0,0 +1,15 @@
+#ifndef _GAS_SENSOR_H__
+#define _GAS_SENSOR_H__
+
+
+typedef struct {
+	float voltage;
+	float lowpass;
+}gas_t;
+
+void gas_sensor_init(void);
+void gas_sample_voltage(void);
+float gas_get_value(void); //归一化
+
+#endif /* _GAS_SENSOR_H__ */
+

Application/FOC/hall_sensor.c

@@ -3,16 +3,16 @@
 #include "libs/task.h"
 #include "math/fast_math.h"
 #include "hall_sensor.h"
-#include "foc/foc.h"
+#include "foc/foc_api.h"
 
 #define HALL_READ_TIMES 3
 /* 
-* 测量HALL_PLACE_OFFSET通用方式就是ST推荐的通过外力带动电机，
-* 测量电机U相反电动势和hall 1的上升沿之间的差值
-* 这里使用的是先通过hall_sensor_calibrate测量hall1，2，3，4，5，6
-* 对应的角度(偏差比较大),然后启动电机，让HALL_PLACE_OFFSET
-* 从0开始增加，每增加1度观察电机电流(看直流电源),
-* 找到一个电机平稳转动并且电流最小的角度作为HALL_PLACE_OFFSET
+* 娴嬮噺HALL_PLACE_OFFSET閫氱敤鏂瑰紡灏辨槸ST鎺ㄨ崘鐨勯€氳繃澶栧姏甯﹀姩鐢垫満锛�
+* 娴嬮噺鐢垫満U鐩稿弽鐢靛姩鍔垮拰hall 1鐨勪笂鍗囨部涔嬮棿鐨勫樊鍊�
+* 杩欓噷浣跨敤鐨勬槸鍏堥€氳繃hall_sensor_calibrate娴嬮噺hall1锛�2锛�3锛�4锛�5锛�6
+* 瀵瑰簲鐨勮�搴�(鍋忓樊姣旇緝澶�),鐒跺悗鍚�姩鐢垫満锛岃�HALL_PLACE_OFFSET
+* 浠�0寮€濮嬪�鍔狅紝姣忓�鍔�1搴﹁�瀵熺數鏈虹數娴�(鐪嬬洿娴佺數婧�),
+* 鎵惧埌涓€涓�數鏈哄钩绋宠浆鍔ㄥ苟涓旂數娴佹渶灏忕殑瑙掑害浣滀负HALL_PLACE_OFFSET
 */
 #define HALL_PLACE_OFFSET 213.0f
 /* 
@@ -79,8 +79,11 @@ float hall_sensor_get_theta(void){
 		return _hall.theta;
 	}
 	_hall.est_theta = tick_2_s(delta_ticks(_hall.ticks)) * _hall.degree_per_s + _hall.theta;
-	if (_hall.est_theta > _hall.theta + 60.0f) {
-		//_hall.est_theta = _hall.theta + 60.0f;
+	float est_delta = _hall.est_theta - _hall.theta;
+	if (est_delta > 60) {
+		_hall.est_theta = _hall.theta + 60;
+	}else if (est_delta < -60){
+		_hall.est_theta = _hall.theta - 60;
 	}
 	
 	float angle = _hall.est_theta;

Application/FOC/park_clark.h

@@ -8,8 +8,8 @@ static __INLINE void Rev_Park(dq_t *dq, float angle, alpha_beta_t *alpha_bata) {
 	float c,s;
 	normal_sincosf(angle, &s, &c);
 #if 1
-	alpha_bata->alpha = dq->d * c - dq->q * s;
-	alpha_bata->beta = dq->d * s + dq->q * c;
+	alpha_bata->alpha = dq->Vd * c - dq->Vq * s;
+	alpha_bata->beta = dq->Vd * s + dq->Vq * c;
 #else
 	alpha_bata->alpha = dq->d * s + dq->q * c;
 	alpha_bata->beta = dq->d * c - dq->q * s;
@@ -25,8 +25,8 @@ static __INLINE void Park(alpha_beta_t *alpha_beta, float angle, dq_t *dq) {
 	float c,s;
 	normal_sincosf(angle, &s, &c);
 
-	dq->d = alpha_beta->alpha * c + alpha_beta->beta * s;
-	dq->q = -alpha_beta->alpha * s + alpha_beta->beta * c;
+	dq->Id = alpha_beta->alpha * c + alpha_beta->beta * s;
+	dq->Iq = -alpha_beta->alpha * s + alpha_beta->beta * c;
 }
 
 #endif  /* _Park_Clark_H__ */

Application/FOC/ramp_ctrl.c

@@ -3,19 +3,26 @@
 #define RAMP_INTVAL 5 //ms
 void ramp_timer_handler(timer_t *timer);
 
-void ramp_ctrl_init(ramp_t *ramp, float start, float final, u32 duration_ms){
-	timer_cancel(&ramp->timer);
-	ramp->start_point = start;
-	ramp->target = start;
-	ramp->final_point = final;
-	ramp->duration_ms = duration_ms;
-	ramp->steps = (final - start) / (duration_ms / RAMP_INTVAL);
-
+void ramp_ctrl_init(ramp_t *ramp){
+	ramp_clear(ramp);
 	ramp->timer.handler = ramp_timer_handler;
 }
 
 void ramp_clear(ramp_t *ramp) {
-	ramp_ctrl_init(ramp, ramp->start_point, ramp->final_point, ramp->duration_ms);
+	timer_cancel(&ramp->timer);
+	ramp->start_point = 0;
+	ramp->target = 0;
+	ramp->final_point = 0;
+	ramp->duration_ms = 0;
+	ramp->steps = 0;
+}
+
+void ramp_set_target(ramp_t *ramp, float start, float final, u32 duration_ms) {
+	timer_cancel(&ramp->timer);
+	ramp->start_point = start;
+	ramp->final_point = final;
+	ramp->duration_ms = duration_ms;
+	ramp->steps = (final - ramp->start_point) / (duration_ms / RAMP_INTVAL);
 }
 
 void ramp_exc(ramp_t *ramp){
@@ -33,7 +40,7 @@ bool ramp_complete(ramp_t *ramp) {
 void ramp_timer_handler(timer_t *timer) {
 	ramp_t *ramp = (ramp_t *)timer;
 	float target = ramp->target + ramp->steps;
-	if (target > ramp->final_point) {
+	if (abs(target) > abs(ramp->final_point)) {
 		target = ramp->final_point;
 	}
 	ramp->target = target;

Application/FOC/ramp_ctrl.h

@@ -11,11 +11,12 @@ typedef struct {
 	float steps;
 }ramp_t;
 
-void ramp_ctrl_init(ramp_t *ramp, float start, float final, u32 durations);
+void ramp_ctrl_init(ramp_t *ramp);
 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);
 
 #endif /* _RAMP_CTRL_H__ */
 

Application/FOC/svpwm.c

@@ -172,7 +172,7 @@ static void __inline Calc_XYZ(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM
 }
 #endif
 static void __inline ModuleTime(u32 *T4, u32 *T6, u32 PWM_Period) {
-	u32 period = PWM_Period * 95 / 100; //95%的调制
+	u32 period = PWM_Period * 95 / 100; //95%锟侥碉拷锟斤拷
 	if (*T4 + *T6 > period){
 		float ration = ((float)period)/((float)*T4 + (float)*T6);
 		*T4 *= ration;
@@ -252,11 +252,7 @@ void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_t
 	phase_out->midle = midle;
 	phase_out->high = high;	
 }
-/* 7段式SVPWM 
- * 返回设置3相PWM的3个CCR寄存器的值
- * 这里使用的是stm32的PWM mode1,在向上计数时，一旦TIMx_CNT<TIMx_CCR1时通道1为有效电平，否则为无效电平
- * 在向下计数时，一旦TIMx_CNT>TIMx_CCR1时通道1为无效电平(OC1REF=0)，否则为有效 电平(OC1REF=1)。
-*/
+
 void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
 	float alpha = alpha_beta->alpha * 2.0f / 3.0f;
 	float beta  = alpha_beta->beta  * 2.0f / 3.0f;
@@ -414,6 +410,155 @@ void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_ti
 //	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
 }
 
+/* 7娈靛紡SVPWM
+ * 杩斿洖璁剧疆3鐩窹WM鐨�3涓狢CR瀵勫瓨鍣ㄧ殑鍊�
+ * 杩欓噷浣跨敤鐨勬槸stm32鐨凱WM mode1,鍦ㄥ悜涓婅�鏁版椂锛屼竴鏃�IMx_CNT<TIMx_CCR1鏃堕€氶亾1涓烘湁鏁堢數骞筹紝鍚﹀垯涓烘棤鏁堢數骞�
+ * 鍦ㄥ悜涓嬭�鏁版椂锛屼竴鏃�IMx_CNT>TIMx_CCR1鏃堕€氶亾1涓烘棤鏁堢數骞�(OC1REF=0)锛屽惁鍒欎负鏈夋晥 鐢靛钩(OC1REF=1)銆�
+ * 鏁翠釜鏃堕棿鐨勮�绠楋紝鍓嶉潰X,Y,Z閮芥槸涓€鏍风殑锛屽悗闈㈣�绠桝BC涓夌浉鐨刾wm CCR瀵勫瓨鍣ㄥ€肩殑鏃跺€欙紝闇€瑕佹敞鎰忥紝寰堝�缃戠粶鍖呮嫭涔︽湰鐨勮祫鏂欓兘鏄�敤PWM2妯″紡鐨�
+   灏辨槸楂樼數骞崇殑鏃堕棿 pwm_period - ccr,鎴戜滑鐢≒WM1妯″紡,鎵€浠ユ渶鍚巃bc鐨勮�绠楃◢寰�湁浜涗笉涓€鏍�
+*/
+
+void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
+	float alpha = alpha_beta->alpha * SQRT3_BY_2;
+	float beta  = alpha_beta->beta  * SQRT3_BY_2;
+	u32   PWM_Period = PWM_half_period * 2;
+	u8 sector = 0xFF;
+	u32 tA, tB, tC;
+	u32 low, midle, high;
+	float X, Y, Z;
+	float modu = (float)(PWM_Period) / vbus;
+
+	if (beta >= 0.0f) {
+		if (alpha >= 0.0f) {
+			//quadrant I
+			if (ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_2;
+			} else {
+				sector = SECTOR_1;
+			}
+		} else {
+			//quadrant II
+			if (-ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_3;
+			} else {
+				sector = SECTOR_2;
+			}
+		}
+	} else {
+		if (alpha >= 0.0f) {
+			//quadrant IV5
+			if (-ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_5;
+			} else {
+				sector = SECTOR_6;
+			}
+		} else {
+			//quadrant III
+			if (ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_4;
+			} else {
+				sector = SECTOR_5;
+			}
+		}
+	}
+	//X = SQRT3 * beta * modu;
+	X = TWO_BY_SQRT3 * beta * modu;
+	//Y = (1.5f * alpha + SQRT3_BY_2 * beta) * modu;
+	Y = (alpha + ONE_BY_SQRT3 * beta) * modu;
+	//Z = (-1.5f * alpha + SQRT3_BY_2 * beta) * modu;
+	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
+	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;
+	}	
+	phase_out->A = tA;
+	phase_out->B = tB;
+	phase_out->C = tC;
+	phase_out->low = low;
+	phase_out->midle = midle;
+	phase_out->high = high;
+	*sector_out = sector;
+#if 0
+	static int tet_p = 0;
+	if (tet_p++ % 10 == 0) {
+		printf("$%d %d %d;", tA, tB, tC);
+	}
+#endif		
+//	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
+}
+
+
 #if 0
 
 void XYZ_step(void)

Application/FOC/svpwm.h

@@ -5,5 +5,6 @@
 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 /* _SVPWM_H__ */
 

Application/Hal/hal.c

@@ -148,6 +148,11 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc)
     GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+	
+    GPIO_InitStruct.Pin = GAS_SENSOR_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GAS_SENSOR_GPIO_Port, &GPIO_InitStruct);
 
   /* USER CODE BEGIN ADC1_MspInit 1 */
 

Application/Hal/hal.h

@@ -31,7 +31,8 @@
 #define CURR_AMPL_U_GPIO_Port GPIOA
 #define BUS_VOLTAGE_Pin GPIO_PIN_1
 #define BUS_VOLTAGE_GPIO_Port GPIOA
-
+#define GAS_SENSOR_Pin GPIO_PIN_1
+#define GAS_SENSOR_GPIO_Port GPIOB
 #define M1_OCP_Pin GPIO_PIN_11
 #define M1_OCP_GPIO_Port GPIOA
 
@@ -53,6 +54,8 @@
 #define TEMP_SENSOR_ADC_CHANNEL 8
 #define VBUS_SENSOR_ADC_CHANNEL 2
 
+#define SPEED_SENSOR_ADC_CHANNEL 12
+
 #define ADC_REFERENCE_VOLTAGE  3.30f
 
 
@@ -84,6 +87,12 @@
 #define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS开关引入的开关噪声时间 */
 #define TADC   ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) * 2 * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC 采样时间 */
 
+#define START_RAMP_DURATION (100)//ms
+#define SPEED_SAMPLE_INVAL (100) //转把采集的间隔,ms
+#define SPEED_RAMP_DURATION SPEED_SAMPLE_INVAL //设置速度的斜波时间，速度平滑上升或下降
+
+#define MAX_VBUS_VOLTAGE 24.0f
+#define MAX_CURRENT      50.0F
 void Error_Handler(void);
 void SystemClock_Config(void);
 void HAL_GPIO_init(void);

Application/Hal/tim4.c

@@ -26,3 +26,11 @@ void TIM4_Disable(void){
 	HAL_NVIC_DisableIRQ(TIM6_DAC1_IRQn);
 }
 
+
+void TIM4_Enable_IRQ(bool enable){
+	if (enable) {
+		HAL_NVIC_EnableIRQ(TIM6_DAC1_IRQn);
+	}else {
+		HAL_NVIC_DisableIRQ(TIM6_DAC1_IRQn);
+	}
+}

Application/Hal/tim4.h

@@ -1,7 +1,9 @@
 #ifndef _TIM4_H__
 #define _TIM4_H__
+#include "libs/types.h"
 void TIM4_Init(void);
 void TIM4_Enable(void);
 void TIM4_Disable(void);
+void TIM4_Enable_IRQ(bool enable);
 #endif /* _TIM4_H__ */
 

Application/Hal/uart2.c

@@ -39,8 +39,9 @@ void UART2_Init(void){
 	  Error_Handler();
 	}
 	LL_USART_DisableIT_TXE(huart2.Instance);
-	LL_USART_DisableDirectionRx(huart2.Instance);
+	LL_USART_EnableDirectionRx(huart2.Instance);
 	LL_USART_EnableDirectionTx(huart2.Instance);
+	LL_USART_EnableIT_RXNE(huart2.Instance);
 	HAL_NVIC_EnableIRQ(USART2_IRQn);
 }
 
@@ -56,10 +57,11 @@ void UART2_TxData(u8 data) {
 }
 
 __weak bool uart_tx_finished(void) {return true;}
+__weak void uart_rx_put(u8 ch) {};
 void uart2_irq_handler(void) {
 	USART_TypeDef *USARTx = huart2.Instance;
 	if (LL_USART_IsActiveFlag_RXNE(USARTx)){
-
+		uart_rx_put(LL_USART_ReceiveData8(USARTx));
 	}
 	if (LL_USART_IsActiveFlag_TXE(USARTx)) {
 		if (uart_tx_finished()){

Application/Libs/utils.c

@@ -0,0 +1,119 @@
+#include "utils.h"
+
+const u16 shark_crc16_table[] = {
+	0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, 0x8C48, 0x9DC1,
+	0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7, 0x1081, 0x0108, 0x3393, 0x221A,
+	0x56A5, 0x472C, 0x75B7, 0x643E, 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64,
+	0xF9FF, 0xE876, 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
+	0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5, 0x3183, 0x200A,
+	0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C, 0xBDCB, 0xAC42, 0x9ED9, 0x8F50,
+	0xFBEF, 0xEA66, 0xD8FD, 0xC974, 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9,
+	0x2732, 0x36BB, 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
+	0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A, 0xDECD, 0xCF44,
+	0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72, 0x6306, 0x728F, 0x4014, 0x519D,
+	0x2522, 0x34AB, 0x0630, 0x17B9, 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3,
+	0x8A78, 0x9BF1, 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
+	0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70, 0x8408, 0x9581,
+	0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7, 0x0840, 0x19C9, 0x2B52, 0x3ADB,
+	0x4E64, 0x5FED, 0x6D76, 0x7CFF, 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324,
+	0xF1BF, 0xE036, 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
+	0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5, 0x2942, 0x38CB,
+	0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD, 0xB58B, 0xA402, 0x9699, 0x8710,
+	0xF3AF, 0xE226, 0xD0BD, 0xC134, 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E,
+	0x5CF5, 0x4D7C, 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
+	0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB, 0xD68D, 0xC704,
+	0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232, 0x5AC5, 0x4B4C, 0x79D7, 0x685E,
+	0x1CE1, 0x0D68, 0x3FF3, 0x2E7A, 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3,
+	0x8238, 0x93B1, 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
+	0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, 0x7BC7, 0x6A4E,
+	0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
+};
+
+u16 shark_crc16_update_byte(u16 crc, u8 value)
+{
+	return shark_crc16_table[(crc ^ value) & 0xFF] ^ (crc >> 8);
+}
+
+u16 shark_crc16_update(u16 crc, const u8 *data, u16 size)
+{
+	const u8 *end;
+
+	for (end = data + size; data < end; data++) {
+		crc = shark_crc16_update_byte(crc, *data);
+	}
+
+	return crc;
+}
+
+u16 shark_crc16_check(const u8 *data, u16 size)
+{
+	return shark_crc16_update(0, data, size);
+}
+
+
+u32 shark_iap_checksum_init(void)
+{
+	return 0xFFFFFFFFU;
+}
+
+u32 shark_iap_checksum_finish(u32 crc)
+{
+	return crc ^ 0xFFFFFFFFU;
+}
+
+u32 shark_iap_checksum_put(u32 crc, const u8 *buff, u32 length)
+{
+	const u8 *buff_end;
+
+	for (buff_end = buff + length; buff < buff_end; buff++) {
+		u8 value = *buff;
+		u8 i;
+
+		for (i = 0; i < 8; i++) {
+			if (((crc ^ value) & 1) != 0) {
+				crc = (crc >> 1) ^ 0xEDB88320U;
+			} else {
+				crc >>= 1;
+			}
+
+			value >>= 1;
+		}
+	}
+
+	return crc;
+}
+
+
+u16 shark_decode_u16(const u8 *buff)
+{
+	return DECODE_U16(buff);
+}
+
+u32 shark_decode_u24(const u8 *buff)
+{
+	return DECODE_U24(buff);
+}
+
+u32 shark_decode_u32(const u8 *buff)
+{
+	return DECODE_U32(buff);
+}
+
+void shark_encode_u16(u8 *buff, u16 value)
+{
+	buff[0] = value;
+	buff[1] = value >> 8;
+}
+
+void shark_encode_u24(u8 *buff, u32 value)
+{
+	shark_encode_u16(buff, value);
+	buff[2] = value >> 16;
+}
+
+void shark_encode_u32(u8 *buff, u32 value)
+{
+	shark_encode_u24(buff, value);
+	buff[3] = value >> 24;
+}
+

Application/Libs/utils.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "types.h"
+
+#define DECODE_U16(buff) \
+	(U16(buff[1]) << 8 | buff[0])
+
+#define DECODE_U24(buff) \
+	(U32(buff[2]) << 16 | DECODE_U16(buff))
+
+#define DECODE_U32(buff) \
+	(U32(buff[3]) << 24 | DECODE_U24(buff))
+
+u16 shark_crc16_update_byte(u16 crc, u8 value);
+u16 shark_crc16_update(u16 crc, const u8 *data, u16 size);
+u16 shark_crc16_check(const u8 *data, u16 size);
+u16 shark_decode_u16(const u8 *buff);
+u32 shark_decode_u24(const u8 *buff);
+u32 shark_decode_u32(const u8 *buff);
+void shark_encode_u16(u8 *buff, u16 value);
+void shark_encode_u24(u8 *buff, u32 value);
+void shark_encode_u32(u8 *buff, u32 value);
+u32 shark_iap_checksum_put(u32 crc, const u8 *buff, u32 length);
+u32 shark_iap_checksum_finish(u32 crc);
+u32 shark_iap_checksum_init(void);
+

Application/Math/fast_math.h

@@ -6,10 +6,13 @@
 #define TWO_BY_SQRT3			(2.0f * 0.57735026919f)
 #define SQRT3_BY_2				(0.86602540378f)
 #define SQRT3                   (1.73205080757f)
+#define SQRT2_BY_SQRT3          (0.8164966f)
 #define M_PI (3.14159265f)
+#define SQ(x) ((x)*(x))
 void fast_sincos(float angle, float *sin, float *cos);
 void fast_norm_angle(float *angle);
 void normal_sincosf(float angle, float *sin, float *cos);
+
 /**
  * A simple low pass filter.
  *

Project/Motor_PMSM.uvoptx

@@ -197,7 +197,7 @@
       <DebugFlag>
         <trace>0</trace>
         <periodic>1</periodic>
-        <aLwin>0</aLwin>
+        <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
@@ -236,15 +236,11 @@
       <SystemViewers>
         <Entry>
           <Name>System Viewer\ADC1</Name>
-          <WinId>35904</WinId>
-        </Entry>
-        <Entry>
-          <Name>System Viewer\TIM1</Name>
           <WinId>35905</WinId>
         </Entry>
         <Entry>
-          <Name>System Viewer\TIM6</Name>
-          <WinId>35903</WinId>
+          <Name>System Viewer\TIM1</Name>
+          <WinId>35904</WinId>
         </Entry>
       </SystemViewers>
       <DebugDescription>
@@ -287,26 +283,26 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
-  </Group>
-
-  <Group>
-    <GroupName>FOC</GroupName>
-    <tvExp>1</tvExp>
-    <tvExpOptDlg>0</tvExpOptDlg>
-    <cbSel>0</cbSel>
-    <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>2</GroupNumber>
+      <GroupNumber>1</GroupNumber>
       <FileNumber>3</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\foc.c</PathWithFileName>
-      <FilenameWithoutPath>foc.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\App\communication.c</PathWithFileName>
+      <FilenameWithoutPath>communication.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+  </Group>
+
+  <Group>
+    <GroupName>FOC</GroupName>
+    <tvExp>1</tvExp>
+    <tvExpOptDlg>0</tvExpOptDlg>
+    <cbSel>0</cbSel>
+    <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>2</GroupNumber>
       <FileNumber>4</FileNumber>
@@ -326,8 +322,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\foc_task.c</PathWithFileName>
-      <FilenameWithoutPath>foc_task.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\PI_Controller.c</PathWithFileName>
+      <FilenameWithoutPath>PI_Controller.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -338,8 +334,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\PI_Controller.c</PathWithFileName>
-      <FilenameWithoutPath>PI_Controller.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\phase_current.c</PathWithFileName>
+      <FilenameWithoutPath>phase_current.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -350,8 +346,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\phase_current.c</PathWithFileName>
-      <FilenameWithoutPath>phase_current.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\vbus_sensor.c</PathWithFileName>
+      <FilenameWithoutPath>vbus_sensor.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -362,8 +358,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\vbus_sensor.c</PathWithFileName>
-      <FilenameWithoutPath>vbus_sensor.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\ntc_sensor.c</PathWithFileName>
+      <FilenameWithoutPath>ntc_sensor.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -374,8 +370,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\ntc_sensor.c</PathWithFileName>
-      <FilenameWithoutPath>ntc_sensor.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\hall_sensor.c</PathWithFileName>
+      <FilenameWithoutPath>hall_sensor.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -386,8 +382,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\hall_sensor.c</PathWithFileName>
-      <FilenameWithoutPath>hall_sensor.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\ramp_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -398,8 +394,56 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\ramp_ctrl.c</PathWithFileName>
-      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\circle_limitation.c</PathWithFileName>
+      <FilenameWithoutPath>circle_limitation.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>12</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\FOC\gas_sensor.c</PathWithFileName>
+      <FilenameWithoutPath>gas_sensor.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>13</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\Foc\foc_api.c</PathWithFileName>
+      <FilenameWithoutPath>foc_api.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>14</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\Foc\foc_core.c</PathWithFileName>
+      <FilenameWithoutPath>foc_core.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>15</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\Foc\foc_stm.c</PathWithFileName>
+      <FilenameWithoutPath>foc_stm.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -413,7 +457,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>12</FileNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -425,7 +469,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>13</FileNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -437,7 +481,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>14</FileNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -447,6 +491,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>3</GroupNumber>
+      <FileNumber>19</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\Libs\utils.c</PathWithFileName>
+      <FilenameWithoutPath>utils.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -457,7 +513,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -469,7 +525,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -481,7 +537,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -493,7 +549,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -513,7 +569,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -525,7 +581,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -537,7 +593,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -549,7 +605,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -561,7 +617,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -581,7 +637,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -593,7 +649,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -605,7 +661,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -617,7 +673,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -629,7 +685,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -641,7 +697,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -653,7 +709,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -665,7 +721,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -677,7 +733,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -689,7 +745,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -701,7 +757,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -713,7 +769,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -725,7 +781,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -737,7 +793,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -749,7 +805,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -761,7 +817,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -773,7 +829,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -785,7 +841,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -797,7 +853,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -809,7 +865,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -821,7 +877,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -833,7 +889,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -845,7 +901,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -857,7 +913,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -869,7 +925,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -889,7 +945,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -901,7 +957,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/Motor_PMSM.uvprojx

@@ -393,26 +393,21 @@
               <FileType>1</FileType>
               <FilePath>..\Application\App\app.c</FilePath>
             </File>
+            <File>
+              <FileName>communication.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\App\communication.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
           <GroupName>FOC</GroupName>
           <Files>
-            <File>
-              <FileName>foc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Application\FOC\foc.c</FilePath>
-            </File>
             <File>
               <FileName>svpwm.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\Application\FOC\svpwm.c</FilePath>
             </File>
-            <File>
-              <FileName>foc_task.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Application\FOC\foc_task.c</FilePath>
-            </File>
             <File>
               <FileName>PI_Controller.c</FileName>
               <FileType>1</FileType>
@@ -443,6 +438,31 @@
               <FileType>1</FileType>
               <FilePath>..\Application\FOC\ramp_ctrl.c</FilePath>
             </File>
+            <File>
+              <FileName>circle_limitation.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\FOC\circle_limitation.c</FilePath>
+            </File>
+            <File>
+              <FileName>gas_sensor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\FOC\gas_sensor.c</FilePath>
+            </File>
+            <File>
+              <FileName>foc_api.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\Foc\foc_api.c</FilePath>
+            </File>
+            <File>
+              <FileName>foc_core.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\Foc\foc_core.c</FilePath>
+            </File>
+            <File>
+              <FileName>foc_stm.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\Foc\foc_stm.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -463,6 +483,11 @@
               <FileType>1</FileType>
               <FilePath>..\Application\Libs\circle_buffer.c</FilePath>
             </File>
+            <File>
+              <FileName>utils.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\Libs\utils.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>