update for simulink code and Uv projects

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

Applications/app/app.c

@@ -1,35 +1,33 @@
 #include "app/app.h"
 #include "bsp/bsp.h"
-#include "os/timer.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "libs/logger.h"
 #include "libs/utils.h"
+#include "foc/motor/motor.h"
+#include "foc/core/foc_core.h"
 #include "prot/can_foc_msg.h"
 #include "prot/can_message.h"
-#include "foc/core/foc_api.h"
-#include "bsp/timer_count32.h"
+#include "libs/time_measure.h"
 #include "app/nv_storage.h"
 
-static void _app_low_task(void *args);
+static u32 _app_low_task(void *args);
+static void print_backtrace(void);
 extern measure_time_t g_meas_hall;
 extern measure_time_t g_meas_foc;
 void app_start(void){
 	set_log_level(MOD_SYSTEM, L_debug);
 	can_message_init();
 	restore_config();
-	foc_init();
-	log_start_task();
-	co_task_create(_app_low_task, NULL, 512);
-	co_task_schedule();
+	PMSM_FOC_CoreInit();
+	shark_task_create(_app_low_task, NULL);
+	
+	print_backtrace();
+	
+	shark_task_run();
 }
 
 static void _can_report_info(void) {
-	can_report_speed(0x45, foc_get_speed());
-	current_samp_t *s = foc_get_current_sample();
-	can_report_phase_current(0x45, F2I(s->Ia * 1000), F2I(s->Ib * 1000), F2I(s->Ic * 1000));
-	//sys_debug("phase current %f %f %f %f\n", s->Ia, s->Ib, s->Ic, s->max_Ia);
-	sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
-	s->max_Ia = 0.0f;
+	can_report_speed(0x45, mc_get_speed());
 }
 
 static void print_backtrace(void){
@@ -50,17 +48,9 @@ static void print_backtrace(void){
 	}
 }
 
-static void _app_low_task(void *args) {
-	sys_debug("reset src: 0x%x\n", gd32_get_reset_source());
-	print_backtrace();
-	while(1) {
-		wdog_reload();
-		_can_report_info();
-		///sys_debug("foc exec time %d, intval %d, max %d, error %d\n", g_meas_foc.exec_time, g_meas_foc.intval_time, g_meas_foc.exec_max_error_time, g_meas_foc.intval_time_error);
-		//sys_debug("hall exec time %d, intval %d\n", g_meas_hall.exec_time, g_meas_hall.intval_time);
-		//sys_debug("vbus voltage: %f\n", foc_get_vbus_voltage());
-		//hall_debug_log();
-		co_task_delay(1000);
-	}
+static u32 _app_low_task(void *args) {
+	wdog_reload();
+	_can_report_info();
+	return 100;
 }
 

Applications/app/nv_storage.h

@@ -1,6 +1,6 @@
 #ifndef _NV_Storage_H__
 #define _NV_Storage_H__
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 #pragma  pack (push,1)
 

Applications/bsp/adc.c

@@ -1,7 +1,7 @@
 #include "bsp/bsp.h"
 #include "bsp/adc.h"
 #include "libs/utils.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "libs/logger.h"
 static void _gpio_init(void);
 
@@ -150,10 +150,10 @@ s32 adc_sample_regular_channel(int channel, int times) {
 	adc_regular_channel_config(adc_device, 0, channel, ADC_SAMPLETIME_55POINT5);
 	while(count < times){
 restart:		
-		start_time = co_task_sys64_ticks();
+		start_time = shark_get_mseconds();
 		adc_software_trigger_enable(adc_device, ADC_REGULAR_CHANNEL);
     	while(SET != adc_flag_get(adc_device, ADC_FLAG_EOC)){
-			if (co_task_sys64_ticks() - start_time >= 2){
+			if (shark_get_mseconds() - start_time >= 2){
 				goto restart;
 			}
 		};

Applications/bsp/adc.h

@@ -1,7 +1,7 @@
 #ifndef _ADC_H__
 #define _ADC_H__
 #include "bsp/bsp.h"
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 /*
 inserted ADC 由timer0 ch3触发，

Applications/bsp/bsp.c

@@ -1,13 +1,12 @@
 #include "bsp/bsp.h"
 #include "bsp/gd32_bkp.h"
 #include "libs/logger.h"
-#include "os/os_type.h"
+#include "os/os_types.h"
 #include "bsp/uart.h"
 #include "bsp/timer_count32.h"
 #include "version.h"
 
 static void wdog_enable(void);
-static void normal_task_timer_init(void);
 
 void bsp_init(void){
 	wdog_enable();
@@ -15,11 +14,11 @@ void bsp_init(void){
 	dbg_periph_enable(DBG_TIMER0_HOLD);
 	dbg_periph_enable(DBG_TIMER1_HOLD);
 	dbg_periph_enable(DBG_TIMER2_HOLD);
-	cpu_counts_enable();
+	systick_open();
+	task_ticks_enable();
 	timer_count32_init();
 	gpio_pin_init();
 	shark_uart_init(SHARK_UART0);
-	normal_task_timer_init();
 }
 
 
@@ -27,6 +26,17 @@ void system_reboot(void){
 	NVIC_SystemReset();
 }
 
+void systick_close(void)
+{
+	SysTick->CTRL  &= ~SysTick_CTRL_ENABLE_Msk;
+}
+
+void systick_open(void)
+{
+	SysTick_Config(SystemCoreClock / 1000);
+}
+
+
 void wdog_reload(void){
 #if CONFIG_DEBUG == 0
     fwdgt_counter_reload();
@@ -80,6 +90,7 @@ int wdog_set_timeout(int wdog_time)
 }
 
 //10 ms
+#if 0
 static void normal_task_timer_init(void) {
     timer_parameter_struct timer_initpara;
     u32 timer = TIMER5;
@@ -103,4 +114,4 @@ static void normal_task_timer_init(void) {
 	nvic_irq_enable(TIMER5_IRQn, 5, 0);
 	timer_enable(timer);
 }
-
+#endif

Applications/bsp/bsp.h

@@ -13,11 +13,9 @@
 #include "bsp/fmc_flash.h"
 #include "bsp/can.h"
 
-//#define GD32_DEMO 1
+//#define GD32_DEMO
 #define USER_ITMER_BRAKE 0
 
-#define ADC_REFERENCE_VOLTAGE  3300.0f
-
 
 #define SYSTEM_CLOCK (120000000u) //system clk 120M Hz
 #define TIM_CLOCK (SYSTEM_CLOCK) /*SystemClock_Config��TIM1��clk��sys PLL �������̶�2����PLLƵ��*/
@@ -56,7 +54,9 @@
 #define SPEED_SAMPLE_INVAL (100) //ת�Ѳɼ��ļ��,ms
 #define SPEED_RAMP_DURATION SPEED_SAMPLE_INVAL //�����ٶȵ�б��ʱ�䣬�ٶ�ƽ���������½�
 
-#define MAX_VBUS_VOLTAGE 54.0f
+#define MAX_VBUS_VOLTAGE _F2sFix6(54) //sfix(1, 16, 6)
+#define ADC_REFERENCE_VOLTAGE  _F2sFix10(3.3F) //sfix(1, 16, 10)
+
 #define MAX_CURRENT      50.0F
 
 #define pwm_timer TIMER0
@@ -73,11 +73,12 @@
 
 #define SHUNT_NUM THREE_SHUNTS_SAMPLE
 
-#define ENABLE_AUX_TIMER 1
+//#define ENABLE_AUX_TIMER 1
 
 void bsp_init(void);
 void wdog_reload(void);
 void system_reboot(void);
 int wdog_set_timeout(int wdog_time);
+void systick_open(void);
 
 #endif /* __BSP_H__ */

Applications/bsp/can.c

@@ -1,8 +1,9 @@
 #include <stdio.h>
-#include "bsp.h"
-#include "can.h"
-#include "libs/utils.h"
 #include "os/queue.h"
+#include "bsp/bsp.h"
+#include "bsp/can.h"
+#include "libs/utils.h"
+#include "libs/circle_buffer.h"
 
 #define CAN_RX_MESSAGE_RX_ID 1
 #define CAN_SEND_QUEUE_SIZE 32
@@ -12,35 +13,54 @@
 #define CAN_SEND_WAIT_TIMEOUT -2
 
 
-static co_queue_t tx_queue;
-static co_queue_t rx_queue;
-static int shark_send_can0_to_fifo(can_trasnmit_message_struct *P_message);
+#define TX_NUM 32
+#define RX_NUM 64
+static c_buffer_t g_tx_circle;
+static c_buffer_t g_rx_circle;
+static uint8_t _g_tx_buffer[sizeof(can_trasnmit_message_struct) * TX_NUM + 1];
+static uint8_t _g_rx_buffer[sizeof(can_receive_message_struct) * RX_NUM + 1];
+
 static int shark_send_can0_data(can_trasnmit_message_struct *P_message);
-static void _can_tx_task(void *args);
-static void _can_rx_task(void *args);
+static uint8_t can_get_mailbox(uint32_t can_periph);
 /* this function can be overide by app, which need recv the can frame */
 __weak void handle_can_frame(can_id_t id, uint8_t *data, int len){
 
 }
 
-static void _can_rx_task(void *args){
+void can_rx_poll(void){
 	can_receive_message_struct message;
-	while(1) {
-		if (queue_get(rx_queue, &message)) {
-			can_id_t can_id;
-			can_id.id = message.rx_efid;
-			handle_can_frame(can_id, message.rx_data, message.rx_dlen);		
+	if (circle_get_data(&g_rx_circle, (uint8_t *)&message, sizeof(message)) != sizeof(message)) {
+		return;
+	}	
+	can_id_t can_id;
+	can_id.id = message.rx_efid;
+	handle_can_frame(can_id, message.rx_data, message.rx_dlen); 
+	return ;
+}
+
+static void can_tx_poll(void){
+	can_trasnmit_message_struct can_tr_m;
+	while (can_get_mailbox(CAN0) != CAN_NOMAILBOX) {
+		if (circle_get_data(&g_tx_circle, (uint8_t * )&can_tr_m, sizeof(can_tr_m)) != sizeof(can_tr_m)) {
+			break;
 		}
-		co_task_yield();
+		can_message_transmit(CAN0,&can_tr_m);
 	}
 }
 
+static u32 _can_poll_task(void *args) {
+	can_rx_poll();
+	can_tx_poll();
+	return 0;
+}
+
 static __inline__ void can_fifo_recv(int fifo){
-	can_receive_message_struct message;
+	can_receive_message_struct Rxmessage;
+
+	can_message_receive(CAN0, fifo, &Rxmessage);
 
-	can_message_receive(CAN0, fifo, &message);  
+	circle_put_data(&g_rx_circle, (uint8_t *)&Rxmessage,  sizeof(Rxmessage));
 
-	queue_put(rx_queue, &message);
 }
 
 void USBD_LP_CAN0_RX0_IRQHandler(void)
@@ -122,41 +142,35 @@ static void shark_can0_config(void)
 
 
 static int shark_send_can0_data(can_trasnmit_message_struct *P_message){
-	if (queue_put(tx_queue, P_message)){
+	if (circle_put_data(&g_tx_circle, (u8 *)P_message, sizeof(can_trasnmit_message_struct))){
 		return CAN_SEND_OK;
 	}
 	return CAN_SEND_ERROR;
 }
 
-static void _can_tx_task(void *args){
-	can_trasnmit_message_struct trasnmit_msg;
-	while(1) {
-		if (queue_get(tx_queue, &trasnmit_msg)) {
-			shark_send_can0_to_fifo(&trasnmit_msg);
-		}
-		co_task_yield();
-	}
-}
-
-/*when system scheduler is not start or stoped, we MUST not use FreeRTOS 
-system api which can cause sleep,context switch */
-static int shark_send_can0_to_fifo(can_trasnmit_message_struct *P_message){
-	uint8_t mailbox_number = CAN_NOMAILBOX;
-	uint32_t retry_count = 20000;
-	if (CAN_ERR(CAN0) & CAN_ERR_BOERR){
-		shark_can0_reset();
-	}
-	do {
-		mailbox_number = can_message_transmit(CAN0, P_message);
-		if (mailbox_number < CAN_NOMAILBOX){
-			return CAN_SEND_OK;
-		}
-	}while(retry_count-- > 0); //wait sender fifo empty
 
-	shark_can0_reset();	
-	return CAN_SEND_ERROR;
+static uint8_t can_get_mailbox(uint32_t can_periph)
+{
+    uint8_t mailbox_number = CAN_MAILBOX0;
+    
+    /* select one empty mailbox */
+    if(CAN_TSTAT_TME0 == (CAN_TSTAT(can_periph)&CAN_TSTAT_TME0)){
+        mailbox_number = CAN_MAILBOX0;
+    }else if(CAN_TSTAT_TME1 == (CAN_TSTAT(can_periph)&CAN_TSTAT_TME1)){
+        mailbox_number = CAN_MAILBOX1;
+    }else if(CAN_TSTAT_TME2 == (CAN_TSTAT(can_periph)&CAN_TSTAT_TME2)){
+        mailbox_number = CAN_MAILBOX2;
+    }else{
+        mailbox_number = CAN_NOMAILBOX;
+    }
+    /* return no mailbox empty */
+    if(CAN_NOMAILBOX == mailbox_number){
+        return CAN_NOMAILBOX;
+    }
+	return mailbox_number;
 }
 
+
 int shark_can0_send_message(uint32_t can_id, const void*buff, int len){
 	can_trasnmit_message_struct trasnmit_msg;
 	can_id_t can_frame_id;
@@ -199,10 +213,10 @@ void shark_can0_deinit(void){
 }
 
 void shark_can0_init(void){
-	tx_queue = queue_create(64, sizeof(can_trasnmit_message_struct));
-	rx_queue = queue_create(32, sizeof(can_trasnmit_message_struct));
-	co_task_create(_can_tx_task, NULL, 512);
-	co_task_create(_can_rx_task, NULL, 512);
+	circle_buffer_init(&g_tx_circle, _g_tx_buffer, sizeof(_g_tx_buffer));
+	circle_buffer_init(&g_rx_circle, _g_rx_buffer, sizeof(_g_rx_buffer));
+
+	shark_task_create(_can_poll_task, NULL);
 	shark_can0_txrx_pin_config();
 	shark_can0_config();
 }

Applications/bsp/can.h

@@ -1,7 +1,7 @@
 #ifndef _Shark_Can0_h__
 #define _Shark_Can0_h__
 #include <stdio.h>
-#include "os/os_type.h"
+#include "os/os_types.h"
 #include "config.h"
 //CAN DLC lenght
 #define   CAN_DLC_LENGTH     8
@@ -70,6 +70,7 @@ void shark_can0_route_message(uint32_t can_id, const void *buff, int len);
 void shark_can0_init(void);
 void shark_can0_reset(void);
 void shark_can0_deinit(void);
+void can_rx_poll(void);
 
 #endif /* _Shark_Can0_h__ */
 

Applications/bsp/delay.c

@@ -0,0 +1,56 @@
+#include "bsp/bsp.h"
+#include "bsp/delay.h"
+
+
+void task_ticks_enable(void)
+{
+	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+}
+
+uint32_t task_ticks_abs(void)
+{
+	return DWT->CYCCNT;
+}
+
+uint32_t task_ticks_rel(uint32_t start)
+{
+	uint32_t ticks = DWT->CYCCNT;
+
+	if (ticks >= start) {
+		return ticks - start;
+	}
+
+	return 0xFFFFFFFF - start + ticks + 1;
+}
+
+void task_ticks_delay(uint32_t ticks)
+{
+	uint32_t start;
+
+	start = task_ticks_abs();
+	while (task_ticks_rel(start) < ticks);
+}
+
+void task_udelay(uint32_t delay)
+{
+	task_ticks_delay(delay * (SystemCoreClock / 1000000));
+}
+
+
+void delay_us(uint16_t cnt)
+{
+	task_udelay(1);
+}
+
+/*!
+    \brief      delay a time in milliseconds
+    \param[in]  count: count in milliseconds
+    \param[out] none
+    \retval     none
+*/
+void delay_ms(uint32_t count)
+{
+	task_udelay(1000);
+}
+

Applications/bsp/delay.h

@@ -0,0 +1,18 @@
+#ifndef __DELAY_H
+#define __DELAY_H 			   
+
+extern uint32_t utc_seconds;
+
+
+void task_ticks_enable(void);
+uint32_t task_ticks_abs(void);
+
+void systick_close(void);
+void delay_ms(uint32_t count);
+void delay_us(uint16_t cnt);
+void task_udelay(uint32_t delay);
+
+#endif
+
+
+

Applications/bsp/dma.h

@@ -1,7 +1,7 @@
 #ifndef _DMA_H__
 #define _DMA_H__
 #include "bsp/bsp.h"
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 #define PWM_TIMER_CC3_DMA_DEV DMA0
 #define PWM_TIMER_CC3_DMA_CLK RCU_DMA0

Applications/bsp/gd32_bkp.h

@@ -12,7 +12,7 @@
 #define bkp_read_data bkp_data_read
 #endif
 #include "config.h"
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 #define POWER_FIRSTFLAG_REG BKP_DATA_0
 #define POWER_FIRSTFLAG_VALUE 0x5AA5

Applications/bsp/i2c.c

@@ -1,6 +1,7 @@
 #include "bsp.h"
 #include "i2c.h"
 #include "libs/utils.h"
+#include "bsp/delay.h"
 static int _shark_i2c_rw_bytes(uint32_t index, uint8_t address, uint8_t reg, uint8_t *buffer, int length, int write);
 
 #define iic_device(id) ((id == 0)?I2C0:I2C1)

Applications/bsp/mc_hall_gpio.c

@@ -1,11 +1,17 @@
 #include "bsp/mc_hall_gpio.h"
+#include "bsp/timer_count32.h"
+#include "os/os_task.h"
+
 static void _gpio_irq_enable(void);
 
+static hall_io_t g_hall;
+
 void mc_hall_init(void){
 	gpio_init(HALL_1_GROUP,  GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, HALL_1_PIN);
 	gpio_init(HALL_2_GROUP,  GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, HALL_2_PIN);
 	gpio_init(HALL_3_GROUP,  GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, HALL_3_PIN);
-
+	g_hall.is_edged = 0;
+	g_hall.hall_time = 0;
 	_gpio_irq_enable();
 }
 
@@ -33,8 +39,33 @@ int get_hall_stat(int samples) {
 	return (READ_HALL1()) | ((READ_HALL2()) << 1) | ((READ_HALL3()) << 2);
 #endif
 }
-
 #endif
+
+
+u32 hall_get_hwcount(u8 *hall) {
+	hall[0] = READ_HALL1();
+	hall[1] = READ_HALL2();
+	hall[2] = READ_HALL3();
+	if (g_hall.is_edged) {
+		g_hall.is_edged = 0;
+		return g_hall.delta_time;
+	}
+	u32 now = timer_count32_get();
+	return timer_get_5us_ticks(now, g_hall.hall_time);
+}
+
+
+void hall_sensor_handler(void){
+	os_disable_irq();
+	g_hall.is_edged = 1;
+	u32 now = timer_count32_get();
+	g_hall.delta_time = timer_get_5us_ticks(now, g_hall.hall_time);
+	g_hall.hall_time = now;
+	os_enable_irq();
+}
+
+
+
 #ifdef GD32_DEMO
 static void _gpio_irq_enable(void){
 	gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOA, GPIO_PIN_SOURCE_6);

Applications/bsp/mc_hall_gpio.h

@@ -42,8 +42,20 @@ static int __inline__ hall_bit_get(uint32_t gpio_periph,uint32_t pin)
 #define READ_HALL2() (hall_bit_get(HALL_2_GROUP, HALL_2_PIN))
 #define READ_HALL3() (hall_bit_get(HALL_3_GROUP, HALL_3_PIN))
 
+#define HALL1 READ_HALL1()
+#define HALL2 READ_HALL2()
+#define HALL3 READ_HALL3()
+
+typedef struct _hall_io {
+	u8 hall[3];
+	u32 hall_time;
+	u32 delta_time;
+	u8 is_edged;
+}hall_io_t;
+
 void mc_hall_init(void);
 int get_hall_stat(int samples);
+u32 hall_get_hwcount(u8 *hall);
 
 #endif /*_MC_HALL_H__ */
 

Applications/bsp/mc_irqs.c

@@ -107,7 +107,7 @@ void TIMER0_BRK_IRQHandler(void) {
 void TIMER5_IRQHandler(void) {
 	if (timer_interrupt_flag_get(TIMER5, TIMER_INT_FLAG_UP)) {
 		timer_interrupt_flag_clear(TIMER5, TIMER_INT_FLAG_UP);
-		foc_normal_task();
+		//foc_normal_task();
 	}
 }
 

Applications/bsp/pwm.c

@@ -1,7 +1,7 @@
 #include "bsp/bsp.h"
 #include "bsp/pwm.h"
 #include "bsp/adc.h"
-#include "os/timer.h"
+#include "os/os_task.h"
 /*
 以下主要是在某一相电路无法采集的时候，需要对这相的pwm挖坑处理
 timer 分配:
@@ -320,7 +320,7 @@ static void _gpio_brakein_irq_enable(void){
 void pwm_start(void){
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE
 	pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
-	pwm_update_2smaples(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1);
+	pwm_update_1smaples(FOC_PWM_Half_Period-1);
 #else
 	pwm_update_duty_dma(0,0,0,0,0,0);
 	dma_channel_enable(DMA0, DMA_CH4);
@@ -369,7 +369,7 @@ void pwm_turn_on_low_side(void)
 {
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE
 	pwm_update_duty(0, 0, 0);
-	pwm_update_2smaples(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1);
+	pwm_update_1smaples(FOC_PWM_Half_Period-1);
 #else
 	pwm_update_duty_dma(0,0,0,0,0,0);
 	dma_channel_enable(DMA0, DMA_CH4);
@@ -391,7 +391,7 @@ __weak void foc_brake_handler(bool brake) {
 }
 
 /*do 50 times filter*/
-static void brake_timer_handler(timer_t *t) {
+static void brake_timer_handler(shark_timer_t *t) {
 	int count = 50;
 	int settimes = 0;
 	while(count-- >= 0) {
@@ -410,7 +410,7 @@ static void brake_timer_handler(timer_t *t) {
 	}
 }
 
-static timer_t _brake_timer = TIMER_INIT(_brake_timer, brake_timer_handler);
+static shark_timer_t _brake_timer = TIMER_INIT(_brake_timer, brake_timer_handler);
 void mc_brake_irq(void) {
-	timer_post(&_brake_timer, 0);
+	shark_timer_post(&_brake_timer, 0);
 }

Applications/bsp/pwm.h

@@ -1,7 +1,7 @@
 #ifndef _PWM_H__
 #define _PWM_H__
 #include "bsp/bsp.h"
-#include "os/os_type.h"
+#include "os/os_types.h"
 #include "bsp/dma.h"
 
 #define TIMxCCER_MASK_CH012        ((uint16_t)  (TIMER_CHCTL2_CH0EN|TIMER_CHCTL2_CH0NEN|\
@@ -30,7 +30,7 @@
 		TIMER_CH0CV(adc_timer) = (uint32_t)samp2; \
 	}while(0)
 #else
-#define pwm_update_2smaples(samp1, samp2) \
+#define pwm_update_1smaples(samp1) \
 	do { \
 		TIMER_CH3CV(pwm_timer) = (uint32_t)samp1; \
 	}while(0)

Applications/bsp/timer_count32.c

@@ -1,6 +1,6 @@
 #include "bsp/bsp.h"
 #include "timer_count32.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 static void init_master_timer(void);
 static void init_slave_timer(void);
 
@@ -27,7 +27,7 @@ static void init_master_timer(void){
 	timer_deinit(MASTER);
 
 	memset(&timer_initpara, 0, sizeof(timer_initpara));
-	timer_initpara.prescaler		= TIM_CLOCK/COUNT_CLK - 1; //clk 10MHz, 100ns/0.1us
+	timer_initpara.prescaler		= TIM_CLOCK/COUNT_CLK - 1; //clk 200KHz, 5us
 	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
 	timer_initpara.counterdirection = TIMER_COUNTER_UP;
 	timer_initpara.period		   = 0xFFFF;
@@ -68,7 +68,7 @@ static void init_slave_timer(void) {
 }
 
 static __inline__ u32 _timer_count32_get(void) {
-#if 0
+#if 1
 	u16 high;
 	u16 low;
 	do {
@@ -86,29 +86,3 @@ u32 timer_count32_get(void) {
 	return _timer_count32_get();
 }
 
-
-u32 timer_count32_delta_us(u32 count, u32 *p_update) {
-	u32 now = timer_count32_get();
-	u32 delta = now - count;
-	if (now < count) { //wrap
-		delta = 0xFFFFFFFF - count + now + 1;
-	}
-	if (p_update) {
-		*p_update = now;
-	}
-	return COUNT_2_US(delta);
-}
-
-
-void time_measure_start(measure_time_t *m){
-	m->intval_time = timer_count32_delta_us(m->intval_count, &m->intval_count);
-	m->exec_count = timer_count32_get();
-}
-void time_measure_end(measure_time_t *m) {
-	m->exec_time = timer_count32_delta_us(m->exec_count, NULL);
-	if (m->exec_time > m->exec_max_time) {
-		m->exec_time_error ++;
-		m->exec_max_error_time = m->exec_time;
-	}
-}
-

Applications/bsp/timer_count32.h

@@ -1,33 +1,19 @@
 #ifndef _TIMER_COUNT32_H__
 #define _TIMER_COUNT32_H__
-#include "os/os_type.h"
+#include "os/os_types.h"
 
-typedef struct {
-	u32 exec_count;
-	u32 exec_time;
-	u32 intval_count;
-	u32 intval_time;
-	u32 exec_max_time;
-	u32 exec_max_error_time;
-	u32 exec_time_error;
-	u32 intval_time_error;
-}measure_time_t;
 
 void timer_count32_init(void);
 u32  timer_count32_get(void);
-u32 timer_count32_delta_us(u32 prev_count, u32 *p_update);
-void time_measure_start(measure_time_t *m);
-void time_measure_end(measure_time_t *m);
 
-#define COUNT_CLK (10 * 1000000)
-#define COUNT_2_US(c) (c/120)
+#define COUNT_CLK (200 * 1000)
 
-static __inline__ u32 timer_count32_getus(u32 now, u32 count) {
+static __inline__ u32 timer_get_5us_ticks(u32 now, u32 count) {
 	u32 delta = now - count;
 	if (now < count) { //wrap
 		delta = 0xFFFFFFFF - count + now + 1;
 	}
-	return COUNT_2_US(delta);
+	return (delta);
 }
 
 #endif	/* _TIMER_COUNT32_H__ */

Applications/bsp/uart.c

@@ -1,5 +1,5 @@
 #include "uart.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "libs/crc16.h"
 #include "libs/logger.h"
 #include "libs/utils.h"
@@ -163,7 +163,6 @@ static void shark_uart_write(shark_uart_t *uart, const u8 *buff, u16 size)
 		shark_uart_dma_tx(uart);
 		buff += length;
 		size -= length;
-		co_task_schedule();
 	}
 }
 
@@ -265,16 +264,14 @@ static void shark_uart_device_init(shark_uart_t *uart){
 #endif
 }
 
-static void shark_uart_task(void *args)
+static u32 shark_uart_task(void *args)
 {
 	shark_uart_t *uart = (shark_uart_t *)args;
-	while(1) {
-		if(uart->uart_com != 0) {
-			shark_uart_rx(uart);
-			shark_uart_dma_tx(uart);
-		}
-		co_task_yield();
+	if(uart->uart_com != 0) {
+		shark_uart_rx(uart);
+		shark_uart_dma_tx(uart);
 	}
+	return 0;
 }
 
 
@@ -362,7 +359,7 @@ void shark_uart_init(uart_enum_t uart_no)
 	shark_uart_tx_dma_init(uart);
 	usart_enable(uart->uart_com);
 
-	co_task_create(shark_uart_task, uart, 256);
+	shark_task_create(shark_uart_task, uart);
 #if ENABLE_RX_DMA==0
 	nvic_irq_enable(SHARK_UART0_irq, 3, 0);
 #endif

Applications/bsp/uart.h

@@ -1,6 +1,6 @@
 #pragma once
 #include "bsp/bsp.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "libs/byte_queue.h"
 #include "libs/circle_buffer.h"
 

Applications/foc/commands.c

@@ -1,4 +1,4 @@
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "os/queue.h"
 #include "libs/logger.h"
 #include "libs/utils.h"
@@ -6,57 +6,43 @@
 #include "bsp/bsp.h"
 #include "bsp/pwm.h"
 #include "bsp/adc.h"
-#include "foc/core/foc_api.h"
+
 #include "foc/core/foc_core.h"
-#include "foc/motor/hall.h"
+#include "foc/motor/motor.h"
 #include "foc/commands.h"
 #include "prot/can_foc_msg.h"
 #include "app/nv_storage.h"
-extern motor_foc_t g_foc;
 
-static void foc_cmd_task(void *args);
+static u32 foc_command_task(void *args);
 static void process_foc_command(foc_cmd_body_t *command);
 
 static co_queue_t _cmd_queue;
 
 void foc_command_init(void) {
 	_cmd_queue = queue_create(16, sizeof(foc_cmd_body_t));
-	co_task_create(foc_cmd_task, NULL, 512);
+	shark_task_create(foc_command_task, NULL);
 }
 
 bool foc_send_command(foc_cmd_body_t *command) {
 	return queue_put(_cmd_queue, command);
 }
 
-static void foc_cmd_task(void *args) {
+static u32 foc_command_task(void *args) {
 	foc_cmd_body_t command;
-	while(1) {
-		if (queue_get(_cmd_queue, &command)) {
-			process_foc_command(&command);
-			if (command.data) {
-				co_free(command.data);
-			}
+	if (queue_get(_cmd_queue, &command)) {
+		process_foc_command(&command);
+		if (command.data) {
+			os_free(command.data);
 		}
-		co_task_yield();
 	}
+	return 0;
 }
 
 
 static void do_hall_calibrate(u8 can_addr, float vd) {
-	sys_debug("cali hall phase, %d, %f\n", g_foc.state, vd);
-	if (g_foc.state == IDLE) {
-		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 1);
-		pwm_turn_on_low_side();
-		delay_ms(10);
-		int result = hall_sensor_calibrate(vd);
-		if (result) {
-			store_hall_table(hall_get_table());
-		}
-		sys_debug("hall phase cali %d\n", result);
-		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Hall_Phase_Cali_Result), result);
-	}else {
-		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 0);
-	}
+	sys_debug("cali hall phase, %f\n", vd);
+	PMSM_FOC_HallCalibrate(true, vd);
+	can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 0);
 }
 
 
@@ -65,38 +51,37 @@ static void process_foc_command(foc_cmd_body_t *command) {
 	switch (command->cmd) {
 		case Foc_Start_Motor:
 		{
-			foc_fault_t f;
-			foc_start_stop_t start = (foc_start_stop_t)decode_u8((u8 *)command->data);
-			if (start == Foc_Start) {
-				f = foc_start_motor();
-			}else if (start == Foc_Stop) {
-				f = foc_stop_motor();
+			u8 erroCode = 0;
+			bool success;
+			foc_start_cmd_t *scmd = (foc_start_cmd_t *)command->data;
+			if (scmd->start_stop == Foc_Start) {
+				success = mc_start(TRQ_MODE);
+			}else if (scmd->start_stop == Foc_Stop) {
+				success = mc_stop();
+			}
+			if (!success) {
+				erroCode = PMSM_FOC_GetErrCode();
 			}
-			sys_debug("start motor %d\n", f);
-			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)f);
+			sys_debug("start motor %d\n", erroCode);
+			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)erroCode);
 			break;
 		}
-		case Foc_Cali_Hall_Phase:
-			do_hall_calibrate(command->can_src, decode_s32((u8 *)command->data));
+		case Foc_Set_Cruise_Mode:
+		{
+			u8 enable = decode_u8(command->data);
+			PMSM_FOC_EnableCruise(enable);
 			break;
-		case Foc_Cali_Hall_Offset:
+		}
+		case Foc_Cali_Hall_Phase:
 		{
-			int offset = 0;
-			u8 inc = decode_u8((u8 *)command->data);
-			if (inc) {
-				offset = hall_offset_increase(1);
-			}else {
-				offset = hall_offset_increase(-1);
-			}
-			sys_debug("cali hall offset %d\n", offset);
-			can_response_hall_offset(command->can_src, offset);
+			do_hall_calibrate(command->can_src, decode_s32((u8 *)command->data));
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:
 		{
-			s32 v_q = decode_s32(((u8 *)command->data) + 4);
-			sys_debug("set v_q %d\n", v_q);
-			foc_set_voltage_ramp(v_q);
+			s32 vq = decode_s32(((u8 *)command->data) + 4);
+			sys_debug("set v_q %d\n", vq);
+			PMSM_FOC_SetOpenVdq(0, vq);
 			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Set_Open_Dq_Vol), 1);
 			break;
 		}

Applications/foc/commands.h

@@ -1,6 +1,6 @@
 #ifndef _FOC_CMD_H__
 #define _FOC_CMD_H__
-#include "os/os_type.h"
+#include "os/os_types.h"
 #include "config.h"
 
 typedef enum {
@@ -11,6 +11,7 @@ typedef enum {
 	Foc_Set_Speed_Limit,     //u32, set the limited speed//rpm
 	Foc_Set_Speed_Target,    //u32, set the target speed, just for test
 	Foc_Set_Cruise_Speed,    //u32, set the speed for curise riding
+	Foc_Set_Cruise_Mode,
 	Foc_Hall_Phase_Cali_Result,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,
@@ -27,7 +28,7 @@ typedef enum {
 typedef enum {
 	Foc_Start = 1,
 	Foc_Stop,
-}foc_start_stop_t;
+}FSwith_t;;
 
 typedef struct {
 	foc_cmd_t cmd;
@@ -37,9 +38,10 @@ typedef struct {
 
 #pragma  pack (push,1)
 typedef struct {
-	foc_start_stop_t start_stop; //1: start, 2: stop
+	u8 start_stop; //1: start, 2: stop
 }foc_start_cmd_t;
 
+
 typedef struct {
 	u32 vq_mvol; //mV
 }hall_phase_cali_cmd_t;

Applications/foc/core/circle_limitation.c

@@ -1,13 +0,0 @@
-#include "circle_limitation.h"
-
-void circle_limitation(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;
-	}
-}
-

Applications/foc/core/circle_limitation.h

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

Applications/foc/core/foc_api.c

@@ -1,196 +0,0 @@
-#include <string.h>
-#include "os/os_type.h"
-#include "libs/utils.h"
-#include "libs/logger.h"
-#include "bsp/bsp.h"
-#include "bsp/adc.h"
-#include "bsp/pwm.h"
-#include "foc/core/foc_api.h"
-#include "foc/core/park_clark.h"
-#include "foc/core/svpwm.h"
-#include "foc/core/foc_fsm.h"
-#include "foc/core/foc_core.h"
-#include "foc/motor/current.h"
-#include "foc/motor/hall.h"
-#include "foc/motor/motor.h"
-#include "foc/samples.h"
-#include "foc/commands.h"
-
-extern motor_foc_t g_foc;
-static void foc_defulat_value(void);
-
-void foc_init(void) {
-	foc_defulat_value();
-	adc_init();
-	pwm_3phase_init();
-	hall_sensor_init();
-	foc_core_init();
-	foc_command_init();
-}
-
-static void foc_defulat_value(void){
-	g_foc.state = IDLE;
-	g_foc.mosfec_gate = false;
-	g_foc.vbus = MAX_VBUS_VOLTAGE;
-	g_foc.mode = FOC_MODE_OPEN_LOOP;
-	g_foc.alpha_beta.alpha = 0;
-	g_foc.alpha_beta.beta = 0;
-	g_foc.dq_command.Id = 0;
-	g_foc.dq_command.Iq = 0;
-	g_foc.foc_fault = foc_success;
-	g_foc.speed_command.speed = -1;
-	g_foc.speed_command.delta_ts = 0;
-	memset(&g_foc.phase_time, 0, sizeof(g_foc.phase_time));
-	g_foc.sector = 0;
-	g_foc.dq_last.Id = 0;
-	g_foc.dq_last.Iq = 0;
-	phase_current_init(&g_foc.current_samp);
-	ramp_ctrl_init(&g_foc.voltage_ramp);
-	ramp_ctrl_init(&g_foc.current_ramp);
-	ramp_ctrl_init(&g_foc.speed_ramp);
-	pi_clear(&g_foc.id_controller);
-	pi_clear(&g_foc.iq_controller);
-	pi_clear(&g_foc.speed_controller);
-}
-
-static bool gas_detect_speed_signal(void) {
-	return false;
-}
-
-current_samp_t *foc_get_current_sample(void) {
-	return &g_foc.current_samp;
-}
-
-
-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_stop(void) {
-	pwm_stop();
-	foc_start_adc(false);
-	g_foc.mosfec_gate = false;
-	foc_defulat_value();
-	hall_sensor_clear();
-}
-
-void foc_start_adc(bool start) {
-	if (start) {
-		adc_start_convert();
-	}else {
-		adc_stop_convert();
-	}
-}
-
-void foc_clear(void) {
-	g_foc.alpha_beta.alpha = 0;
-	g_foc.alpha_beta.beta = 0;
-	g_foc.dq_command.Id = 0;
-	g_foc.dq_command.Iq = 0;
-	g_foc.foc_fault = foc_success;
-	g_foc.speed_command.speed = -1;
-	g_foc.speed_command.delta_ts = 0;
-	memset(&g_foc.phase_time, 0, sizeof(g_foc.phase_time));
-	g_foc.sector = 0;
-	g_foc.dq_last.Id = 0;
-	g_foc.dq_last.Iq = 0;
-	phase_current_init(&g_foc.current_samp);
-	ramp_ctrl_init(&g_foc.voltage_ramp);
-	ramp_ctrl_init(&g_foc.current_ramp);
-	ramp_ctrl_init(&g_foc.speed_ramp);
-	pi_clear(&g_foc.id_controller);
-	pi_clear(&g_foc.iq_controller);
-	pi_clear(&g_foc.speed_controller);
-}
-
-u32 foc_get_speed(void) {
-	u32 speed = motor_get_speed()/(motor_params.poles);
-	return abs(speed);
-}
-
-
-void foc_set_dq_command(float d, float q) {
-	g_foc.dq_command.Vd = d;
-	g_foc.dq_command.Vq = q;
-}
-
-void foc_set_voltage_ramp(float final){
-	sys_debug("voltage %f -> %f\n", g_foc.dq_last.Vq, final);
-	ramp_set_target(&g_foc.voltage_ramp, g_foc.dq_last.Vq, final, START_RAMP_DURATION);
-	ramp_exc(&g_foc.voltage_ramp);
-}
-
-void foc_set_speed_ramp(u16 rpm){
-	if ((g_foc.speed_command.delta_ts == 0) || (g_foc.speed_command.delta_ts > START_RAMP_DURATION)) {
-		g_foc.speed_command.delta_ts = START_RAMP_DURATION;
-	}
-	ramp_set_target(&g_foc.speed_ramp, foc_get_speed(), rpm, g_foc.speed_command.delta_ts);
-	ramp_exc(&g_foc.speed_ramp);
-}
-
-void foc_set_controller_mode(control_mode_t mode) {
-	g_foc.mode = mode;
-}
-
-void foc_set_speed(u16 rpm, u32 delta_ms) {
-	if (g_foc.state == IDLE || g_foc.state == ANY_STOP) {
-		return;
-	}
-	g_foc.speed_command.speed = rpm;
-	g_foc.speed_command.delta_ts = delta_ms;
-	if (g_foc.mode == FOC_MODE_OPEN_LOOP) {
-		foc_set_voltage_ramp(speed_to_voltage(rpm));
-	}
-}
-
-foc_fault_t foc_start_motor(void){
-	if (gas_detect_speed_signal()) {
-		return foc_start_with_gas_error;
-	}
-	return foc_fsm_next_state(START);
-}
-
-foc_fault_t foc_stop_motor(void) {
-	if (foc_get_speed() > 0) {
-		return foc_stop_with_speed_error;
-	}
-	return foc_fsm_next_state(ANY_STOP);
-}
-
-bool foc_motor_is_started(void) {
-	return g_foc.state >= START;
-}
-
-void foc_current_calibrate(void){
-	g_foc.current_samp.adc_offset_a = 0;
-	g_foc.current_samp.adc_offset_b = 0;
-	g_foc.current_samp.adc_offset_c = 0;
-
-	phase_current_init(&g_foc.current_samp);
-	g_foc.current_samp.is_calibrating_offset = true;
-	g_foc.current_samp.sector = SECTOR_5;
-	adc_current_sample_config(g_foc.current_samp.sector);
-	motor_start();
-	while(g_foc.current_samp.offset_sample_count != 0){};
-#if SHUNT_NUM==THREE_SHUNTS_SAMPLE
-	motor_stop();
-	cpu_udelay(100);
-	phase_current_init(&g_foc.current_samp);	
-	g_foc.current_samp.sector = SECTOR_1;
-	adc_current_sample_config(g_foc.current_samp.sector);
-	motor_start();
-	while(g_foc.current_samp.offset_sample_count != 0){};
-#endif
-	g_foc.current_samp.is_calibrating_offset = false;
-}
-
-float foc_get_vbus_voltage(void){
-	return g_foc.vbus;
-}
-
-

Applications/foc/core/foc_api.h

@@ -1,28 +0,0 @@
-#ifndef _FOC_API_H__
-#define _FOC_API_H__
-#include "foc/core/foc_type.h"
-
-void foc_init(void);
-void foc_clear(void);
-void foc_stop(void);
-void foc_set_controller_mode(control_mode_t mode);
-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_set_dq_command(float d, float q);
-void foc_set_voltage_ramp(float final);
-void foc_set_speed_ramp(u16 rpm);
-void foc_current_calibrate(void);
-u32 foc_get_speed(void);
-float foc_get_vbus_voltage(void);
-bool foc_motor_is_started(void);
-void foc_set_speed(u16 rpm, u32 delta_ms);
-float speed_to_voltage(u16 rpm);
-float speed_to_current(u16 rpm);
-void foc_start_adc(bool start);
-current_samp_t *foc_get_current_sample(void);
-
-#endif /* _FOC_API_H__ */
-

Applications/foc/core/foc_core.c

@@ -1,254 +1,171 @@
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "bsp/pwm.h"
 #include "bsp/adc.h"
+#include "bsp/mc_hall_gpio.h"
 #include "foc/core/foc_core.h"
-#include "foc/core/foc_api.h"
-#include "foc/core/foc_fsm.h"
 #include "foc/motor/current.h"
-#include "foc/core/park_clark.h"
-#include "foc/motor/hall.h"
-#include "foc/motor/ntc.h"
-#include "foc/motor/motor.h"
-#include "foc/core/circle_limitation.h"
-#include "foc/samples.h"
-#include "foc/core/svpwm.h"
 #include "bsp/timer_count32.h"
+#include "libs/time_measure.h"
 #include "libs/logger.h"
 
-motor_foc_t g_foc = {
-	.id_controller = {
-		.Kp_gain = 9,
-		.Ki_gain = 1071,
-		.max_output = MAX_VBUS_VOLTAGE,
-		.min_output = -MAX_VBUS_VOLTAGE,		
-	},
-	.iq_controller = {
-		.Kp_gain = 10,
-		.Ki_gain = 1080,
-		.max_output = MAX_VBUS_VOLTAGE,
-		.min_output = -MAX_VBUS_VOLTAGE,
-	},
-	.speed_controller = {
-		.Kp_gain = 1,
-		.Ki_gain = 200,
-		.max_output = MAX_CURRENT,
-		.min_output = -MAX_CURRENT,
-	},	
-};
-
-
-static void foc_sample_task(void*);
-
-void foc_core_init(void) {
-	samples_init();
-	ntc_sensor_init();
-	co_task_create(foc_sample_task, NULL, 256);
-}
-
-#if 1
-static void __inline foc_update_theta(motor_foc_t *foc) {
-	foc->motor_stat.angle = hall_sensor_get_theta();
-	foc->motor_stat.theta = degree_2_pi(foc->motor_stat.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();
+pmsm_foc_t pmsm_foc = {0};
+
+extern void PMSM_FOC_Init(void);
+extern ExtU *PMSM_FOC_GetInputs(void);
+extern ExtY *PMSM_FOC_GetOutputs(void);
+extern void PMSM_FOC_Step(void);
+
+void PMSM_FOC_CoreInit(void) {
+	mc_hall_init();
+	PMSM_FOC_Init();
+	pmsm_foc.FOC_In  = PMSM_FOC_GetInputs();
+	pmsm_foc.FOC_Out = PMSM_FOC_GetOutputs();
+}
+
+static __INLINE void PMSM_FOC_Controller(void) {
+	u8  hall[3];
+	pwm_clear_updata();
+	phase_current_sample(&pmsm_foc.FOC_In->adc_a, &pmsm_foc.FOC_In->adc_b);
+	pmsm_foc.FOC_In->hw_count = hall_get_hwcount(hall);
+	pmsm_foc.FOC_In->hall_a = hall[0];
+	pmsm_foc.FOC_In->hall_b = hall[1];
+	pmsm_foc.FOC_In->hall_c = hall[2];
+	pmsm_foc.FOC_In->input_target = (s16)ramp_get_target(&pmsm_foc.speed_ramp);
+
+	if (pmsm_foc.b_brake_in) {
+		pmsm_foc.FOC_In->b_motEna = false;
 	}else {
-		angle += 0.5f;
+		pmsm_foc.FOC_In->b_motEna = pmsm_foc.b_FocEna;
 	}
-	fast_norm_angle(&angle);
-	foc->motor_s.angle = angle;
-	foc->motor_s.theta = degree_2_pi(angle);
-}
-#endif
+	
+	PMSM_FOC_Step();
 
+	pwm_update_duty(pmsm_foc.FOC_Out->PWM[0], pmsm_foc.FOC_Out->PWM[1], pmsm_foc.FOC_Out->PWM[2]);
+	pwm_update_1smaples(pmsm_foc.FOC_Out->PWM[6]);
+	adc_current_sample_config(pmsm_foc.FOC_Out->sector);
+}
 
-static void __inline foc_calc_voltage(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
-	//float vd = pi_control(&foc->PI_id, foc->dq_ref.Id - sampled->Id);
-	//float vq = pi_control(&foc->PI_iq, foc->dq_ref.Iq - sampled->Iq);
-	if (foc->mode == FOC_MODE_CURRENT_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP) {
-		ref_out->Vd = pi_control(&foc->id_controller, foc->dq_command.Id - sampled->Id);
-		ref_out->Vq = pi_control(&foc->iq_controller, foc->dq_command.Iq - sampled->Iq);
-		//printf("vd = %f, vq = %f\n", vd, vq);
-	}else {
-		ref_out->Vd = foc->dq_command.Vd;
-		ref_out->Vq = foc->dq_command.Vq;
+void PMSM_FOC_Start(u8 nCtrlMode) {
+	if (pmsm_foc.b_FocEna) {
+		return;
 	}
-	foc->dq_last.Vd = ref_out->Vd;
-	foc->dq_last.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());
+	pmsm_foc.b_FocEna = true;
+	pmsm_foc.FOC_In->b_motEna = true;
+	pmsm_foc.FOC_In->n_ctrlModReq = nCtrlMode;
+	pmsm_foc.FOC_In->b_cruiseEna = 0;
+	pmsm_foc.FOC_In->b_hall_calibrate = 0;
+	pmsm_foc.FOC_In->input_target = 0;
+	pmsm_foc.FOC_In->vd_open_target = 0;
+	pmsm_foc.FOC_In->vq_open_target = 0;
+}
+
+void PMSM_FOC_Stop(void) {
+	if (!pmsm_foc.b_FocEna) {
+		return;
 	}
-#endif	
+	pmsm_foc.b_FocEna = false;
+	memset(pmsm_foc.FOC_In, 0, sizeof(ExtU));
 }
 
-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	
+void PMSM_FOC_iBusLimit(int16_T ibusLimit) {
+	pmsm_foc.FOC_In->i_dc_limit = ibusLimit;
 }
 
-/* FOC 主控制任务 */
-void do_motor_foc(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;
+void PMSM_FOC_SpeedLimit(int16_T speedLimit) {
+	pmsm_foc.FOC_In->speed_limit = speedLimit;
+}
 
-	pwm_clear_updata();
-	/* 更新电角度 */
-	foc_update_theta(foc);
-	/* 采集相电流 */
-	phase_current_sample(c_sample);
-		
-	/* ABC三相坐标到alpha-beta坐标 */
-	do_clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
-	/* alpha-beta坐标系到D-Q旋转坐标系 */
-	do_park(&sample_ab, foc->motor_stat.theta, &sample_dq);
-	/* 电流环，输出电压给SVPWM */
-	foc_calc_voltage(foc, &sample_dq, &v_dq);
-	/* 确保电压在6个扇区的内切圆中 */
-	circle_limitation(&v_dq, foc->vbus, 0.96f);
-	/* d-q坐标系到alpha-beta坐标系，输出给svpwm */
-	Rev_Park(&v_dq, foc->motor_stat.theta, &pwm_ab);
-	/* SVPWM，获取三相逆变器的开关时间，用的是pwm1模式，如果是pwm2模式，这个函数需要修改 */
-	svpwm_get_phase_time(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &c_sample->time, &foc->sector);
-	/* 计算三相电流的采样点 */
-	get_phase_sample_point(c_sample, foc->sector);
-	/* 死区补偿 */
-	deadtime_compensation(c_sample, &c_sample->time);
-	/* 更新 TIM1的CCR0-2，生成互补pwm, 相电流更新采样点 */
-	motor_update_duty(c_sample->time.A, c_sample->time.B, c_sample->time.C, c_sample->time.A_next, c_sample->time.B_next, c_sample->time.C_next);
-	/* 更新采样点 */
-	motor_update_sample(c_sample->time.Samp_p1, c_sample->time.Samp_p2, c_sample->sector);
-
-	Debug_Log(foc);
-
-	Debug_dq(&sample_dq);
-}
-
-/* 计算电流环的参考输入 */
-void foc_calc_current(motor_foc_t *foc) {
-	float speed_ref = ramp_get_target(&foc->speed_ramp);
-	float speed_feedback = foc_get_speed();
-	float vq_out = pi_control(&foc->speed_controller, speed_ref - speed_feedback);	
-	if (foc->mode == FOC_MODE_SPEED_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP){
-		foc->dq_command.Iq = vq_out;
-		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0		
-	}else {
-		foc->dq_command.Iq = ramp_get_target(&foc->current_ramp);
-		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0
-	}
+void PMSM_FOC_VbusVoltage(int16_T vbusVol) {
+	pmsm_foc.FOC_In->vbus_voltage = vbusVol;
 }
 
-void foc_speed_ramp(motor_foc_t *foc){
-	if (foc->speed_command.speed >= 0 && foc->mode != FOC_MODE_OPEN_LOOP){
-		u16 current_rpm = foc_get_speed();
-		u16 ref_rpm = foc->speed_command.speed;
-		foc->speed_command.speed = -1;
-		
-		if (ref_rpm + RPM_FOR_CLOSE_LOOP < current_rpm){
-			foc_set_voltage_ramp(speed_to_voltage(ref_rpm));
-			foc->mode = FOC_MODE_OPEN_LOOP;
-		}else {
-			foc_set_speed_ramp(ref_rpm);
+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->vd_open_target = vd;
+	pmsm_foc.FOC_In->vq_open_target = vq;
+}
+
+bool PMSM_FOC_EnableCruise(boolean_T enable) {
+	if (enable) {
+		if (pmsm_foc.FOC_Out->rpm < 100) { //
+			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->rpm, 500);
 	}
+	pmsm_foc.FOC_In->b_cruiseEna = enable;
+	return true;
 }
 
-void foc_brake_handler(bool brake) {
-	g_foc.is_brake_in = brake;
+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);
+	}
+	return true;
 }
 
-void foc_pwm_up_handler(void){
-	phase_current_adc_triger(&g_foc.current_samp);
+bool PMSM_FOC_Set_CruiseSpeed(s16 rpm) {
+	if (pmsm_foc.FOC_In->b_cruiseEna) {
+		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, 0);
+		return true;
+	}
+	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
+	return false;
 }
 
-measure_time_t g_meas_foc = {.exec_max_time = 15,};
-/*ADC 电流采集中断，调用FOC的核心处理函数*/
-void mc_phase_current_irq(void) {
-	if (g_foc.current_samp.is_calibrating_offset) {
-		phase_current_offset(&g_foc.current_samp);
+void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, int16_T open_vd) {
+	if (pmsm_foc.FOC_In->b_hall_calibrate == b_caliHall) {
 		return;
 	}
-	time_measure_start(&g_meas_foc);
-	do_motor_foc(&g_foc);
-	time_measure_end(&g_meas_foc);
-	if (g_meas_foc.intval_time < 62 || g_meas_foc.intval_time > 62) {
-		//log_chan_value(2, g_meas_foc.intval_time);
+	if (pmsm_foc.FOC_In->b_motEna == b_caliHall) { //motor must be stoped when start cali
+		return;
 	}
+	pmsm_foc.FOC_In->b_motEna = b_caliHall;
+	pmsm_foc.FOC_In->b_hall_calibrate = b_caliHall;
+	pmsm_foc.FOC_In->vq_open_target = 0;
+	pmsm_foc.FOC_In->vd_open_target = open_vd;
+	pmsm_foc.FOC_In->n_ctrlModReq = 0;
 }
 
-void foc_pwm_start(bool start) {
-	if (start == g_foc.mosfec_gate) {
-		return;
-	}
-	if (start) {
-		pwm_start();
-	}else {
-		pwm_stop();
+s16 PMSM_FOC_GetSpeed(void) {
+	return pmsm_foc.FOC_Out->rpm;
+}
+
+
+void PMSM_FOC_SetErrCode(u8 code) {
+	pmsm_foc.error_code = code;
+}
+
+
+u8 PMSM_FOC_GetErrCode(void) {
+	return pmsm_foc.error_code;
+}
+
+void foc_brake_handler(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;
 	}
-	g_foc.mosfec_gate = start;	
 }
 
-/*10ms 定时任务，主要处理foc状态机(里面包含速度环)*/
-void foc_normal_task(void) {
-	foc_fsm(&g_foc);
+void foc_pwm_up_handler(void){
+	phase_current_adc_triger();
 }
 
-static void foc_sample_task(void *args){
-//	u64 ts = co_task_sys64_ts();
-	while(1) {
-		ntc_sensor_sample();
-		g_foc.vbus = get_vbus_sample();
-		wdog_reload();
-		co_task_yield();
+measure_time_t g_meas_foc = {.exec_max_time = 15,};
+#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
+#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
+
+/*ADC 电流采集中断，调用FOC的核心处理函数*/
+void mc_phase_current_irq(void) {
+	if (phase_current_offset()) {//check if is adc offset checked
+		return;
 	}
+	TIME_MEATURE_START();
+	PMSM_FOC_Controller();
+	TIME_MEATURE_END();
 }
 

Applications/foc/core/foc_core.h

@@ -1,10 +1,23 @@
 #ifndef _FOC_CORE_H__
 #define _FOC_CORE_H__
 #include "foc_type.h"
-void do_motor_foc(motor_foc_t *foc);
-void foc_calc_current(motor_foc_t *foc);
-void foc_speed_ramp(motor_foc_t *foc);
-void foc_core_init(void);
+
+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);
+void PMSM_FOC_VbusVoltage(int16_T vbusVol);
+void PMSM_FOC_SetCtrlMode(uint8_T mode);
+void PMSM_FOC_SetOpenVdq(int16_T vd, int16_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);
+void PMSM_FOC_SetErrCode(u8 code);
+u8 PMSM_FOC_GetErrCode(void);
+
 
 #endif /* _FOC_CORE_H__ */
 

Applications/foc/core/foc_fsm.c

@@ -1,108 +0,0 @@
-#include <string.h>
-#include "bsp/bsp.h"
-#include "bsp/pwm.h"
-#include "foc/core/foc_api.h"
-#include "foc/core/foc_core.h"
-#include "foc/motor/hall.h"
-#include "foc/motor/motor.h"
-
-extern motor_foc_t g_foc;
-
-foc_state_t foc_fsm_state(void){
-	return g_foc.state;
-}
-
-foc_fault_t foc_fsm_next_state(foc_state_t state) {
-	bool changed = false;
-	if (state == g_foc.state) {
-		return foc_success;
-	}
-	if (state == START) {
-		if (g_foc.state == IDLE) {
-			changed = true;
-		}
-	}else if (state == IDLE) {
-		if (g_foc.state == ANY_STOP) {
-			changed = true;
-		}
-	}else if (state == ANY_STOP) {
-		if (g_foc.state != IDLE) {
-			changed = true;
-		}
-	}else if (state == CURRENT_CALIBRATE) {
-		if (g_foc.state == START) {
-			changed = true;
-		}
-	}else if (state == READY_TO_RUN) {
-		if (g_foc.state == CURRENT_CALIBRATE) {
-			changed = true;
-		}
-	}else if (state == RAMPING_START) {
-		if (g_foc.state == READY_TO_RUN || g_foc.state == RUNNING) {
-			changed = true;
-		}
-	}
-	if (changed) {
-		g_foc.state = state;
-		return foc_success;
-	}
-	return foc_not_allowed;
-}
-
-void foc_fsm(motor_foc_t *foc) {
-	switch (foc->state) {
-		case IDLE:
-			foc->mode = FOC_MODE_OPEN_LOOP;
-			break;
-		case START:
-			foc_clear();
-			motor_drvier_low_side(true);
-			foc_fsm_next_state(CURRENT_CALIBRATE);
-			break;
-		case CURRENT_CALIBRATE:
-			foc_current_calibrate();
-			foc_fsm_next_state(READY_TO_RUN);
-			break;
-		case READY_TO_RUN:
-			foc_pwm_start(true);
-			foc_fsm_next_state(RAMPING_START);
-			break;
-		case RAMPING_START:
-			foc_set_dq_command(0.0f, ramp_get_target(&foc->voltage_ramp));
-			//printf("target %f\n", ramp_get_target(&foc->voltage_ramp));
-			if (foc_get_speed() >= RPM_FOR_CLOSE_LOOP){
-				//foc_fsm_next_state(CLOSED_LOOP);
-				//foc_overide_vdq(false);
-			}
-			break;
-		// case CLOSED_LOOP:
-		// 	foc_speed_ramp(foc);
-		// 	foc->mode = FOC_MODE_PI_FULL;
-		// 	foc_fsm_next_state(RUNNING);
-		// 	ramp_clear(&foc->voltage_ramp);
-		// 	break;
-		case RUNNING:
-			foc_speed_ramp(foc);
-			foc_calc_current(foc);
-			if (foc->foc_fault == foc_brake_error){
-				foc_fsm_next_state(ANY_STOP);
-				break;
-			}
-			/*
-			if (foc_get_speed() <= RPM_FOR_CLOSE_LOOP){
-				foc_set_voltage_ramp(speed_to_voltage(foc_get_speed()));
-				ramp_exc(&foc->voltage_ramp);
-				foc_fsm_next_state(RAMPING_START);
-				foc->mode = FOC_MODE_OPEN_LOOP;
-				foc_overide_vdq(true);
-			}*/
-			break;
-		case ANY_STOP:
-			foc_stop();
-			foc_fsm_next_state(IDLE);
-			break;
-		default:
-			break;
-	}
-
-}

Applications/foc/core/foc_fsm.h

@@ -1,9 +0,0 @@
-#ifndef _FOC_STM_H__
-#define _FOC_STM_H__
-#include "foc/core/foc_api.h"
-#include "foc/core/foc_core.h"
-
-foc_state_t foc_fsm_state(void);
-foc_fault_t foc_fsm_next_state(foc_state_t state);
-void foc_fsm(motor_foc_t *foc);
-#endif /* _FOC_STM_H__ */

Applications/foc/core/foc_type.h

@@ -1,10 +1,10 @@
 #ifndef _FOC_TYPE_H__
 #define _FOC_TYPE_H__
 #include <math.h>
-#include "os/os_type.h"
-#include "math/fast_math.h"
-#include "pi_controller.h"
+#include "os/os_types.h"
 #include "ramp_ctrl.h"
+#include "PMSM_Controller.h"
+
 typedef struct _alphabeta {
 	float alpha;
 	float beta;
@@ -21,11 +21,6 @@ typedef struct _dqaix {
 	};
 }dq_t;
 
-typedef struct _motor_s {
-	float theta;//ת�ӵ�Ƕ�, PI
-	int angle;//ת�ӵ�Ƕ�, ��
-	float rpm; //ת��
-}motor_stat_t;
 
 typedef struct _phase_time {
 	s32 A;
@@ -56,13 +51,6 @@ typedef enum {
 	RUNNING
 }foc_state_t;
 
-typedef enum {
-	FOC_MODE_OPEN_LOOP, //开环模式，霍尔，电流等校准都在开环模式下，启动开始默认开环，等速度上来后切换到闭环
-	FOC_MODE_CURRENT_LOOP,      //ֻ电流环模式
-	FOC_MODE_SPEED_LOOP,     //ֻ速度环模式
-	FOC_MODE_CLOSE_LOOP,         //电流速度双闭环模式
-}control_mode_t;
-
 typedef enum {
 	INVERT_NO,
 	INVERT_A,
@@ -70,76 +58,34 @@ typedef enum {
 	INVERT_C
 }phase_invert_t;
 
-typedef struct current_sample {
-	s32   adc_offset_a;
-	s32   adc_offset_b;
-	s32   adc_offset_c;
-	float Ia;
-	float Ib;
-	float Ic;
-	float old_Ia;
-	float old_Ib;
-	float old_Ic;
-	float max_Ia;
-	float max_Ib;
-	float max_Ic;
-	u8    sector;
-	phase_time_t time;	
-	volatile int   offset_sample_count;
-	bool  is_calibrating_offset;
-}current_samp_t;
-
-/*
-typedef struct _override {
-	bool is_theta;
-	float theta;
-	bool is_vdq;
-	dq_t vdq;
-}override_param_t;
-*/
+
 typedef enum {
-	foc_success = 0,
-	foc_not_allowed = 1,
-	foc_start_with_gas_error, //ready的时候检测到转把信号
-	foc_stop_with_speed_error,
-	foc_brake_error = 100,
+	FOC_Success = 0,
+	FOC_NotAllowed = 1,
+	FOC_Throttle_Err, //ready的时候检测到转把信号
+	FOC_NowAllowed_With_Speed,
+	FOC_Speed_TooLow,
+	FOC_NotCruiseMode,
+	FOC_Param_Err,
+	FOC_Phase_Err,
+	FOC_Hall_Err,
+	FOC_Brake_Err = 100,
 }foc_fault_t;
 
-typedef struct {
-	int  speed;
-	u32  delta_ts; //ms
-}speed_cmd_t;
-typedef struct foc_s {
-	bool is_motor_start;
-	alpha_beta_t alpha_beta;
-	current_samp_t current_samp; /*三相电流采集 */
-	dq_t dq_command; 
-	speed_cmd_t speed_command;
-	dq_t dq_last;
-	u8   sector; //svpwm 扇区
-	float vbus; //母线电压
-	motor_stat_t  motor_stat;
-//	hall_param_t  hall_param;
-	phase_time_t phase_time;
-	pi_controller_t    id_controller;
-	pi_controller_t    iq_controller;
-	pi_controller_t    speed_controller;
-	foc_state_t state;
-	control_mode_t mode;
-	//override_param_t override;
-	bool mosfec_gate;
-	ramp_t voltage_ramp; //开环情况下直接设置svm的voltage
-	ramp_t current_ramp; //���ٵ���б��
-	ramp_t speed_ramp; //�ı��ٶȵ�ramp���������ű仯��Ҫ����speed_ramp
-	foc_fault_t foc_fault;
-	bool   is_brake_in;
-}motor_foc_t;
 
-typedef enum {
-	NoError = 0,
-	STMNotAllow = 1,
-	
-}FError;
+typedef struct _foc {
+	ramp_t 		speed_ramp; 
+	u8          error_code;
+	bool   		b_brake_in;
+	bool        b_FocEna;
+	ExtU 		*FOC_In;                       /* External inputs */
+	ExtY 		*FOC_Out;                       /* External outputs */	
+}pmsm_foc_t;
+
+
+#define OPEN_MODE                      ((uint8_T)0U)
+#define SPD_MODE                       ((uint8_T)1U)
+#define TRQ_MODE                       ((uint8_T)2U)
 
 #define degree_2_pi(d) ((float)d * M_PI / 180.0f)
 #define pi_2_degree(d) ((float)d * 180.0f / M_PI)

Applications/foc/core/foc_wapper.c

@@ -0,0 +1,47 @@
+/*
+ *	参考 ert_main.c， 对Simulink生成的代码做一次封装
+ */
+
+#include <stddef.h>
+#include <stdio.h>              /* This ert_main.c example uses printf/fflush */
+#include "PMSM_Controller.h"           /* Model's header file */
+#include "rtwtypes.h"
+#include "zero_crossing_types.h"
+
+static RT_MODEL rtM_;
+static RT_MODEL *const rtMPtr = &rtM_; /* Real-time model */
+static DW rtDW;                        /* Observable states */
+static PrevZCX rtPrevZCX;              /* Triggered events */
+static ExtU rtU;                       /* External inputs */
+static ExtY rtY;                       /* External outputs */
+
+
+void PMSM_FOC_Init(void) {
+	RT_MODEL *const rtM = rtMPtr;
+  	/* Pack model data into RTM */
+  	rtM->dwork = &rtDW;
+  	rtM->prevZCSigState = &rtPrevZCX;
+  	rtM->inputs = &rtU;
+  	rtM->outputs = &rtY;
+
+  	/* Initialize model */
+  	PMSM_Controller_initialize(rtM);	
+}
+
+
+
+ExtU *PMSM_FOC_GetInputs(void) {
+	return rtMPtr->inputs;
+}
+
+ExtY *PMSM_FOC_GetOutputs(void) {
+	return rtMPtr->outputs;
+}
+
+
+void PMSM_FOC_Step(void) {
+	
+	PMSM_Controller_step(rtMPtr);
+}
+
+

Applications/foc/core/park_clark.h

@@ -1,29 +0,0 @@
-#ifndef _Park_Clark_H__
-#define _Park_Clark_H__
-#include "bsp/bsp.h"
-#include "foc/core/foc_type.h"
-#include "math/fast_math.h"
-/* ·´Park ±ä»» */
-static __INLINE void Rev_Park(dq_t *dq, float angle, alpha_beta_t *alpha_bata) {
-	float c,s;
-	normal_sincosf(angle, &s, &c);
-
-	alpha_bata->alpha = dq->Vd * c - dq->Vq * s;
-	alpha_bata->beta = dq->Vd * s + dq->Vq * c;
-}
-
-static __INLINE void do_clark(float phaseU, float phaseV, float phaseW, alpha_beta_t *alpha_bata){
-	alpha_bata->alpha = (2.0f * phaseU - phaseV - phaseW) / 3.0f;
-	alpha_bata->beta = (ONE_BY_SQRT3 * (phaseV - phaseW));
-}
-
-static __INLINE void do_park(alpha_beta_t *alpha_beta, float angle, dq_t *dq) {
-	float c,s;
-	normal_sincosf(angle, &s, &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__ */
-

Applications/foc/core/pi_controller.c

@@ -1,29 +0,0 @@
-#include "pi_controller.h"
-
-float pi_control(pi_controller_t *pi, float error){
-	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_controller_t *pi) {
-	pi->i_errors = 0.0f;
-	pi->output = 0.0f;
-}

Applications/foc/core/pi_controller.h

@@ -1,16 +0,0 @@
-#ifndef _PI_CONTROLLER_H__
-#define _PI_CONTROLLER_H__
-typedef struct _pi {
-	float Kp_gain;
-	float Ki_gain;
-	float i_errors;
-	float output;
-	float max_output;
-	float min_output;
-}pi_controller_t;
-
-float pi_control(pi_controller_t *pi, float error);
-void pi_clear(pi_controller_t *pi);
-
-#endif  /* _PI_CONTROLLER_H__ */
-

Applications/foc/core/ramp_ctrl.c

@@ -1,7 +1,7 @@
 #include "ramp_ctrl.h"
 
 #define RAMP_INTVAL 50 //ms
-void ramp_timer_handler(timer_t *timer);
+void ramp_timer_handler(shark_timer_t *timer);
 
 void ramp_ctrl_init(ramp_t *ramp){
 	ramp_clear(ramp);
@@ -9,7 +9,7 @@ void ramp_ctrl_init(ramp_t *ramp){
 }
 
 void ramp_clear(ramp_t *ramp) {
-	timer_cancel(&ramp->timer);
+	shark_timer_cancel(&ramp->timer);
 	ramp->start_point = 0;
 	ramp->target = 0;
 	ramp->final_point = 0;
@@ -18,15 +18,22 @@ void ramp_clear(ramp_t *ramp) {
 }
 
 void ramp_set_target(ramp_t *ramp, float start, float final, u32 duration_ms) {
-	timer_cancel(&ramp->timer);
+	shark_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);
+	
+	if (duration_ms == 0) {
+		ramp->steps = (final - ramp->start_point);
+		ramp->target = final;
+	}else {
+		ramp->steps = (final - ramp->start_point) / (duration_ms / RAMP_INTVAL);
+		ramp_exc(ramp);
+	}
 }
 
 void ramp_exc(ramp_t *ramp){
-	timer_post(&ramp->timer, RAMP_INTVAL);
+	shark_timer_post(&ramp->timer, RAMP_INTVAL);
 }
 
 float ramp_get_target(ramp_t *ramp){
@@ -37,7 +44,7 @@ bool ramp_complete(ramp_t *ramp) {
 	return ramp->target == ramp->final_point;
 }
 
-void ramp_timer_handler(timer_t *timer) {
+void ramp_timer_handler(shark_timer_t *timer) {
 	ramp_t *ramp = (ramp_t *)timer;
 	float target = ramp->target + ramp->steps;
 	if (ramp->steps < 0) {
@@ -51,9 +58,9 @@ void ramp_timer_handler(timer_t *timer) {
 	}
 	ramp->target = target;
 	if (target != ramp->final_point) {
-		timer_post(&ramp->timer, RAMP_INTVAL);
+		shark_timer_post(&ramp->timer, RAMP_INTVAL);
 	}else {
-		timer_cancel(&ramp->timer);
+		shark_timer_cancel(&ramp->timer);
 	}
 }
 

Applications/foc/core/ramp_ctrl.h

@@ -1,8 +1,8 @@
 #ifndef _RAMP_CTRL_H__
 #define _RAMP_CTRL_H__
-#include "os/timer.h"
+#include "os/os_task.h"
 typedef struct {
-	timer_t timer;
+	shark_timer_t timer;
 	float start_point;
 	float final_point;
 	float target;

Applications/foc/core/svpwm.c

@@ -1,148 +0,0 @@
-#include "foc/core/svpwm.h"
-#include "math/fast_math.h"
-
-/* 7段式SVPWM
- * 返回设置3相PWM的3个CCR寄存器的值
- * 这里使用的是stm32的PWM mode1,在向上计数时，一旦TIMx_CNT<TIMx_CCR1时通道1为有效电平，否则为无效电平
- * 在向下计数时，一旦TIMx_CNT>TIMx_CCR1时通道1为无效电平(OC1REF=0)，否则为有效 电平(OC1REF=1)。
- * 整个时间的计算，前面X,Y,Z都是一样的，后面计算ABC三相的pwm CCR寄存器值的时候，需要注意，很多网络包括书本的资料都是用PWM2模式的
-   就是高电平的时间 pwm_period - ccr,我们用PWM1模式,所以最后abc的计算稍微有些不一样
-*/
-
-void svpwm_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 = TWO_BY_SQRT3 * beta * modu;
-	Y = (alpha + ONE_BY_SQRT3 * 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);
-}
-

Applications/foc/core/svpwm.h

@@ -1,10 +0,0 @@
-#ifndef _SVPWM_H__
-#define _SVPWM_H__
-#include "foc/core/foc_type.h"
-
-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 svpwm_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__ */
-

Applications/foc/motor/current.c

@@ -1,9 +1,12 @@
 #include <math.h>
 #include "bsp/adc.h"
+#include "bsp/pwm.h"
 #include "foc/motor/current.h"
 #include "libs/utils.h"
 #include "libs/logger.h"
 
+static current_samp_t g_cs;
+
 #define NB_OFFSET_SAMPLES 32
 
 #define Rvbus  0.0005f
@@ -18,21 +21,51 @@
 
 #define current_i(v, r) ((v)/(r))
 
-void phase_current_init(current_samp_t *cs) {
+void phase_current_init(void) {
+	current_samp_t *cs = &g_cs;
 	cs->offset_sample_count = NB_OFFSET_SAMPLES + 1;
-	cs->Ia = 0.0f;
-	cs->Ib = 0.0f;
-	cs->Ic = 0.0f;
+	cs->adc_ia = 0;
+	cs->adc_ib = 0;
+	cs->adc_ic = 0;
+}
+
+void phase_current_calibrate(void){
+	g_cs.adc_offset_a = 0;
+	g_cs.adc_offset_b = 0;
+	g_cs.adc_offset_c = 0;
+
+	phase_current_init();
+	g_cs.is_calibrating_offset = true;
+	g_cs.sector = SECTOR_5;
+	adc_current_sample_config(g_cs.sector);
+	pwm_start();
+	adc_start_convert();
+	while(g_cs.offset_sample_count != 0){};
+
+	adc_stop_convert();
+	pwm_stop();
+	task_udelay(100);
+	phase_current_init();	
+	g_cs.sector = SECTOR_1;
+	adc_current_sample_config(g_cs.sector);
+	pwm_start();
+	adc_start_convert();
+	while(g_cs.offset_sample_count != 0){};
+
+	g_cs.is_calibrating_offset = false;
 }
 
 
-#if SHUNT_NUM==THREE_SHUNTS_SAMPLE
-void phase_current_offset(current_samp_t *cs) {
+bool phase_current_offset(void) {
+	current_samp_t *cs = &g_cs;
+	if (!g_cs.is_calibrating_offset) {
+		return false;
+	}
 	s32 phase_current1, phase_current2;
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
 	if (cs->offset_sample_count == (NB_OFFSET_SAMPLES + 1)) {
 		cs->offset_sample_count --;
-		return;
+		return true;
 	}
 	if (cs->offset_sample_count > 0) {
 		cs->offset_sample_count--;
@@ -51,56 +84,48 @@ void phase_current_offset(current_samp_t *cs) {
 			}
 		}
 	}
+	return true;
 }
 
 
 #define LowPass_filter 1.0f
-void phase_current_sample(current_samp_t *cs){
+void phase_current_sample(s16 *ia, s16 *ib){
+	current_samp_t *cs = &g_cs;
 	s32 phase_current1, phase_current2;
-	float Ia, Ib, Ic;
 	phase_time_t *time = &cs->time;
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
 	if (time->three_shunts_flags == 1) {
+		*ia = cs->adc_ia;
+		*ib = cs->adc_ib;		
 		time->three_shunts_flags = 0;
 		return; //use old current;
 	}
 	if (cs->sector == SECTOR_4 || cs->sector == SECTOR_5) {
 		/* Current on Phase C is not accessible */
 		/* Ia = PhaseAOffset - ADC converted value) */
-		Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Rds_Defualt);
-		LowPass_Filter(cs->Ib, Ib, LowPass_filter);
-		Ia = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_a), Rds_Defualt);
-		LowPass_Filter(cs->Ia, Ia, LowPass_filter);
-		cs->Ic = -(cs->Ia + cs->Ib);
+		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
+		cs->adc_ia = (phase_current2 - cs->adc_offset_a);
+		cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
 	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
 		/* Current on Phase A is not accessible 	*/
 		/* Ib = PhaseBOffset - ADC converted value) */
-		Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Rds_Defualt);
-		LowPass_Filter(cs->Ib, Ib, LowPass_filter);
-		Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Rds_Defualt);
-		LowPass_Filter(cs->Ic, Ic, LowPass_filter);
-		cs->Ia = -(cs->Ib + cs->Ic);
+		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
+		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
+		cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
 	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_3) {
 		/* Current on Phase B is not accessible 	*/
 		/* Ia = PhaseAOffset - ADC converted value) */
-		Ia = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_a), Rds_Defualt);
-		LowPass_Filter(cs->Ia, Ia, LowPass_filter);
-		Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Rds_Defualt);
-		LowPass_Filter(cs->Ic, Ic, LowPass_filter);
-		cs->Ib = -(cs->Ia + cs->Ic);
+		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
+		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
+		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
 	}
-	
-	{
-		static int count = 0;
-		if (count++ % 2 == 0) {
-			//log_chan_value(1, (int)(cs->Ia * 1000));
-		}
-	}
-
+	*ia = cs->adc_ia;
+	*ib = cs->adc_ib;
 }
 
 
-void get_phase_sample_point(current_samp_t *cs, u8 sector){
+void get_phase_sample_point(u8 sector){
+	current_samp_t *cs = &g_cs;
 	phase_time_t *time = &cs->time;
 	u32 low_side_low_duty = FOC_PWM_Half_Period - time->low;
 	u32 low_side_mid_duty = FOC_PWM_Half_Period - time->midle;
@@ -140,505 +165,9 @@ void get_phase_sample_point(current_samp_t *cs, u8 sector){
 		time->Samp_p1 = FOC_PWM_Half_Period - 1;//dumy trigger
 	}
 }
-#else
-
-#define TBEFOR (TDead + MAX(TRise, TNoise))
-#define TMIN (TDead + MAX(TRise, TNoise) + TADC)
-
-static __inline u8 _get_sample_boundary(current_samp_t *cs, phase_time_t *time) {
-#if 0
-	s32 delta_duty0 = (s32)time->midle - (s32)time->high;
-	s32 delta_duty1 = (s32)time->low - (s32)time->midle;
-
-	if (delta_duty0 <= TMIN && delta_duty1 <= TMIN) {
-		return BOUNDARY_3;
-	}else if (delta_duty0 <= TMIN && delta_duty1 > TMIN) {
-		return BOUNDARY_2;
-	}else if (delta_duty0 > TMIN && delta_duty1 <= TMIN) {
-		return BOUNDARY_1;
-	}else {
-		return REGULAR;
-	}
-#else
-	return REGULAR;
-#endif
-}
-
-static __inline void _get_boundary1_samp(current_samp_t *cs, phase_time_t *time) {
-	s32 delta_duty1 = (s32)time->low - (s32)time->midle;
-	s32 delta_time_inc = (TMIN - delta_duty1);
-	s32 delta_time_dec = min(delta_time_inc, delta_duty1);
-	s32 sample_p;
-	switch(cs->sector) {
-		case SECTOR_1: //AB big and delta small
-			time->Samp_p1 = time->B - TADC;
-			sample_p = time->A;
-			time->A = sample_p + delta_time_inc;
-			time->A_next = sample_p - delta_time_dec;
-			time->Samp_p2 = time->A - TADC;
-			time->sampe_phase_1 = SAMP_NIC;
-			time->sampe_phase_2 = SAMP_IA;
-			break;
-		case SECTOR_2://BA big and delta small
-			time->Samp_p1 = time->A - TADC;
-			sample_p = time->B;
-			time->B = sample_p + delta_time_inc;
-			time->B_next = sample_p - delta_time_dec;
-			time->Samp_p2 = time->B - TADC;
-			time->sampe_phase_1 = SAMP_NIC;
-			time->sampe_phase_2 = SAMP_IB;		
-			break;
-		case SECTOR_3://BC big and delta small
-			time->Samp_p1 = time->C - TADC;
-			sample_p = time->B;
-			time->B = sample_p + delta_time_inc;
-			time->B_next = sample_p - delta_time_dec;
-			time->Samp_p2 = time->B - TADC;
-			time->sampe_phase_1 = SAMP_NIA;
-			time->sampe_phase_2 = SAMP_IB;				
-			break;
-		case SECTOR_4://CB big and delta small
-			time->Samp_p1 = time->B - TADC;
-			sample_p = time->C;
-			time->C = sample_p + delta_time_inc;
-			time->C_next = sample_p - delta_time_dec;
-			time->Samp_p2 = time->C - TADC;
-			time->sampe_phase_1 = SAMP_NIA;
-			time->sampe_phase_2 = SAMP_IC;			
-			break;
-		case SECTOR_5://CA big and delta small
-			time->Samp_p1 = time->A - TADC;
-			sample_p = time->C;
-			time->C = sample_p + delta_time_inc;
-			time->C_next = sample_p - delta_time_dec;
-			time->Samp_p2 = time->C - TADC;
-			time->sampe_phase_1 = SAMP_NIB;
-			time->sampe_phase_2 = SAMP_IC;			
-			break;
-		case SECTOR_6://AC big and delta small
-			time->Samp_p1 = time->C - TADC;
-			sample_p = time->A;
-			time->A = sample_p + delta_time_inc;
-			time->A_next = sample_p - delta_time_dec;
-			time->Samp_p2 = time->A - TADC;
-			time->sampe_phase_1 = SAMP_NIB;
-			time->sampe_phase_2 = SAMP_IA;				
-			break;
-		default:
-			break;
-	}
-}
-
-static __inline void _get_boundary2_samp(current_samp_t *cs, phase_time_t *time) {
-	s32 delta_duty0 = (s32)time->midle - (s32)time->high;
-	s32 delta_time_dec = (TMIN - delta_duty0);
-	s32 delta_time_inc = min(delta_time_dec, delta_duty0);
-	s32 sample_p;
-	switch(cs->sector) {
-		case SECTOR_1: //BC samll and delta small
-			time->Samp_p2 = time->A - TADC;
-			sample_p = time->C;
-			time->C = sample_p - delta_time_dec;
-			time->C_next = sample_p + delta_time_inc;
-			time->Samp_p1 = time->B - TADC;
-			time->sampe_phase_2 = SAMP_IA;
-			time->sampe_phase_1 = SAMP_NIC;		
-			break;
-		case SECTOR_2://AC samll and delta small
-			time->Samp_p2 = time->B - TADC;
-			sample_p = time->C;
-			time->C = sample_p - delta_time_dec;
-			time->C_next = sample_p + delta_time_inc;
-			time->Samp_p1 = time->A - TADC;
-			time->sampe_phase_2 = SAMP_IB;
-			time->sampe_phase_1 = SAMP_NIC;			
-			break;
-		case SECTOR_3://CA samll and delta small
-			time->Samp_p2 = time->B - TADC;
-			sample_p = time->A;
-			time->A = sample_p - delta_time_dec;
-			time->A_next = sample_p + delta_time_inc;
-			time->Samp_p1 = time->C - TADC;
-			time->sampe_phase_2 = SAMP_IB;
-			time->sampe_phase_1 = SAMP_NIA;				
-			break;
-		case SECTOR_4://BA samll and delta small
-			time->Samp_p2 = time->C - TADC;
-			sample_p = time->A;
-			time->A = sample_p - delta_time_dec;
-			time->A_next = sample_p + delta_time_inc;
-			time->Samp_p1 = time->B - TADC;
-			time->sampe_phase_2 = SAMP_IC;
-			time->sampe_phase_1 = SAMP_NIA;			
-			break;
-		case SECTOR_5://AB samll and delta small
-			time->Samp_p2 = time->C - TADC;
-			sample_p = time->B;
-			time->B = sample_p - delta_time_dec;
-			time->B_next = sample_p + delta_time_inc;
-			time->Samp_p1 = time->A - TADC;
-			time->sampe_phase_2 = SAMP_IC;
-			time->sampe_phase_1 = SAMP_NIB;			
-			break;
-		case SECTOR_6://CB samll and delta small
-			time->Samp_p2 = time->A - TADC;
-			sample_p = time->B;
-			time->B = sample_p - delta_time_dec;
-			time->B_next = sample_p + delta_time_inc;
-			time->Samp_p1 = time->C - TADC;
-			time->sampe_phase_2 = SAMP_IA;
-			time->sampe_phase_1 = SAMP_NIB;			
-			break;
-		default:
-			break;
-	}
-
-}
-
-
-static __inline void _get_boundary3_samp(current_samp_t *cs, phase_time_t *time) {
-#if 1
-	s32 sample_p;
-	if ((time->boundary3_flags & 1) == 0) {
-		time->boundary3_flags |= 1;
-		sample_p = time->A;
-		time->A = sample_p + TMIN;
-		time->A_next = sample_p - TMIN;
-		time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
-		time->Samp_p2 = time->A - TADC;
-		time->sampe_phase_1 = SAMP_OLDB;
-		time->sampe_phase_2 = SAMP_IA;			
-	}else {
-		time->boundary3_flags &= ~1;
-		sample_p = time->B;
-		time->B = sample_p + TMIN;
-		time->B_next = sample_p - TMIN;
-		time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
-		time->Samp_p2 = time->B - TADC;
-		time->sampe_phase_1 = SAMP_OLDA;
-		time->sampe_phase_2 = SAMP_IB;			
-	}
-#else
-	s32 delta_duty0 = (s32)time->midle - (s32)time->high;
-	s32 delta_duty1 = (s32)time->low - (s32)time->midle;
-
-	s32 delta_time_inc1 = (TMIN - delta_duty1);
-	s32 delta_time_dec1 = min(delta_time_inc1, delta_duty1);
-	
-	s32 delta_time_dec2 = (TMIN - delta_duty0);
-	s32 delta_time_inc2 = min(delta_time_dec2, delta_duty0);
-
-	s32 sample_p;
-	switch(cs->sector) {
-		case SECTOR_1: //deltaBC > deltaAB
-			if (delta_duty0 > delta_duty1) {
-				sample_p = time->C;
-				time->C = sample_p - delta_time_dec2;
-				time->C_next = sample_p + delta_time_inc2;
-				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->B - TADC;
-				time->sampe_phase_1 = SAMP_OLDB;
-				time->sampe_phase_2 = SAMP_NIC;					
-			}else {
-				sample_p = time->A;
-				time->A = sample_p + delta_time_inc1;
-				time->A_next = sample_p - delta_time_dec1;	
-				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->A - TADC;
-				time->sampe_phase_1 = SAMP_OLDC;
-				time->sampe_phase_2 = SAMP_IA;						
-			}
-			break;
-		case SECTOR_2: //deltaAC > deltaBA
-			if (delta_duty0 > delta_duty1) {
-				sample_p = time->C;
-				time->C = sample_p - delta_time_dec2;
-				time->C_next = sample_p + delta_time_inc2;
-				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->A - TADC;
-				time->sampe_phase_1 = SAMP_OLDA;
-				time->sampe_phase_2 = SAMP_NIC;					
-			}else {
-				sample_p = time->B;
-				time->B = sample_p + delta_time_inc1;
-				time->B_next = sample_p - delta_time_dec1;	
-				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->B - TADC;
-				time->sampe_phase_1 = SAMP_OLDB;
-				time->sampe_phase_2 = SAMP_IB;						
-			}	
-			break;
-		case SECTOR_3: //deltaCA > deltaBC
-			if (delta_duty0 > delta_duty1) {
-				sample_p = time->A;
-				time->A = sample_p - delta_time_dec2;
-				time->A_next = sample_p + delta_time_inc2;
-				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->C - TADC;
-				time->sampe_phase_1 = SAMP_OLDC;
-				time->sampe_phase_2 = SAMP_NIA;					
-			}else {
-				sample_p = time->B;
-				time->B = sample_p + delta_time_inc1;
-				time->B_next = sample_p - delta_time_dec1;	
-				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->B - TADC;
-				time->sampe_phase_1 = SAMP_OLDA;
-				time->sampe_phase_2 = SAMP_IB;						
-			}		
-			break;
-		case SECTOR_4: //CBA, //deltaBA > deltaCB
-			if (delta_duty0 > delta_duty1) {
-				sample_p = time->A;
-				time->A = sample_p - delta_time_dec2;
-				time->A_next = sample_p + delta_time_inc2;
-				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->B - TADC;
-				time->sampe_phase_1 = SAMP_OLDB;
-				time->sampe_phase_2 = SAMP_NIA;					
-			}else {
-				sample_p = time->C;
-				time->C = sample_p + delta_time_inc1;
-				time->C_next = sample_p - delta_time_dec1;	
-				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->C - TADC;
-				time->sampe_phase_1 = SAMP_OLDC;
-				time->sampe_phase_2 = SAMP_IC;						
-			}	
-			break;
-		case SECTOR_5: //CAB, //deltaAB > deltaCA
-			if (delta_duty0 > delta_duty1) {
-				sample_p = time->B;
-				time->B = sample_p - delta_time_dec2;
-				time->B_next = sample_p + delta_time_inc2;
-				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->A - TADC;
-				time->sampe_phase_1 = SAMP_OLDA;
-				time->sampe_phase_2 = SAMP_NIB;					
-			}else {
-				sample_p = time->C;
-				time->C = sample_p + delta_time_inc1;
-				time->C_next = sample_p - delta_time_dec1;	
-				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->C - TADC;
-				time->sampe_phase_1 = SAMP_OLDB;
-				time->sampe_phase_2 = SAMP_IC;						
-			}		
-			break;
-		case SECTOR_6: //ACB, //deltaCB > deltaAC
-			if (delta_duty0 > delta_duty1) {
-				sample_p = time->B;
-				time->B = sample_p - delta_time_dec2;
-				time->B_next = sample_p + delta_time_inc2;
-				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->C - TADC;
-				time->sampe_phase_1 = SAMP_OLDC;
-				time->sampe_phase_2 = SAMP_NIB;					
-			}else {
-				sample_p = time->A;
-				time->A = sample_p + delta_time_inc1;
-				time->A_next = sample_p - delta_time_dec1;	
-				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
-				time->Samp_p2 = time->A - TADC;
-				time->sampe_phase_1 = SAMP_OLDA;
-				time->sampe_phase_2 = SAMP_IA;						
-			}		
-			break;
-		default:
-			break;
-	}
-#endif
-}
 
-static __inline void _get_regular_samp(current_samp_t *cs, phase_time_t *time) {
-	time->Samp_p1 = time->midle - TADC;
-	time->Samp_p2 = time->low - TADC;
-	switch(cs->sector) {
-		case SECTOR_1: //ABC
-			time->sampe_phase_1 = SAMP_NIC;
-			time->sampe_phase_2 = SAMP_IA;	
-			break;
-		case SECTOR_2: //BAC
-			time->sampe_phase_1 = SAMP_NIC;
-			time->sampe_phase_2 = SAMP_IB;		
-			break;
-		case SECTOR_3: //BCA
-			time->sampe_phase_1 = SAMP_NIA;
-			time->sampe_phase_2 = SAMP_IB;		
-			break;
-		case SECTOR_4: //CBA
-			time->sampe_phase_1 = SAMP_NIA;
-			time->sampe_phase_2 = SAMP_IC;		
-			break;
-		case SECTOR_5: //CAB
-			time->sampe_phase_1 = SAMP_NIB;
-			time->sampe_phase_2 = SAMP_IC;		
-			break;
-		case SECTOR_6: //ACB
-			time->sampe_phase_1 = SAMP_NIB;
-			time->sampe_phase_2 = SAMP_IA;		
-			break;
-		default:
-			break;
-	}
-
-}
-
-void phase_current_offset(current_samp_t *cs) {
-	s32 phase_current1, phase_current2;
-	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
-	if (cs->offset_sample_count > 0) {
-		cs->offset_sample_count--;
-		if (cs->offset_sample_count >= 0) {
-			cs->adc_offset_a += phase_current1;
-			cs->adc_offset_b += phase_current2;
-			if (cs->offset_sample_count == 0) {
-				cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
-				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
-			}
-		}
-	}
-}
-
-void phase_current_sample(current_samp_t *cs){
-	s32 phase_current1, phase_current2;
-	u8  b_curr_a = 0;
-	u8  b_curr_b = 0;
-	u8  b_curr_c = 0;
-	phase_time_t *time = &cs->time;
-	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
-	phase_current1 -= cs->adc_offset_a;
-	phase_current2 -= cs->adc_offset_b;
-	float current = current_i(VBUS_VOL(abs(phase_current1)), Sample_R);
-	switch (time->sampe_phase_1) {
-		case SAMP_IA:
-			cs->Ia = current;
-			b_curr_a = 1;
-			break;
-		case SAMP_IB:
-			cs->Ib = current;
-			b_curr_b = 1;
-			break;
-		case SAMP_IC:
-			cs->Ic = current;
-			b_curr_c = 1;
-			break;
-		case SAMP_NIA:
-			cs->Ia = -current;
-			b_curr_a = 1;
-			break;
-		case SAMP_NIB:
-			cs->Ib = -current;
-			b_curr_b = 1;
-			break;
-		case SAMP_NIC:
-			cs->Ic = -current;
-			b_curr_c = 1;
-			break;		
-		case SAMP_OLDA:
-			cs->Ia = cs->old_Ia;
-			b_curr_a = 1;
-			break;
-		case SAMP_OLDB:
-			cs->Ib = cs->old_Ib;
-			b_curr_b = 1;
-			break;
-		case SAMP_OLDC:
-			cs->Ic = cs->old_Ic;
-			b_curr_c = 1;
-			break;
-		default:
-			break;
-	}
-
-	current = current_i(VBUS_VOL(abs(phase_current2)), Sample_R);
-
-	switch (time->sampe_phase_2) {
-		case SAMP_IA:
-			cs->Ia = current;
-			b_curr_a = 1;
-			break;
-		case SAMP_IB:
-			cs->Ib = current;
-			b_curr_b = 1;
-			break;
-		case SAMP_IC:
-			cs->Ic = current;
-			b_curr_c = 1;
-			break;
-		case SAMP_NIA:
-			cs->Ia = -current;
-			b_curr_a = 1;
-			break;
-		case SAMP_NIB:
-			cs->Ib = -current;
-			b_curr_b = 1;
-			break;
-		case SAMP_NIC:
-			cs->Ic = -current;
-			b_curr_c = 1;
-			break;		
-		case SAMP_OLDA:
-			cs->Ia = cs->old_Ia;
-			b_curr_a = 1;
-			break;
-		case SAMP_OLDB:
-			cs->Ib = cs->old_Ib;
-			b_curr_b = 1;
-			break;
-		case SAMP_OLDC:
-			cs->Ic = cs->old_Ic;
-			b_curr_c = 1;
-			break;
-		default:
-			break;
-	}
-	if (b_curr_a == 0) {
-		cs->Ia = -(cs->Ib + cs->Ic);
-	}
-	if (b_curr_b == 0) {
-		cs->Ib = -(cs->Ia + cs->Ic);
-	}
-	if (b_curr_c == 0) {		
-		cs->Ic = -(cs->Ia + cs->Ib);
-	}
-	cs->old_Ia = cs->Ia;
-	cs->old_Ib = cs->Ib;
-	cs->old_Ic = cs->Ic;
-	{
-		static int count = 0;
-		if (count++ % 3 == 0) {
-			log_chan_value(1, (int)(cs->Ia * 1000));
-		}
-	}
-}
-
-
-void get_phase_sample_point(current_samp_t *cs, u8 sector){
-	phase_time_t *time = &cs->time;
-	if (cs->is_calibrating_offset) {
-		time->Samp_p1 = FOC_PWM_Half_Period - 2 * TMIN;
-		time->Samp_p2 = FOC_PWM_Half_Period - 1;
-		return;
-	}
-	cs->sector = sector;
-	time->A_next = time->A;
-	time->B_next = time->B;
-	time->C_next = time->C;
-	u8 boundary = _get_sample_boundary(cs, time);
-	if (boundary == BOUNDARY_1) {
-		_get_boundary1_samp(cs, time);
-	}else if (boundary == BOUNDARY_2) {
-		_get_boundary2_samp(cs, time);
-	}else if (boundary == BOUNDARY_3) {
-		_get_boundary3_samp(cs, time);
-	}else { //REGULAR, 直接可以采样
-		_get_regular_samp(cs, time);
-	}
-}
 
-#endif
-void phase_current_adc_triger(current_samp_t *cs){
+void phase_current_adc_triger(void){
 	adc_enable_ext_trigger();
 }
 

Applications/foc/motor/current.h

@@ -18,11 +18,24 @@
 #define SAMP_OLDB 8u
 #define SAMP_OLDC 9u
 
+typedef struct current_sample {
+	s32   adc_offset_a;
+	s32   adc_offset_b;
+	s32   adc_offset_c;
+	s16   adc_ia;
+	s16   adc_ib;
+	s16   adc_ic;
+	u8    sector;
+	phase_time_t time;	
+	volatile int   offset_sample_count;
+	bool  is_calibrating_offset;
+}current_samp_t;
 
-void phase_current_init(current_samp_t *cs);
-void phase_current_sample(current_samp_t *cs);
-void phase_current_offset(current_samp_t *cs);
-void get_phase_sample_point(current_samp_t *cs, u8 sector);
-void phase_current_adc_triger(current_samp_t *cs);
+void phase_current_init(void);
+void phase_current_sample(s16 *ia, s16 *ib);
+bool phase_current_offset(void);
+void get_phase_sample_point(u8 sector);
+void phase_current_adc_triger(void);
+void phase_current_calibrate(void);
 #endif /* _PHASE_CURRENT_H__ */
 

Applications/foc/motor/hall.c

@@ -1,478 +0,0 @@
-#include <string.h>
-#include "bsp/bsp.h"
-#include "bsp/mc_hall_gpio.h"
-#include "os/co_task.h"
-#include "os/timer.h"
-#include "libs/utils.h"
-#include "libs/logger.h"
-#include "math/fast_math.h"
-#include "foc/motor/hall.h"
-#include "foc/core/foc_api.h"
-#include "app/nv_storage.h"
-#include "bsp/timer_count32.h"
-
-#define USE_DETECTED_ANGLE 1
-
-#define HALL_READ_TIMES 3
-
-static void _hall_detect_task(void *args);
-static void _hall_init_el_angle(void);
-
-
-#define HALL_PLACE_OFFSET (315) //(345) //315
-/* 
-4,5,1,3,2,6,4
-*/
-
-static hall_sensor_t _sensor_hander;
-
-measure_time_t g_meas_hall = {.exec_max_time = 6,};
-
-#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
-#define us_2_s(tick) ((float)tick / 1000000.0f)
-
-#define rand_angle(a) {if (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;else if (a < 0) a +=PHASE_360_DEGREE;};
-
-static float hall_speed[8];
-static float  hall_angle[8];
-static void __inline _hall_put_sample(u32 ticks, float angle) {
-	hall_sample_t *s = &_sensor_hander.samples;
-	s->ticks_sum -= s->ticks[s->index];
-	s->angles_sum -= s->angles[s->index];
-	s->ticks[s->index] = ticks;
-	s->angles[s->index] = angle;
-	s->ticks_sum += s->ticks[s->index];
-	s->angles_sum += s->angles[s->index];
-	s->index += 1;
-	if (s->index >= SAMPLE_MAX_COUNT) {
-		s->full = true;
-		s->index = 0;
-	}
-}
-
-static float __inline _hall_angle_speed(void){
-	hall_sample_t *s = &_sensor_hander.samples;
-	if (s->ticks_sum == 0) {
-		return 0.0f;
-	}
-	
-	if (!s->full) {
-		return s->angles[s->index - 1] / us_2_s(s->ticks[s->index-1]);
-	}else {
-		return s->angles_sum / us_2_s(s->ticks_sum);
-	}
-}
-
-void hall_debug_log(void) {
-	for (int i = 0; i < 8; i++) {
-		if (i != 0 && i != 7) {
-			sys_debug("hall speed: %d,  %f - %f, %d\n", i, hall_speed[i], hall_angle[i], _sensor_hander.sensor_error);
-			sys_debug("hall angle %d\n", _sensor_hander.angle_table[i]);
-		}
-	}
-	sys_debug("angle dir %d\n", _sensor_hander.direction);
-}
-
-/*
-static bool __inline _hall_data_empty(void) {
-	hall_sample_t *s = &_sensor_hander.samples;
-	if ((!s->full) && (s->index == 0)){
-		return true;
-	}
-	return false;
-}
-*/
-
-static void hall_sensor_default(void) {
-	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
-	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
-	memcpy((char *)_sensor_hander.angle_table, (char *)mc_config_get()->hall_table, sizeof(_sensor_hander.angle_table));
-	_hall_init_el_angle();
-	hall_debug_log();
-}
-
-void hall_sensor_init(void) {
-	mc_hall_init();
-	hall_sensor_default();
-	co_task_create(_hall_detect_task, NULL, 512);
-}
-
-void hall_sensor_clear(void) {
-	hall_sensor_default();
-}
-
-
-static void _hall_detect_task(void *args) {
-	while(1) {
-		if (_sensor_hander.el_speed != 0) {
-			u32 ticks_now = timer_count32_get();
-			u32 delta_us = timer_count32_getus(ticks_now, _sensor_hander.hall_ticks);
-			if (delta_us >= (1200*1000)) {
-				hall_sensor_clear();
-			}
-		}
-		co_task_delay(100);
-	}
-}
-
-float hall_sensor_get_theta(void){
-	if (_sensor_hander.is_override_angle) {
-		return _sensor_hander.override_el_angle;
-	}
-
-	u32 us_now = timer_count32_delta_us(_sensor_hander.estimate_time_ticks, &_sensor_hander.estimate_time_ticks);
-	
-	float angle_step = _sensor_hander.estimate_el_speed * us_2_s(us_now);
-	_sensor_hander.estimate_delta_angle += angle_step;
-
-	if (_sensor_hander.direction == POSITIVE) {
-		_sensor_hander.estimate_el_angle += angle_step;
-	}else {
-		_sensor_hander.estimate_el_angle -= angle_step;
-	}
-
-	rand_angle(_sensor_hander.estimate_el_angle);
-
-	//log_chan_value(1, (int)_sensor_hander.estimate_el_angle);
-
-	return _sensor_hander.estimate_el_angle;
-}
-
-
-void hall_sensor_set_theta(bool override, float theta){
-	_sensor_hander.is_override_angle = override;
-	_sensor_hander.override_el_angle = theta;
-}
-
-float hall_sensor_get_speed(void) {
-	return _sensor_hander.rpm;
-}
-
-int hall_offset_increase(int inc) {
-	if (_sensor_hander.phase_offset + inc >= 360) {
-		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - 360;
-	}else {
-		_sensor_hander.phase_offset += inc;
-	}
-	return _sensor_hander.phase_offset;
-}
-
-u16 *hall_get_table(void) {
-	return _sensor_hander.angle_table;
-}
-
-int hall_sensor_calibrate(float voltage){
-	foc_set_controller_mode(FOC_MODE_OPEN_LOOP);
-	hall_sensor_set_theta(true, 0.0f);
-	foc_set_dq_command(0.0f, 0.0f);
-	foc_pwm_start(true);
-	foc_start_adc(true);
-	for (int i = 0;i < 100;i++) {
-		foc_set_dq_command((float)i * voltage / 100.0f, 0.0f);
-		co_task_delay(1);
-		wdog_reload();
-	}
-	float sin_hall[8];
-	float cos_hall[8];
-	int hall_iterations[8];
-	memset(sin_hall, 0, sizeof(sin_hall));
-	memset(cos_hall, 0, sizeof(cos_hall));
-	memset(_sensor_hander.angle_table, 0, sizeof(_sensor_hander.angle_table));
-	memset(hall_iterations, 0, sizeof(hall_iterations));
-	co_task_delay(2 * 1000);
-	// Forwards
-#if 1	
-	for (int i = 0;i < 5;i++) {
-		for (int j = 0;j < 360;j++) {
-			hall_sensor_set_theta(true, j);
-			co_task_delay(5);
-			wdog_reload();
-			int hall = get_hall_stat(7);
-			float s, c;
-			normal_sincosf(degree_2_pi(j), &s, &c);
-			sin_hall[hall] += s;
-			cos_hall[hall] += c;
-			hall_iterations[hall]++;
-		}
-	}
-#endif	
-#if 0
-	//hall_sensor_set_theta(true, 360);
-	//co_task_delay(2 * 1000);
-	sys_debug("Revers\n");
-	// Reverse
-	for (int i = 0;i < 5;i++) {
-		for (int j = 360;j >= 0;j--) {
-			hall_sensor_set_theta(true, j);
-			co_task_delay(5);
-			wdog_reload();
-			int hall = get_hall_stat(7);
-			float s, c;
-			normal_sincosf(degree_2_pi(j), &s, &c);
-			sin_hall[hall] += s;
-			cos_hall[hall] += c;
-			hall_iterations[hall]++;
-		}
-	}
-#endif
-	for (int i = 99;i >= 0;i--) {
-		foc_set_dq_command((float)i * voltage / 100.0f, 0.0f);
-		co_task_delay(1);
-		wdog_reload();
-	}
-	foc_pwm_start(false);
-	foc_start_adc(false);
-	hall_sensor_set_theta(false, 0.0f);
-	int fails = 0;
-	for(int i = 0;i < 8;i++) {
-		if (hall_iterations[i] > 30) {
-			float ang = pi_2_degree(atan2f(sin_hall[i], cos_hall[i]));
-			fast_norm_angle(&ang);
-			_sensor_hander.angle_table[i] = (u16)ang;
-			sys_debug("%d: %d\n", i, _sensor_hander.angle_table[i]);
-			co_task_delay(10);
-		} else {
-			fails++;
-			_sensor_hander.angle_table[i] = 0xFFFF;
-		}
-	}
-	
-	return fails == 2;	
-}
-
-
-static void _hall_init_el_angle(void) {
-	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
-#ifdef USE_DETECTED_ANGLE
-	if (_sensor_hander.hall_stat == 0 || _sensor_hander.hall_stat == 7) {
-		_sensor_hander.sensor_error ++;
-		return;
-	}
-	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + _sensor_hander.angle_table[_sensor_hander.hall_stat];
-#else
-	s32 sector_center = PHASE_60_DEGREE/2;
-  	switch ( _sensor_hander.hall_stat )
-  	{
-    case STATE_5:
-      	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + sector_center;
-      	break;
-    case STATE_1:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + sector_center;
-      	break;
-    case STATE_3:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + sector_center;
-      	break;
-    case STATE_2:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_180_DEGREE + sector_center;
-      	break;
-    case STATE_6:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + sector_center;
-      	break;
-    case STATE_4:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + sector_center;
-      	break;
-    default:
-      	/* Bad hall sensor configutarion so update the speed reliability */
-      	_sensor_hander.sensor_error ++;
-      	return;
- 	}
-#endif
-	_sensor_hander.sensor_error = 0;
-  	/* Initialize the measured angle */
-	rand_angle(_sensor_hander.measured_el_angle);
-  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
-	_sensor_hander.hall_ticks = timer_count32_get();
-	_sensor_hander.estimate_time_ticks = timer_count32_get();
-}
-
-static __inline__ s32 _get_angle(u8 state, s32 added) {
-#ifdef USE_DETECTED_ANGLE
-	return _sensor_hander.phase_offset + _sensor_hander.angle_table[state];
-#else
-	return _sensor_hander.phase_offset + added;
-#endif
-}
-/* 4,5,1,3,2,6,4 */
-static s32 _hall_position(u8 state_now, u8 state_prev) {
-	s32 theta_now =  0xFFFFFFFF;
-	switch (state_now) {
-		case STATE_1:
-			if (state_prev == STATE_5) {
-				_sensor_hander.direction = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
-			}else if (state_prev == STATE_3) {
-				_sensor_hander.direction = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
-			}
-			break;
-		case STATE_2:
-			if (state_prev == STATE_3) {
-				_sensor_hander.direction = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
-			}else if (state_prev == STATE_6) {
-				_sensor_hander.direction = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
-			}
-			break;
-		case STATE_3:
-			if (state_prev == STATE_1) {
-				_sensor_hander.direction = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
-			}else if (state_prev == STATE_2) {
-				_sensor_hander.direction = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
-			}
-			break;
-		case STATE_4:
-			if (state_prev == STATE_6) {
-				_sensor_hander.direction = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
-			}else if (state_prev == STATE_5) {
-				_sensor_hander.direction = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
-			}
-			break;
-		case STATE_5:
-			if (state_prev == STATE_4) {
-				_sensor_hander.direction = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
-			}else if (state_prev == STATE_1) {
-				_sensor_hander.direction = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
-			}
-			break;
-		case STATE_6:
-			if (state_prev == STATE_2) {
-				_sensor_hander.direction = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
-			}else if (state_prev == STATE_4) {
-				_sensor_hander.direction = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
-			}
-			break;
-		default:
-			_sensor_hander.sensor_error ++;
-			return 0xFFFFFFFF;
-	}
-	rand_angle(theta_now);
-	return theta_now;
-}
-
-static __inline u8 _next_hall(u8 hall_now) {
-	switch (hall_now) {
-		case STATE_1:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_3;
-			}else {
-				return STATE_5;
-			}
-		case STATE_2:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_6;
-			}else {
-				return STATE_3;
-			}
-		case STATE_3:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_2;
-			}else {
-				return STATE_1;
-			}
-		case STATE_4:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_5;
-			}else {
-				return STATE_6;
-			}
-		case STATE_5:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_1;
-			}else {
-				return STATE_4;
-			}
-		case STATE_6:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_4;
-			}else {
-				return STATE_2;
-			}
-		default: //not reached here
-			return STATE_1;
-	}
-
-} 
-
-#ifdef USE_DETECTED_ANGLE
-static __inline__ float _get_delta_angle(u8 now, u8 next) {
-	float delta_angle = _sensor_hander.angle_table[next] - _sensor_hander.angle_table[now];
-	if (_sensor_hander.direction == POSITIVE) {
-		if (delta_angle < 0) { //process cross 360 degree
-			delta_angle += 360.0f;
-		}
-	}else if (_sensor_hander.direction == NEGATIVE) {
-		if (delta_angle > 0) { //process cross 360 degree
-			delta_angle -= 360.0f;
-		}
-		delta_angle = -delta_angle;
-	}
-	return delta_angle;
-}
-#endif
-void hall_sensor_handler(void) {
-	if (_sensor_hander.is_override_angle) {
-		sys_debug("irq: %d:%d\n", (int)get_hall_stat(HALL_READ_TIMES), (int)_sensor_hander.override_el_angle);
-		return;
-	}
-	time_measure_start(&g_meas_hall);
-	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
-	u8 hall_stat_prev = _sensor_hander.hall_stat;
-	u32 hall_ticks_now = timer_count32_get();	
-
-	/*获取当前转子角度*/
-	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
-	if (theta_now == 0xFFFFFFFF) {
-		return;
-	}
-	//获取两次中断的时间间隔，估计速度
-	u32 delta_us = timer_count32_getus(hall_ticks_now, _sensor_hander.hall_ticks);
-	if (delta_us == 0) {
-		return;
-	}
-	//获取两次中断之间转子转过的角度,获取预期的下次hall状态变换转过的角度
-#ifdef USE_DETECTED_ANGLE
-	float delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
-	float next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
-#else
-	float delta_angle = (float)PHASE_60_DEGREE;
-	float next_delta_angle = delta_angle;
-#endif
-	float delta_time = us_2_s(delta_us);
-	float prev_imme_el_speed = _sensor_hander.immediately_el_speed;
-	_sensor_hander.immediately_el_speed = delta_angle/delta_time;
-	float delta_el_speed = fabs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
-	if (delta_el_speed/prev_imme_el_speed >= 0.1f) { //即时速度增加10%,认为不稳定，需要使用即时速度估计转子位置
-		_sensor_hander.trns_detect = true;
-	}else {
-		_sensor_hander.trns_detect = false;
-	}
-	_hall_put_sample(delta_us, delta_angle);
-	os_disable_irq();
-	_sensor_hander.el_speed = _hall_angle_speed();	
-	_sensor_hander.estimate_delta_angle = _sensor_hander.estimate_delta_angle - delta_angle;
-	/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
-	if (_sensor_hander.trns_detect) {
-		_sensor_hander.estimate_el_speed = (next_delta_angle - _sensor_hander.estimate_delta_angle)/delta_angle * _sensor_hander.immediately_el_speed;
-	}else {
-		_sensor_hander.estimate_el_speed = (next_delta_angle - _sensor_hander.estimate_delta_angle)/delta_angle * _sensor_hander.el_speed;
-	}
-	_sensor_hander.next_delta_angle = next_delta_angle;
-	//_sensor_hander.measured_el_angle = theta_now;
-	os_enable_irq();
-	_sensor_hander.hall_stat = hall_stat_now;
-	_sensor_hander.hall_ticks = hall_ticks_now;
-	_sensor_hander.rpm = _sensor_hander.el_speed / 360.0f * 60.0f;
-	
-	time_measure_end(&g_meas_hall);
-}
-
-

Applications/foc/motor/hall.h

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

Applications/foc/motor/motor.c

@@ -1,78 +1,76 @@
 #include "foc/motor/motor.h"
-#include "foc/motor/hall.h"
+#include "foc/motor/current.h"
+#include "foc/core/foc_core.h"
 #include "math/fast_math.h"
+#include "bsp/delay.h"
 #include "bsp/bsp.h"
 #include "bsp/adc.h"
 #include "bsp/pwm.h"
 
-const motor_param_t motor_params = {
-	.poles = 5,
-	.ld = 0.578477f,
-	.lq = 5.78477f,
-	.rs = 1.088f,
-	.inertia = 3.319367f,
-	.b_emf = 4.332566f,
-};
-
 static bool _motor_started = false;
 
-void motor_start() {
+bool mc_start(u8 mode) {
 	if (_motor_started) {
-		return;
+		return true;
+	}
+	if (mode > TRQ_MODE) {
+		PMSM_FOC_SetErrCode(FOC_Param_Err);
+		return false;
+	}
+	if (mc_get_speed() > 10) {
+		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+		return false;
 	}
+	if (!mc_throttle_released()) {
+		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		return false;
+	}
+	pwm_turn_on_low_side();
+	task_udelay(500);
+	phase_current_calibrate();
+	PMSM_FOC_Start(mode);
 	pwm_start();
 	adc_start_convert();
 	_motor_started = true;
+	return true;
 }
 
-void motor_stop() {
+bool mc_stop(void) {
 	if (!_motor_started) {
-		return;
+		return true;
+	}
+	if (mc_get_speed() > 10) {
+		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+		return false;
 	}
+	if (!mc_throttle_released()) {
+		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		return false;
+	}	
 	adc_stop_convert();
 	pwm_stop();
+	PMSM_FOC_Stop();
 	_motor_started = false;
+	return true;
 }
 
-void motor_drvier_low_side(bool on){
-	if (on) {
-		pwm_turn_on_low_side();
-	}
+bool mc_target_speed(s16 rpm) {
+	return PMSM_FOC_Set_Speed(rpm, 1000);
 }
 
-u32  motor_get_raw_speed(void) {
-	return (u32)hall_sensor_get_speed();
+bool mc_cruise_speed(s16 rpm) {
+	return PMSM_FOC_Set_CruiseSpeed(rpm);
 }
 
-u32  motor_get_speed(void) {
-	static u32 _speed = 0;
-	float raw_speed = hall_sensor_get_speed();
-	if (raw_speed == 0.0f) {
-		_speed = 0;
-	}
-	_speed = LowPass_Filter(_speed, raw_speed, 0.8f);
-	return _speed;
+bool mc_enable_cruise(boolean_T enable) {
+	return PMSM_FOC_EnableCruise(enable);
 }
 
-
-void motor_update_duty(s32 A, s32 B, s32 C, s32 next_a, s32 next_b, s32 next_c) {
-#if SHUNT_NUM==THREE_SHUNTS_SAMPLE	
-	pwm_update_duty(A, B, C);
-#else
-	pwm_wait_and_clear_updata();
-	pwm_update_duty_dma(A, B, C, next_a, next_b, next_c);
-#endif
-}
-void motor_update_sample(u32 samp1, u32 samp2, u8 sector) {
-	pwm_update_2smaples(samp1, samp2);
-#ifdef ENABLE_AUX_TIMER
-	if (samp2 < FOC_PWM_Half_Period) {
-		adc_update_ext_trigger(ADC_TRIGGER_PHASE);
-	}else {
-		adc_update_ext_trigger(ADC_TRIGGER_PHASE2);
-	}
-#endif	
-	adc_current_sample_config(sector);
+s16 mc_get_speed(void) {
+	return PMSM_FOC_GetSpeed();
 }
 
+bool mc_throttle_released(void) {
+	return true;
+}
 

Applications/foc/motor/motor.h

@@ -1,24 +1,15 @@
 #ifndef _MOTOR_H__
 #define _MOTOR_H__
-#include "os/os_type.h"
+#include "os/os_types.h"
+#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);
 
-typedef struct _motor_p {
-	int   poles; //电机极对数
-	float  ld;    //q轴电感
-	float  lq;    //d轴电感
-	float  rs;     //定子内阻
-	float  flux_linkage; //永磁磁链
-	float  inertia; 
-	float  b_emf;
-}motor_param_t;
 
-extern const motor_param_t motor_params;
-void motor_start(void);
-void motor_stop(void);
-void motor_drvier_low_side(bool on);
-void motor_update_duty(s32 A, s32 B, s32 C, s32 next_a, s32 next_b, s32 next_c);
-void motor_update_sample(u32 samp1, u32 samp2, u8 sector);
-u32  motor_get_raw_speed(void); //给pid使用
-u32  motor_get_speed(void);
 #endif /* _MOTOR_H__ */
 

Applications/foc/samples.c

@@ -2,60 +2,49 @@
 #include "bsp/bsp.h"
 #include "bsp/adc.h"
 #include "math/fast_math.h"
-#include "os/co_task.h"
-#include "os/timer.h"
+#include "os/os_task.h"
+#include "math/fix_math.h"
 
 typedef struct {
-	float value;
-	float filted_value;
-	float lowpass;
+	sfix6_t value;
+	sfix6_t filted_value;
+	sfix10_t lowpass;
 }samples_t;
 
 static void sample_vbus(void);
-static void sample_gas(void);
-static void sample_timer_handler(timer_t *);
+
+static void sample_timer_handler(shark_timer_t *);
 static samples_t _vbus;
-static samples_t _gas;
-static timer_t   sample_timer = TIMER_INIT(sample_timer, sample_timer_handler);
+
+static shark_timer_t   sample_timer = TIMER_INIT(sample_timer, sample_timer_handler);
 void samples_init(void){
 	_vbus.filted_value = MAX_VBUS_VOLTAGE;
 	_vbus.value = MAX_VBUS_VOLTAGE;
-	_vbus.lowpass = 0.2f;
-	_gas.filted_value = 0.0f;
-	_gas.value = 0.0f;
-	_gas.lowpass = 0.2f;
+	_vbus.lowpass = _F2sFix10(0.2f); 
 
 	sample_vbus();
-	sample_gas();
-	timer_post(&sample_timer, 1);
+
+	shark_timer_post(&sample_timer, 1);
 }
 
-float get_vbus_sample(void) {
+s16 get_vbus_sfix6(void) {
 	return _vbus.filted_value;
 }
 
-float get_gas_sample(void) {
-	return _gas.filted_value;
+float get_vbus_float(void) {
+	return sfix6toF(_vbus.filted_value);
 }
 
-
-static void sample_timer_handler(timer_t *timer) {
+static void sample_timer_handler(shark_timer_t *timer) {
 	sample_vbus();
-	sample_gas();
-	timer_post(&sample_timer, 5);
+	shark_timer_post(&sample_timer, 1);
 }
 
 static void sample_vbus(void){
-	u32 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 16);
-	_vbus.value = ((float)vadc)/(4096.0f) * ADC_REFERENCE_VOLTAGE * 45 / 1000; //1:44
-	LowPass_Filter(_vbus.filted_value, _vbus.value, _vbus.lowpass);
+	s32 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 16);
+	s32 fix_vbus = (vadc * ADC_REFERENCE_VOLTAGE * 45 / 4096);//1:44
+	s32 fix_filter = _vbus.filted_value;
+	LowPass_Filter(fix_filter, fix_vbus, _vbus.lowpass); //sfix6 * sfix10
+	_vbus.filted_value = fix_filter >> 10; // to sfix6
 }
 
-static void sample_gas(void){
-#ifdef GD32_DEMO
-#else
-	u32 vadc = adc_sample_regular_channel(HANDLERBAR_CHAN, 16);
-	_gas.value = vadc * ADC_REFERENCE_VOLTAGE /4096.0f;
-	LowPass_Filter(_gas.filted_value, _gas.value, _gas.lowpass);
-#endif	
-}

Applications/libs/backtrace.c

@@ -3,7 +3,7 @@
 #include <stdio.h>
 #include "libs/backtrace.h"
 #include "bsp/gd32_bkp.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 
 #if defined(__CC_ARM)
     #define SECTION_START(_name_)                _name_##$$Base
@@ -33,8 +33,8 @@
 
 //extern int STACK$$Base;
 static void get_cur_thread_stack_info(uint32_t sp, uint32_t *start_addr, size_t *size) {
-    *start_addr = co_task_current_stack();
-    *size = co_task_stack_size();
+    *start_addr = SECTION_START(STACK);//(uint32_t)vTaskStackAddr();
+    *size = SECTION_END(STACK) - SECTION_START(STACK);//vTaskStackSize() * sizeof( StackType_t );
 }
 
 #ifdef __TARGET_FPU_VFP

Applications/libs/byte_queue.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "os/os_type.h"
+#include "os/os_types.h"
 #include "bsp/bsp.h"
 
 typedef struct {

Applications/libs/circle_buffer.h

@@ -2,7 +2,7 @@
 #define _Cirule_Buffer_h__
 
 #include <stdint.h>
-#include "os/os_type.h"
+#include "os/os_types.h"
 #include "libs/utils.h"
 
 #define CBUFF_SUCCESS 0

Applications/libs/crc16.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 u16 crc16_update_byte(u16 crc, u8 value);
 u16 crc16_update(u16 crc, const u8 *data, u16 size);

Applications/libs/gtime.c

@@ -1,71 +0,0 @@
-#include "gtime.h"
-#include <time.h>
-#include "os/co_task.h"
-//one while will use 4 ins
-
-/* below function is dumy here, will overide by xxx_rtc.c */
-__weak void rtc_set_time_with_utc_second(uint32_t s){
-
-}
-__weak uint32_t rtc_get_timestamp(void){
-	return 0;
-}
-
-
-/* set rtc time, used utc timestamp, can not include time zone*/
-uint32_t calendar_to_utc(time_val *time){
-
-	struct tm utc_time;
-	struct tm my_time;
-	memset(&utc_time, 0, sizeof (struct tm));
-	memset(&my_time, 0, sizeof (struct tm));
-	
-	my_time.tm_mday = time->day;
-	my_time.tm_mon = time->month- 1;
-	my_time.tm_year = time->year - 1900;
-	my_time.tm_hour = time->hour;
-	my_time.tm_min = time->minute;
-	my_time.tm_sec = time->second;
-
-	utc_time.tm_mday = 1;
-	utc_time.tm_mon = 1 - 1;
-	utc_time.tm_year = 1970 - 1900;
-	utc_time.tm_hour = 0;
-	utc_time.tm_min = 0;
-	utc_time.tm_sec = 0;
-
-	return mktime(&my_time) - mktime(&utc_time);
-}
-
-void set_utc_time(time_val *time){
-	uint32_t second = calendar_to_utc(time);
-	rtc_set_time_with_utc_second(second);
-}
-
-/* get the utc readable time */
-void utc_to_calendar(uint32_t second, time_val *out){
-	struct tm *tm;
-	tm = localtime((time_t *)&second);
-
-	out->year = tm->tm_year + 1900;
-	out->month = tm->tm_mon + 1;
-	out->day = tm->tm_mday;
-	out->hour = tm->tm_hour;
-	out->minute = tm->tm_min;
-	out->second = tm->tm_sec;
-}
-
-/*tz quarter-hour ,15min ,-48~+48*/
-uint32_t get_timestamp_with_time_zone(int8_t tz){
-	return (rtc_get_timestamp()+ tz*15*60);
-}
-
-uint32_t get_timestamp(void){
-	return rtc_get_timestamp();
-}
-
-void get_local_time(time_val *out, int8_t tz){
-	utc_to_calendar(get_timestamp_with_time_zone(tz), out);
-}
-
-

Applications/libs/gtime.h

@@ -1,23 +0,0 @@
-#ifndef _G_Time_H__
-#define _G_Time_H__
-
-#include "os/os_type.h"
-
-typedef struct{
-	uint16_t year; 	/*YYYY*/
-	uint8_t month;
-	uint8_t day;
-	uint8_t hour;
-	uint8_t minute;
-	uint8_t second;
-}time_val;
-
-
-uint32_t calendar_to_utc(time_val *time);
-void set_utc_time(time_val *time);
-void get_local_time(time_val *out, int8_t tz);
-uint32_t get_timestamp(void);
-uint32_t get_timestamp_with_time_zone(int8_t tz);
-
-#endif /* _G_Time_H__ */
-

Applications/libs/logger.c

@@ -2,14 +2,14 @@
 #include <stdarg.h>
 #include "bsp/bsp.h"
 #include "logger.h"
-#include "os/co_task.h"
-#include "os/queue.h"
+#include "os/os_task.h"
 
 static uint32_t level_data[2];
 static char log_cache[128];
-static co_queue_t log_queue = NULL;
+//static co_queue_t log_queue = NULL;
 int can_fputc(char c);
 
+#if 0
 static void log_co_task(void *args) {
 	log_chan_t log_v;
 	while(1) {
@@ -31,12 +31,11 @@ void log_start_task(void) {
 	log_queue = queue_create(5000, sizeof(log_chan_t));
 	co_task_create(log_co_task, NULL, 512);
 }
+#endif
+
 
 void log_chan_value(u16 id, s32 value) {
-	log_chan_t log_v;
-	log_v.id = id;
-	log_v.value = value;
-	queue_put(log_queue, &log_v);
+
 }
 
 static int _get_level(int mod){

Applications/libs/logger.h

@@ -1,7 +1,7 @@
 #ifndef _Shark_Logger_h__
 #define _Shark_Logger_h__
 #include <stdarg.h>
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 #pragma  pack (push,1)
 typedef struct {

Applications/libs/task_.c

@@ -1,181 +0,0 @@
-#include "libs/task.h"
-#include "bsp/bsp.h"
-
-static u64 task_mseconds;
-static task_t _task_handler[MAX_TASK];
-static u32 timer_task_handler(void);
-
-static timer_t timer_head = {
-	.prev = &timer_head,
-	.next = &timer_head
-};
-
-static task_t task_head = {
-	.next = &task_head,
-	.handler = timer_task_handler
-};
-
-void system_tick_handler(void)
-{
-	task_mseconds++;
-}
-
-u64 get_mseconds(void)
-{
-	return task_mseconds;
-}
-
-u32 get_seconds(void){
-	return task_mseconds/1000;
-}
-
-void task_ticks_enable(void)
-{
-	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
-}
-
-u32 task_ticks_abs(void)
-{
-	return DWT->CYCCNT;
-}
-
-u32 task_ticks_rel(u32 start)
-{
-	u32 ticks = DWT->CYCCNT;
-
-	if (ticks >= start) {
-		return ticks - start;
-	}
-
-	return 0xFFFFFFFF - start + ticks + 1;
-}
-
-void task_ticks_delay(u32 ticks)
-{
-	u32 start;
-
-	start = task_ticks_abs();
-	while (task_ticks_rel(start) < ticks);
-}
-
-void task_udelay(u32 delay)
-{
-	task_ticks_delay(delay * (SystemCoreClock / 1000000));
-}
-
-
-static inline void timer_sync(void)
-{
-	task_head.time = timer_head.next->time;
-}
-
-void timer_post(timer_t *timer, u32 delay)
-{
-	timer_t *node;
-	u64 time;
-
-	__disable_irq();
-	time = task_mseconds + (delay > 0 ? delay : 1);
-
-	if (timer->prev != NULL) {
-		timer->prev->next = timer->next;
-	}
-
-	if (timer->next != NULL) {
-		timer->next->prev = timer->prev;
-	}
-
-	for (node = timer_head.next; node != &timer_head; node = node->next) {
-		if (node->time > time) {
-			break;
-		}
-	}
-
-	timer->prev = node->prev;
-	node->prev->next = timer;
-	node->prev = timer;
-	timer->next = node;
-	timer->time = time;
-	timer_sync();
-	__enable_irq();
-}
-
-void timer_cancel(timer_t *timer)
-{
-	__disable_irq();
-	if (timer->prev != NULL && timer->next != NULL) {
-		timer->prev->next = timer->next;
-		timer->next->prev = timer->prev;
-	}
-	timer->next = timer->prev = timer;
-	timer_sync();
-	__enable_irq();
-}
-
-static u32 timer_task_handler(void)
-{
-	while (1) {
-		timer_t *timer = timer_head.next;
-
-		if (timer->time > task_mseconds) {
-			return timer->time - task_mseconds;
-		}
-
-		if (timer != &timer_head) {
-			timer_cancel(timer);
-			timer->handler(timer);
-		} else {
-			break;
-		}
-
-		wdog_reload();
-	}
-
-	return 0xFFFFFFFF;
-}
-
-void task_add(task_t *task)
-{
-	__disable_irq();
-
-	task->next = task_head.next;
-	task_head.next = task;
-
-	__enable_irq();
-}
-
-task_t* task_start(task_func func, u32 delay){
-	task_t *task = NULL;
-	
-	for (int i = 0; i < MAX_TASK; i++) {
-		__disable_irq();
-		if (_task_handler[i].handler == NULL) {
-			task = _task_handler + i;
-			task->handler = func;
-			task->time = delay;
-			task->next = task_head.next;
-			task_head.next = task;	
-			__enable_irq();
-			return task;
-		}
-		__enable_irq();
-	}	
-	return NULL;
-}
-
-
-void task_run(void)
-{
-	task_t *head = &task_head;
-
-	while (1) {
-		wdog_reload();
-
-		if (head->time <= task_mseconds) {
-			head->time = task_mseconds + head->handler();
-		}
-
-		head = head->next;
-	}
-}

Applications/libs/task_.h

@@ -1,33 +0,0 @@
-#pragma once
-
-#include "libs/os.h"
-
-typedef u32 (*task_func)(void);
-
-typedef struct _task {
-	u64 time;
-	struct _task *next;
-	u32 (*handler)(void);
-} task_t;
-
-typedef struct _timer {
-	void (*handler)(struct _timer *timer);
-	struct _timer *prev;
-	struct _timer *next;
-	u64 time;	
-} timer_t;
-
-#define MAX_TASK 8
-#define TIMER_INIT(timer, handler) 	{ handler, &timer, &timer }
-
-u64 get_mseconds(void);
-u32 get_seconds(void);
-void timer_post(timer_t *timer, u32 delay);
-void timer_cancel(timer_t *timer);
-void task_add(task_t *task);
-void task_run(void);
-void task_udelay(u32 delay);
-u32 task_ticks_abs(void);
-void task_ticks_enable(void);
-u32 task_ticks_rel(u32 start);
-task_t *task_start(task_func func, u32 delay);

Applications/libs/time_measure.c

@@ -0,0 +1,32 @@
+#include "bsp/bsp.h"
+#include "bsp/delay.h"
+#include "os/os_task.h"
+#include "libs/time_measure.h"
+
+#define COUNT_2_US(c) (c/120)
+
+u32 time_delta_us(u32 count, u32 *p_update) {
+	u32 now = task_ticks_abs();
+	u32 delta = now - count;
+	if (now < count) { //wrap
+		delta = 0xFFFFFFFF - count + now + 1;
+	}
+	if (p_update) {
+		*p_update = now;
+	}
+	return COUNT_2_US(delta);
+}
+
+
+void time_measure_start(measure_time_t *m){
+	m->intval_time = time_delta_us(m->intval_count, &m->intval_count);
+	m->exec_count = task_ticks_abs();
+}
+void time_measure_end(measure_time_t *m) {
+	m->exec_time = time_delta_us(m->exec_count, NULL);
+	if (m->exec_time > m->exec_max_time) {
+		m->exec_time_error ++;
+		m->exec_max_error_time = m->exec_time;
+	}
+}
+

Applications/libs/time_measure.h

@@ -0,0 +1,22 @@
+#ifndef _TIME_MEASURE_H__
+#define _TIME_MEASURE_H__
+#include "os/os_types.h"
+
+typedef struct {
+	u32 exec_count;
+	u32 exec_time;
+	u32 intval_count;
+	u32 intval_time;
+	u32 exec_max_time;
+	u32 exec_max_error_time;
+	u32 exec_time_error;
+	u32 intval_time_error;
+}measure_time_t;
+
+
+void time_measure_start(measure_time_t *m);
+void time_measure_end(measure_time_t *m);
+
+
+#endif	/* _TIME_MEASURE_H__ */
+

Applications/libs/utils.h

@@ -1,13 +1,12 @@
 #ifndef _UTILS_H__
 #define _UTILS_H__
 
-#include "os/os_type.h"
-#include "os/co_task.h"
-
-#define delay_us cpu_udelay
+#include "os/os_types.h"
+#include "os/os_task.h"
+#include "bsp/delay.h"
 
 static __inline__ void delay_ms(u32 ms) {
-	cpu_udelay(ms*1000);
+	task_udelay(ms*1000);
 }
 
 static __inline__ u8 decode_u8(u8 *buff) {

Applications/math/fast_math.c

@@ -3,6 +3,7 @@
 #include <stdlib.h>
 #include <arm_math.h>
 #include "math/fast_math.h"
+#if 0
 /*
 生成sin/cos 查找表，(0-90°，间隔0.1°)
 #include <string.h>
@@ -229,3 +230,4 @@ void fast_sincosf(float angle, float *sin, float *cos){
 		*cos = sin_table[index];		
 	}
 }
+#endif

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 -= (filter_constant) * ((value) - (sample)))
+#define LowPass_Filter(value, sample, filter_constant)	(value = value * (_F2sFix10(1) - filter_constant) + sample * filter_constant)
 
 #endif /* _Fast_Math_H__ */
 

Applications/math/fix_math.h

@@ -0,0 +1,20 @@
+#ifndef _FIX_MATH_H__
+#define _FIX_MATH_H__
+#include "bsp/bsp.h"
+#include "os/os_types.h"
+typedef signed short sfix10_t;
+typedef signed short sfix6_t;
+
+#define _F2sFix10(x) (sfix10_t)(x * 1024.0f)
+#define _F2sFix6(x)  (sfix6_t)(x * 64.0f)
+
+#define Fix10_6(x) (x >> 4)
+
+static __INLINE float sfix6toF(sfix6_t v) {
+	s16 num = (v >> 6) & 0xFFFF;
+	u16 tail = v & 0x003F;
+	float f = num + (float)tail / 64.0f;
+	return f;
+} 
+
+#endif /* _FIX_MATH_H__ */

Applications/os/co_task.c

@@ -1,140 +0,0 @@
-#include "os/co_task.h"
-
-typedef enum {
-	Task_Pending,
-	Task_Blocking,
-	Task_Ready,
-	Task_Running
-}Task_Stat_t;
-typedef struct _tcb{
-	volatile u32 fp;
-	volatile u32 *pstack; //stack addr
-	u32          stack_size;
-	u32 start_tm_ms;
-	Task_Stat_t  stat; //task stat, running, pending, blocked
-	struct _tcb *next;
-}tcb_t;
-
-volatile tcb_t *current_tcb;
-u64 g_task_ticks64;
-u32 g_task_ticks;
-
-static tcb_t *g_tcb_head;
-static volatile u32 _context_swap_cnt = 0;
-
-extern void arch_save_context(void);
-extern void arch_restore_context(void);
-extern void arch_start_first_task(void);
-extern u32 arch_stack_init(volatile u32 *stack_base, u16 stack_size, void *fptr, void *p_arg);
-extern void co_task_tick_init(int ticks);
-
-static tcb_t *_next_task(void);
-static void _task_add(tcb_t *tcb);
-static void _task_context_swap(tcb_t *prev, tcb_t *next);
-
-
-void *co_task_create(task_func func, void *args, u16 stack_size){
-	tcb_t *p_tcb = co_malloc(sizeof(tcb_t));
-	p_tcb->pstack = (u32 *)co_malloc(stack_size);
-	p_tcb->fp = arch_stack_init(p_tcb->pstack, stack_size>>2, func, args);
-	p_tcb->start_tm_ms = 0;
-	p_tcb->stat = Task_Ready;
-	p_tcb->next = p_tcb;
-	_task_add(p_tcb);
-	return p_tcb;
-}
-
-void co_task_schedule(void){
-	co_task_tick_init(TICKS_PER_HZ);
-	current_tcb = g_tcb_head;
-	arch_start_first_task();
-	while(1);
-}
-
-void co_task_yield(void){
-	tcb_t *next = _next_task();
-	if (next != NULL && next != current_tcb) {
-		_task_context_swap((tcb_t *)current_tcb, next);
-	}
-}
-
-u32 co_task_current_stack(void) {
-	return (u32)current_tcb->pstack;
-}
-
-u32 co_task_stack_size(void) {
-	return current_tcb->stack_size;
-}
-
-void co_task_delay(int ms){
-	current_tcb->start_tm_ms = ms + ticks_2_ms(g_task_ticks);
-	while(1) {
-		tcb_t *next = _next_task();
-		if (next != NULL){
-			if (next == current_tcb) {
-				break;
-			}
-			_task_context_swap((tcb_t *)current_tcb, next);
-			break;
-		}else {
-			co_task_yield();
-		}
-	}
-}
-
-u64 co_task_sys64_ticks(void) {
-	return g_task_ticks64;
-}
-
-u64 co_task_sys64_ts(void) {
-	return ticks_2_ms(g_task_ticks64);
-}
-
-/* peak next task to run */
-static tcb_t *_next_task(void) {
-	tcb_t *next = current_tcb->next;
-	for (; next != current_tcb; next = next->next) {
-		if (next->stat != Task_Ready) {
-			continue;
-		}
-		if (next->start_tm_ms <= ticks_2_ms(g_task_ticks)){
-			return next;
-		}
-	}
-	if (current_tcb->start_tm_ms <= ticks_2_ms(g_task_ticks)) {
-		return (tcb_t *)current_tcb;
-	}	
-	return NULL;
-}
-
-/* add task to running list */
-static void _task_add(tcb_t *tcb) {
-	if (g_tcb_head == NULL) {
-		g_tcb_head = tcb;
-		return;
-	}
-	tcb_t *next = g_tcb_head->next;
-	//get the last one
-	for (; next->next != g_tcb_head; next = next->next);
-	tcb->next = next->next;
-	next->next = tcb;
-}
-
-static void _task_context_swap(tcb_t *prev, tcb_t *next) {
-	u32 prev_cnt = ++_context_swap_cnt;
-	arch_save_context();
-	if (prev_cnt == _context_swap_cnt) {
-		current_tcb = next;
-		arch_restore_context();
-	}
-}
-
-extern void *pvPortMalloc(u32);
-extern void vPortFree(void *);
-void *co_malloc(u32 size) {
-	return pvPortMalloc(size);
-}
-
-void co_free(void *ptr) {
-	vPortFree(ptr);
-}

Applications/os/co_task.h

@@ -1,36 +0,0 @@
-#ifndef _OS_TASK_H__
-#define _OS_TASK_H__
-#include "bsp/bsp.h"
-#include "os/os_type.h"
-/*
-Task如果没有调用task_yield/delay 永远执行下去
-*/
-
-#define TICKS_PER_HZ 1000
-#define ticks_2_ms(ticks) (ticks*(1000/TICKS_PER_HZ))
-
-#define os_disable_irq() __disable_irq()
-#define os_enable_irq()  __enable_irq()
-
-typedef void (*task_func)(void *);
-
-void cpu_counts_enable(void);
-u32 cpu_counts_abs(void);
-void cpu_udelay(u32 delay);
-u64 co_task_sys64_ticks(void);
-u64 co_task_sys64_ts(void);
-void *co_task_create(task_func func, void *param, u16 stack_size);
-void co_task_delay(int ms); //task 延时
-void co_task_yield(void); //task 让出cpu
-void co_task_schedule(void); //task开始调度
-void *co_malloc(u32 size);
-void co_free(void *ptr);
-float cpu_count_times(u32 prev);
-float cpu_times_bew_counts(u32 now, u32 prev);
-u32 co_task_current_stack(void);
-u32 co_task_stack_size(void);
-
-#define cpu_count_2_time(c) ((float)c / (float)SystemCoreClock)
-
-#endif /*_OS_TASK_H__*/
-

Applications/os/cpu.c

@@ -1,57 +0,0 @@
-#include "os/os_type.h"
-#include "bsp/bsp.h"
-
-void cpu_counts_enable(void)
-{
-	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
-}
-
-u32 cpu_counts_abs(void)
-{
-	return DWT->CYCCNT;
-}
-
-static __inline__ u32 cpu_counts_rel(u32 start)
-{
-	u32 ticks = DWT->CYCCNT;
-
-	if (ticks >= start) {
-		return ticks - start;
-	}
-
-	return 0xFFFFFFFF - start + ticks + 1;
-}
-
-static void cpu_counts_delay(u32 ticks)
-{
-	volatile u32 start = cpu_counts_abs();
-	while (cpu_counts_rel(start) < ticks);
-}
-
-void cpu_udelay(u32 delay)
-{
-	cpu_counts_delay(delay * (SystemCoreClock / 1000000));
-}
-
-
-float cpu_count_times(u32 prev) {
-	u32 delta = cpu_counts_rel(prev);
-	return (float)delta / (float)SystemCoreClock;
-}
-
-float cpu_times_bew_counts(u32 now, u32 prev) {
-	u32 delta = now - prev;
-	if (now < prev) {
-		delta = 0xFFFFFFFF - prev + now + 1;
-	}
-	return (float)delta / (float)SystemCoreClock;
-}
-
-u32 cpu_count_delta_us(u32 count) {
-	u32 cpu_c = cpu_counts_rel(count);
-	float us = (float)cpu_c/120;
-	
-	return (u32)us;
-}
-

Applications/os/os_task.c

@@ -0,0 +1,159 @@
+#include "os/os_task.h"
+#include "os/os_types.h"
+#include "bsp/fmc_flash.h"
+#include "config.h"
+#include "bsp/bsp.h"
+extern uint32_t get_system_sleep_time(void);
+
+static u64 shark_mseconds;
+static u32 shark_timer_task_handler(void *);
+
+static shark_timer_t shark_timer_head = {
+	.prev = &shark_timer_head,
+	.next = &shark_timer_head
+};
+
+static shark_task_t shark_task_head = {
+	.next = &shark_task_head,
+	.handler = shark_timer_task_handler
+};
+
+
+
+void SysTick_Handler(void)
+{
+	shark_mseconds++;
+}
+
+void os_disable_irq(void) {
+	__disable_irq();
+}
+
+void os_enable_irq(void) {
+	__enable_irq();
+}
+
+
+u64 shark_get_mseconds(void)
+{
+	return shark_mseconds;
+}
+
+u32 shark_get_seconds(void){
+	return shark_mseconds/1000;
+}
+
+static inline void shark_timer_sync(void)
+{
+	shark_task_head.time = shark_timer_head.next->time;
+}
+
+void shark_timer_post(shark_timer_t *timer, u32 delay)
+{
+	shark_timer_t *node;
+	u64 time;
+
+	__disable_irq();
+
+	time = shark_mseconds + (delay > 0 ? delay : 1);
+
+	if (timer->prev != NULL) {
+		timer->prev->next = timer->next;
+	}
+
+	if (timer->next != NULL) {
+		timer->next->prev = timer->prev;
+	}
+
+	for (node = shark_timer_head.next; node != &shark_timer_head; node = node->next) {
+		if (node->time > time) {
+			break;
+		}
+	}
+
+	timer->prev = node->prev;
+	node->prev->next = timer;
+	node->prev = timer;
+	timer->next = node;
+	timer->time = time;
+	shark_timer_sync();
+
+	__enable_irq();
+}
+
+void shark_timer_cancel(shark_timer_t *timer)
+{
+	__disable_irq();
+
+	timer->prev->next = timer->next;
+	timer->next->prev = timer->prev;
+	timer->next = timer->prev = timer;
+	shark_timer_sync();
+
+	__enable_irq();
+}
+
+static u32 shark_timer_task_handler(void *p)
+{
+	while (1) {
+		shark_timer_t *timer = shark_timer_head.next;
+
+		if (timer->time > shark_mseconds) {
+			return timer->time - shark_mseconds;
+		}
+
+		if (timer != &shark_timer_head) {
+			shark_timer_cancel(timer);
+			timer->handler(timer);
+		} else {
+			break;
+		}
+
+		wdog_reload();
+	}
+
+	return 0xFFFFFFFF;
+}
+
+void shark_task_add(shark_task_t *task)
+{
+	__disable_irq();
+
+	task->next = shark_task_head.next;
+	shark_task_head.next = task;
+
+	__enable_irq();
+}
+
+shark_task_t *shark_task_create(TimerFunction_t handler, void *params) {
+	shark_task_t *ntsk = os_alloc(sizeof(shark_task_t));
+	if (ntsk == NULL) {
+		return ntsk;
+	}
+	ntsk->handler = handler;
+	ntsk->params = params;
+	shark_task_add(ntsk);
+	return ntsk;
+}
+
+void shark_task_run(void)
+{
+	shark_task_t *head = &shark_task_head;
+
+	fmc_write_magic(IAP_MAGIC_SUCCESS);
+
+	/* setup systick timer for 1000Hz interrupts */
+	SysTick_Config(SystemCoreClock / 1000);
+	/* configure the systick handler priority */
+	NVIC_SetPriority(SysTick_IRQn, 0x00U);
+
+	while (1) {
+		wdog_reload();
+
+		if (head->time <= shark_mseconds) {
+			head->time = shark_mseconds + head->handler(head->params);
+		}
+
+		head = head->next;
+	}
+}

Applications/os/os_task.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "os/os_types.h"
+#include "bsp/delay.h"
+#define MAX_TASK 32
+
+#define TIMER_INIT(timer, handler) {&timer, &timer, handler}
+typedef uint32_t (*TimerFunction_t)( void * );
+
+typedef struct shark_task {
+	u64 time;
+	struct shark_task *next;
+	TimerFunction_t handler;
+	void *params;
+} shark_task_t;
+
+typedef struct shark_timer {
+	struct shark_timer *prev;
+	struct shark_timer *next;
+	void (*handler)(struct shark_timer *timer);
+	u64 time;
+} shark_timer_t;
+
+void os_enable_irq(void);
+void os_disable_irq(void);
+
+u64 shark_get_mseconds(void);
+u32 shark_get_seconds(void);
+void shark_timer_post(shark_timer_t *timer, u32 delay);
+void shark_timer_cancel(shark_timer_t *timer);
+void shark_task_add(shark_task_t *task);
+void shark_task_run(void);
+shark_task_t *shark_task_create(TimerFunction_t, void *);
+

Applications/os/os_type.h → Applications/os/os_types.h

@@ -6,7 +6,6 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-
 #ifndef TRUE
 #define TRUE 1
 #endif
@@ -33,5 +32,10 @@ typedef unsigned char	uint8;
 typedef unsigned short	uint16;
 typedef unsigned int	uint32;
 
+extern void *pvPortMalloc( size_t xWantedSize );
+#define os_alloc pvPortMalloc
+extern void vPortFree(void *);
+#define os_free vPortFree
+
 #endif /* _OS_Type_H__ */
 

Applications/os/port/cortex-m4.c

@@ -1,66 +0,0 @@
-#include "os/os_type.h"
-#include "bsp/bsp.h"
-
-void co_task_tick_init(int ticks){
-	/* setup systick timer for 1000Hz interrupts */
-	SysTick_Config(SystemCoreClock / ticks);
-}
-
-u32 arch_stack_init(u32 *stack_base, u16 stack_size, void *fptr, void *p_arg) {
-	u32 *p_stk = &stack_base[stack_size];
-	u32 u_stk = (u32) p_stk;
-	u_stk = u_stk & 0xFFFFFFFF8;
-	p_stk = (u32 *)u_stk;
-    *(--p_stk) = (u32)fptr; //r14
-    //r12 - r1
-    p_stk -= 12;
-    *(--p_stk) = (u32)p_arg; //r0
-                                                               										
-    return (u32)p_stk;
-}
-
-__asm void arch_start_first_task(void) {
-	extern current_tcb;
-	PRESERVE8
-	ldr     r0, =current_tcb
-	ldr     r0, [r0]
-	ldr     r0, [r0] //load tcb->sp to psp
-	msr     psp, r0
-	
-    mrs     r0, CONTROL //thread mode use psp stack pointer
-    orr     r0, r0, #2
-    msr     CONTROL, r0	
-	isb
-	pop  {r0-r12, pc}
-	nop
-}
-
-__asm int arch_save_context(void){
-	PRESERVE8
-	STMFD   sp!, {r0-r12,r14}	
-
-	ldr     r0, =current_tcb
-	ldr     r0, [r0]
-	str     sp, [r0] //save sp to tcb->sp
-	bx		lr
-	nop
-}
-
-
-__asm void arch_restore_context(void){
-	PRESERVE8
-	ldr     r0, =current_tcb
-	ldr     r0, [r0]
-	ldr     sp, [r0] //load tcb->sp to r0
-	LDMFD	sp!, {r0-r12, pc}
-}
-
-extern u64 g_task_ticks64;
-extern u32 g_task_ticks;
-
-void SysTick_Handler(void) {
-	g_task_ticks ++;
-	g_task_ticks64 ++;
-}
-
-

Applications/os/queue.c

@@ -1,78 +1,34 @@
 #include "os/queue.h"
-#include "os/co_task.h"
+#include "libs/circle_buffer.h"
 
 typedef struct {
-	u8 * buffer;
-	int16_t count;
+	c_buffer_t buffer;
 	int16_t esize;
-	int16_t w_pos;
-	int16_t r_pos;
 }queue_t;
-static __inline__ bool _have_write_space(queue_t *queue){
-	int16_t buffer_len = queue->count * queue->esize;
-	int16_t size = (queue->w_pos >= queue->r_pos)?(buffer_len - queue->w_pos + queue->r_pos):(queue->r_pos - queue->w_pos);
-	size -= queue->esize;
-	if (size <= 0){
-		return false;
-	}
-	return true;
-}
-
-static __inline__ bool _have_read_space(queue_t *queue){
-	int16_t buffer_len = queue->count * queue->esize;
-	int16_t size = (queue->r_pos > queue->w_pos) ? (buffer_len - queue->r_pos + queue->w_pos) : (queue->w_pos - queue->r_pos);
-	if (size < queue->esize){
-		return false;
-	}
-	return true;
-}
 
 void *queue_create(u16 queue_count, u16 esize){
-	queue_t *queue = (queue_t *)co_malloc(queue_count * esize + sizeof(queue_t));
+	queue_t *queue = (queue_t *)os_alloc(queue_count * esize + sizeof(queue_t));
 	if (queue == NULL) {
 		return NULL;
 	}
-	queue->buffer = (u8 *)(queue + 1);
-	queue->count = queue_count;
 	queue->esize = esize;
-	queue->w_pos = queue->r_pos = 0;
+	circle_buffer_init(&queue->buffer, (u8 *)(queue + 1), queue_count * esize);
 	return queue;
 }
 bool queue_put(co_queue_t vqueue, void *data){
 	queue_t *queue = (queue_t *)vqueue;
-	os_disable_irq();
-	if (!_have_write_space(queue)){
-		os_enable_irq();
-		return false;
+	if (circle_put_data(&queue->buffer, data, queue->esize) > 0) {
+		return true;
 	}
-
-	memcpy(queue->buffer + queue->w_pos, data, queue->esize);
-
-	queue->w_pos = queue->w_pos + queue->esize;
-	if (queue->w_pos == queue->count * queue->esize){
-		queue->w_pos = 0;
-	}
-	os_enable_irq();
-	return true;
-
+	return false;
 }
 
 bool queue_get(co_queue_t vqueue, void *data) {
 	queue_t *queue = (queue_t *)vqueue;
-	os_disable_irq();
-	if(!_have_read_space(queue)){
-		os_enable_irq();
-		return false;
-	}
-	if (data) {
-		memcpy(data, queue->buffer + queue->r_pos, queue->esize);
-	}
-	queue->r_pos = queue->r_pos + queue->esize;
-	if (queue->r_pos == queue->count * queue->esize){
-		queue->r_pos = 0;
+	if (circle_get_data(&queue->buffer, data, queue->esize) > 0) {
+		return true;
 	}
-	os_enable_irq();
-	return true;
+	return false;
 }
 
 

Applications/os/queue.h

@@ -1,6 +1,6 @@
 #ifndef _Queue_H__
 #define _Queue_H__
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 typedef void * co_queue_t;
 

Applications/os/timer.c

@@ -1,72 +0,0 @@
-#include "os/timer.h"
-#include "os/co_task.h"
-#include "bsp/bsp.h"
-
-extern u64 g_task_ticks64;
-
-static void timer_task_handler(void *);
-static timer_t timer_head = {
-	.prev = &timer_head,
-	.next = &timer_head
-};
-static bool _timer_start = false;
-
-void timer_post(timer_t *timer, u32 delay)
-{
-	timer_t *node;
-	u64 time = g_task_ticks64 + (delay > 0 ? delay : 1);
-	os_disable_irq();
-	if (timer->prev != NULL) {
-		timer->prev->next = timer->next;
-	}
-
-	if (timer->next != NULL) {
-		timer->next->prev = timer->prev;
-	}
-
-	for (node = timer_head.next; node != &timer_head; node = node->next) {
-		if (node->time > time) {
-			break;
-		}
-	}
-
-	timer->prev = node->prev;
-	node->prev->next = timer;
-	node->prev = timer;
-	timer->next = node;
-	timer->time = time;
-	os_enable_irq();
-	if (!_timer_start) {
-		co_task_create(timer_task_handler, NULL, 384);
-		_timer_start = true;
-	}
-}
-
-void timer_cancel(timer_t *timer)
-{
-	os_disable_irq();
-	if (timer->prev != NULL && timer->next != NULL) {
-		timer->prev->next = timer->next;
-		timer->next->prev = timer->prev;
-	}
-	timer->next = timer->prev = timer;
-	os_enable_irq();
-}
-
-static void timer_task_handler(void *args)
-{
-	while (1) {
-		timer_t *timer = timer_head.next;
-#if 0
-		if (timer->time > g_task_ticks64) {
-			co_task_delay((u32)(timer->time - g_task_ticks64));
-		}
-#endif
-		if ((timer->time <= g_task_ticks64) && (timer != &timer_head)) {
-			timer_cancel(timer);
-			timer->handler(timer);
-		}
-		co_task_yield();
-	}
-}
-

Applications/os/timer.h

@@ -1,17 +0,0 @@
-#pragma once
-
-#include "os/os_type.h"
-
-typedef struct _timer {
-	void (*handler)(struct _timer *timer);
-	struct _timer *prev;
-	struct _timer *next;
-	u64 time;	
-} timer_t;
-
-#define TIMER_INIT(timer, handler) 	{ handler, &timer, &timer }
-
-void timer_post(timer_t *timer, u32 delay);
-void timer_cancel(timer_t *timer);
-void timer_task_start(void);
-

Applications/prot/can_message.c

@@ -1,5 +1,5 @@
 #include "bsp/bsp.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "libs/logger.h"
 #include "libs/utils.h"
 #include "can_message.h"
@@ -78,13 +78,13 @@ void handle_can_frame(can_id_t id, uint8_t *data, int len){
 		if ((idx == 1) && (len >= 2)) { //first frame for message
 			key = decoder_can_key(data);
 			if (message->data) {
-				co_free(message->data);
+				os_free(message->data);
 			}
 
 			message->key = key;
 			message->dest = id.dest;
 			message->src = id.src;
-			message->data = co_malloc(total * CAN_DLC_LENGTH);
+			message->data = os_alloc(total * CAN_DLC_LENGTH);
 			message->len = 0;
 			message->idx = idx;
 			message->type= id.type;
@@ -120,7 +120,7 @@ static void can_process_message(can_message_t *message){
 			foc_cmd_body_t command;
 			command.cmd = (foc_cmd_t)(message->key & 0xFF);
 			command.can_src = message->src;
-			command.data = co_malloc(message->len);
+			command.data = os_alloc(message->len);
 			if (command.data) {
 				memcpy(command.data, message->data, message->len);
 				foc_send_command(&command);
@@ -132,7 +132,7 @@ static void can_process_message(can_message_t *message){
 
 static void free_can_message(can_message_t *message){
 	if (message->data) {
-		co_free(message->data);
+		os_free(message->data);
 	}
 	message->data = NULL;
 	message->src = 0;

Applications/prot/can_pc_message.c

@@ -3,7 +3,7 @@
 #include "bsp/fmc_flash.h"
 #include "bsp/gd32_bkp.h"
 #include "libs/logger.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 
 static void can_send_backtrace(void){
 	set_log_level(MOD_SYSTEM, L_error);
@@ -37,7 +37,7 @@ bool can_process_iap_message(can_message_t *can_message) {
 		case BUILD_CMD_KEY(0xF0):
 			fmc_write_magic(0xFFFFFFFF);
 			iap_send_data(can_message->src, response, rsplen);
-			co_task_delay(100);
+			delay_ms(100);
 			system_reboot();
 			break;
 		case BUILD_CMD_KEY(0xF5):

Applications/prot/wait_queue.c

@@ -1,9 +1,10 @@
 #include "wait_queue.h"
-#include "os/co_task.h"
+#include "os/os_task.h"
 #include "libs/utils.h"
+#include "bsp/can.h"
 
 void wait_queue_init(wait_queue_t *queue, int max_size){
-	queue->keys = co_malloc(max_size * sizeof(response_t));
+	queue->keys = os_alloc(max_size * sizeof(response_t));
 	memset(queue->keys, 0, sizeof(max_size * sizeof(response_t)));
 	queue->max_key_count = max_size;
 }
@@ -59,7 +60,8 @@ bool wait_queue_key_acked(wait_queue_t *queue, u16 key){
 s32 wait_queue_wait_key(wait_queue_t *queue, s32 index, s32 timeout) {
 	volatile response_t *resp_key = queue->keys + index;
 	while((resp_key->key != ACKED_KEY) && (timeout-- >= 0)){
-		co_task_delay(1);
+		can_rx_poll();
+		delay_ms(1);
 	}
 	if (resp_key->key == ACKED_KEY){
 		return resp_key->data_len;

Applications/prot/wait_queue.h

@@ -1,5 +1,5 @@
 #pragma once
-#include "os/os_type.h"
+#include "os/os_types.h"
 
 typedef struct {
 	u16 key;

Project/MC100_OS.uvoptx → Project/MC100.uvoptx

@@ -28,7 +28,7 @@
     <TargetOption>
       <CLKADS>12000000</CLKADS>
       <OPTTT>
-        <gFlags>1</gFlags>
+        <gFlags>0</gFlags>
         <BeepAtEnd>1</BeepAtEnd>
         <RunSim>0</RunSim>
         <RunTarget>1</RunTarget>
@@ -77,7 +77,7 @@
         <tvExpOptDlg>0</tvExpOptDlg>
         <IsCurrentTarget>1</IsCurrentTarget>
       </OPTFL>
-      <CpuCode>255</CpuCode>
+      <CpuCode>0</CpuCode>
       <DebugOpt>
         <uSim>0</uSim>
         <uTrg>1</uTrg>
@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>4</nTsel>
+        <nTsel>0</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -114,38 +114,13 @@
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
-        <pMon>Segger\JL2CM3.dll</pMon>
+        <pMon>BIN\UL2CM3.DLL</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>UL2CM3</Key>
-          <Name>UL2CM3(-S0 -C0 -P0 ) -FN1 -FC1000 -FD20000000 -FF0GD32F30x_HD -FL010000 -FS08000000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM)</Name>
-        </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>JL2CM3</Key>
-          <Name>-U81480416 -O78 -S1 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST2 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD.FLM -FS08000000 -FL010000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM)</Name>
-        </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>ARMRTXEVENTFLAGS</Key>
-          <Name>-L70 -Z18 -C0 -M0 -T1</Name>
-        </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>DLGTARM</Key>
-          <Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
-        </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>ARMDBGFLAGS</Key>
-          <Name></Name>
-        </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>DLGUARM</Key>
-          <Name></Name>
+          <Name>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD -FS08000000 -FL010000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM))</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint/>
@@ -155,12 +130,12 @@
       <DebugFlag>
         <trace>0</trace>
         <periodic>0</periodic>
-        <aLwin>1</aLwin>
+        <aLwin>0</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
         <aPa>0</aPa>
-        <viewmode>1</viewmode>
+        <viewmode>0</viewmode>
         <vrSel>0</vrSel>
         <aSym>0</aSym>
         <aTbox>0</aTbox>
@@ -196,7 +171,7 @@
 
   <Group>
     <GroupName>Application</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -251,8 +226,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\circle_limitation.c</PathWithFileName>
-      <FilenameWithoutPath>circle_limitation.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\foc_core.c</PathWithFileName>
+      <FilenameWithoutPath>foc_core.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -263,8 +238,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_api.c</PathWithFileName>
-      <FilenameWithoutPath>foc_api.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\ramp_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -275,8 +250,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_core.c</PathWithFileName>
-      <FilenameWithoutPath>foc_core.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\commands.c</PathWithFileName>
+      <FilenameWithoutPath>commands.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -287,8 +262,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_fsm.c</PathWithFileName>
-      <FilenameWithoutPath>foc_fsm.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\samples.c</PathWithFileName>
+      <FilenameWithoutPath>samples.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -299,112 +274,84 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\pi_controller.c</PathWithFileName>
-      <FilenameWithoutPath>pi_controller.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\foc_wapper.c</PathWithFileName>
+      <FilenameWithoutPath>foc_wapper.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+  </Group>
+
+  <Group>
+    <GroupName>Motor</GroupName>
+    <tvExp>0</tvExp>
+    <tvExpOptDlg>0</tvExpOptDlg>
+    <cbSel>0</cbSel>
+    <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>2</GroupNumber>
+      <GroupNumber>3</GroupNumber>
       <FileNumber>9</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\ramp_ctrl.c</PathWithFileName>
-      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\motor\current.c</PathWithFileName>
+      <FilenameWithoutPath>current.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>2</GroupNumber>
+      <GroupNumber>3</GroupNumber>
       <FileNumber>10</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\svpwm.c</PathWithFileName>
-      <FilenameWithoutPath>svpwm.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>2</GroupNumber>
-      <FileNumber>11</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\commands.c</PathWithFileName>
-      <FilenameWithoutPath>commands.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>..\Applications\foc\samples.c</PathWithFileName>
-      <FilenameWithoutPath>samples.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\motor\motor.c</PathWithFileName>
+      <FilenameWithoutPath>motor.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
   </Group>
 
   <Group>
-    <GroupName>Motor</GroupName>
+    <GroupName>Simulink</GroupName>
     <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>3</GroupNumber>
-      <FileNumber>13</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\motor\current.c</PathWithFileName>
-      <FilenameWithoutPath>current.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>3</GroupNumber>
-      <FileNumber>14</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>11</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\motor\hall.c</PathWithFileName>
-      <FilenameWithoutPath>hall.c</FilenameWithoutPath>
+      <PathWithFileName>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller.c</PathWithFileName>
+      <FilenameWithoutPath>PMSM_Controller.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>3</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>12</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\motor\motor.c</PathWithFileName>
-      <FilenameWithoutPath>motor.c</FilenameWithoutPath>
+      <PathWithFileName>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller_data.c</PathWithFileName>
+      <FilenameWithoutPath>PMSM_Controller_data.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>3</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>13</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\motor\ntc.c</PathWithFileName>
-      <FilenameWithoutPath>ntc.c</FilenameWithoutPath>
+      <PathWithFileName>..\Simulink\PMSM_Controller_ert_rtw\Shunt_Three_Sample_Point.c</PathWithFileName>
+      <FilenameWithoutPath>Shunt_Three_Sample_Point.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -417,8 +364,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>4</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>14</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -429,8 +376,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>4</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>15</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -441,8 +388,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>4</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -453,8 +400,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>4</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -473,8 +420,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>5</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <GroupNumber>6</GroupNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -484,18 +431,6 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
-    <File>
-      <GroupNumber>5</GroupNumber>
-      <FileNumber>22</FileNumber>
-      <FileType>4</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\math\arm_cortexM4lf_math.lib</PathWithFileName>
-      <FilenameWithoutPath>arm_cortexM4lf_math.lib</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
   </Group>
 
   <Group>
@@ -505,8 +440,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -517,8 +452,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -529,8 +464,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -541,8 +476,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -553,8 +488,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -565,8 +500,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -577,8 +512,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -589,8 +524,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -601,8 +536,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -613,8 +548,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -625,8 +560,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -637,8 +572,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -649,8 +584,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>6</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -660,6 +595,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>32</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\bsp\delay.c</PathWithFileName>
+      <FilenameWithoutPath>delay.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -669,8 +616,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -681,8 +628,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -693,8 +640,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -705,8 +652,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -717,8 +664,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -729,14 +676,14 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\libs\gtime.c</PathWithFileName>
-      <FilenameWithoutPath>gtime.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\libs\time_measure.c</PathWithFileName>
+      <FilenameWithoutPath>time_measure.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -749,44 +696,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>8</GroupNumber>
-      <FileNumber>42</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\os\port\cortex-m4.c</PathWithFileName>
-      <FilenameWithoutPath>cortex-m4.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>8</GroupNumber>
-      <FileNumber>43</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\os\co_task.c</PathWithFileName>
-      <FilenameWithoutPath>co_task.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>8</GroupNumber>
-      <FileNumber>44</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\os\cpu.c</PathWithFileName>
-      <FilenameWithoutPath>cpu.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>8</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <GroupNumber>9</GroupNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -797,26 +708,26 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <GroupNumber>9</GroupNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\os\timer.c</PathWithFileName>
-      <FilenameWithoutPath>timer.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\os\queue.c</PathWithFileName>
+      <FilenameWithoutPath>queue.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>8</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <GroupNumber>9</GroupNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\os\queue.c</PathWithFileName>
-      <FilenameWithoutPath>queue.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\os\os_task.c</PathWithFileName>
+      <FilenameWithoutPath>os_task.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -829,8 +740,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -841,8 +752,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -853,8 +764,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -865,8 +776,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -877,8 +788,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -889,8 +800,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -901,8 +812,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -913,8 +824,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -925,8 +836,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -937,8 +848,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -949,8 +860,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -961,8 +872,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -973,8 +884,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -985,8 +896,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -997,8 +908,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1009,8 +920,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1029,8 +940,8 @@
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
-      <GroupNumber>10</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <GroupNumber>11</GroupNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1041,8 +952,8 @@
       <bShared>0</bShared>
     </File>
     <File>
-      <GroupNumber>10</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <GroupNumber>11</GroupNumber>
+      <FileNumber>59</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/MC100.uvprojx

@@ -14,16 +14,16 @@
       <uAC6>0</uAC6>
       <TargetOption>
         <TargetCommonOption>
-          <Device>GD32F303CC</Device>
+          <Device>GD32F303RC</Device>
           <Vendor>GigaDevice</Vendor>
           <PackID>GigaDevice.GD32F30x_DFP.2.0.0</PackID>
           <PackURL>http://gd32mcu.21ic.com/data/documents/yingyongruanjian/</PackURL>
           <Cpu>IRAM(0x20000000,0x0C000) IROM(0x08000000,0x040000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
           <StartupFile></StartupFile>
-          <FlashDriverDll>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD -FS08000000 -FL010000 -FP0($$Device:GD32F303CC$Flash\GD32F30x_HD.FLM))</FlashDriverDll>
+          <FlashDriverDll>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD -FS08000000 -FL010000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM))</FlashDriverDll>
           <DeviceId>0</DeviceId>
-          <RegisterFile>$$Device:GD32F303CC$Device\Include\gd32f30x.h</RegisterFile>
+          <RegisterFile>$$Device:GD32F303RC$Device\Include\gd32f30x.h</RegisterFile>
           <MemoryEnv></MemoryEnv>
           <Cmp></Cmp>
           <Asm></Asm>
@@ -33,7 +33,7 @@
           <SLE66CMisc></SLE66CMisc>
           <SLE66AMisc></SLE66AMisc>
           <SLE66LinkerMisc></SLE66LinkerMisc>
-          <SFDFile>$$Device:GD32F303CC$SVD\GD32F30x_HD.svd</SFDFile>
+          <SFDFile>$$Device:GD32F303RC$SVD\GD32F30x_HD.svd</SFDFile>
           <bCustSvd>0</bCustSvd>
           <UseEnv>0</UseEnv>
           <BinPath></BinPath>
@@ -339,7 +339,7 @@
               <MiscControls>--gnu</MiscControls>
               <Define>USE_STDPERIPH_DRIVER,GD32F30X_HD,BACK_TRACE,CONFIG_CAN_IAP</Define>
               <Undefine></Undefine>
-              <IncludePath>..\Librarys\CMSIS\Include,..\Librarys\CMSIS\GD\GD32F30x\Include,..\Librarys\GD32F30x_Drivers\include,..\Applications</IncludePath>
+              <IncludePath>..\Librarys\CMSIS\Include,..\Librarys\CMSIS\GD\GD32F30x\Include,..\Librarys\GD32F30x_Drivers\include,..\Applications;..\Simulink\PMSM_Controller_ert_rtw</IncludePath>
             </VariousControls>
           </Cads>
           <Aads>
@@ -393,135 +393,110 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\app\app.c</FilePath>
             </File>
+            <File>
+              <FileName>nv_storage.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\app\nv_storage.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
           <GroupName>Foc</GroupName>
           <Files>
-            <File>
-              <FileName>circle_limitation.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\circle_limitation.c</FilePath>
-            </File>
-            <File>
-              <FileName>foc_api.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\foc_api.c</FilePath>
-            </File>
             <File>
               <FileName>foc_core.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\foc_core.c</FilePath>
-            </File>
-            <File>
-              <FileName>foc_stm.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\foc_stm.c</FilePath>
-            </File>
-            <File>
-              <FileName>gas_sensor.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\gas_sensor.c</FilePath>
+              <FilePath>..\Applications\foc\core\foc_core.c</FilePath>
             </File>
             <File>
-              <FileName>hall_sensor.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\hall_sensor.c</FilePath>
-            </File>
-            <File>
-              <FileName>ntc_sensor.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\ntc_sensor.c</FilePath>
-            </File>
-            <File>
-              <FileName>phase_current.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\phase_current.c</FilePath>
-            </File>
-            <File>
-              <FileName>pi_controller.c</FileName>
+              <FileName>ramp_ctrl.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\pi_controller.c</FilePath>
+              <FilePath>..\Applications\foc\core\ramp_ctrl.c</FilePath>
             </File>
             <File>
-              <FileName>ramp_ctrl.c</FileName>
+              <FileName>commands.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\ramp_ctrl.c</FilePath>
+              <FilePath>..\Applications\foc\commands.c</FilePath>
             </File>
             <File>
-              <FileName>svpwm.c</FileName>
+              <FileName>samples.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\svpwm.c</FilePath>
+              <FilePath>..\Applications\foc\samples.c</FilePath>
             </File>
             <File>
-              <FileName>vbus_sensor.c</FileName>
+              <FileName>foc_wapper.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\foc\vbus_sensor.c</FilePath>
+              <FilePath>..\Applications\foc\core\foc_wapper.c</FilePath>
             </File>
           </Files>
         </Group>
         <Group>
-          <GroupName>Math</GroupName>
+          <GroupName>Motor</GroupName>
           <Files>
             <File>
-              <FileName>fast_math.c</FileName>
+              <FileName>current.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\math\fast_math.c</FilePath>
+              <FilePath>..\Applications\foc\motor\current.c</FilePath>
             </File>
             <File>
-              <FileName>arm_cortexM4lf_math.lib</FileName>
-              <FileType>4</FileType>
-              <FilePath>..\Applications\math\arm_cortexM4lf_math.lib</FilePath>
+              <FileName>motor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\motor\motor.c</FilePath>
             </File>
           </Files>
         </Group>
         <Group>
-          <GroupName>Proto</GroupName>
+          <GroupName>Simulink</GroupName>
           <Files>
             <File>
-              <FileName>can_message.c</FileName>
+              <FileName>PMSM_Controller.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\prot\can_message.c</FilePath>
+              <FilePath>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller.c</FilePath>
             </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Libs</GroupName>
-          <Files>
             <File>
-              <FileName>backtrace.c</FileName>
+              <FileName>PMSM_Controller_data.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\backtrace.c</FilePath>
+              <FilePath>..\Simulink\PMSM_Controller_ert_rtw\PMSM_Controller_data.c</FilePath>
             </File>
             <File>
-              <FileName>circle_buffer.c</FileName>
+              <FileName>Shunt_Three_Sample_Point.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\circle_buffer.c</FilePath>
+              <FilePath>..\Simulink\PMSM_Controller_ert_rtw\Shunt_Three_Sample_Point.c</FilePath>
             </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>Proto</GroupName>
+          <Files>
             <File>
-              <FileName>logger.c</FileName>
+              <FileName>can_message.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\logger.c</FilePath>
+              <FilePath>..\Applications\prot\can_message.c</FilePath>
             </File>
             <File>
-              <FileName>os.c</FileName>
+              <FileName>wait_queue.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\os.c</FilePath>
+              <FilePath>..\Applications\prot\wait_queue.c</FilePath>
             </File>
             <File>
-              <FileName>crc16.c</FileName>
+              <FileName>can_pc_message.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\crc16.c</FilePath>
+              <FilePath>..\Applications\prot\can_pc_message.c</FilePath>
             </File>
             <File>
-              <FileName>task.c</FileName>
+              <FileName>can_foc_msg.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\task.c</FilePath>
+              <FilePath>..\Applications\prot\can_foc_msg.c</FilePath>
             </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>Math</GroupName>
+          <Files>
             <File>
-              <FileName>byte_queue.c</FileName>
+              <FileName>fast_math.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Applications\libs\byte_queue.c</FilePath>
+              <FilePath>..\Applications\math\fast_math.c</FilePath>
             </File>
           </Files>
         </Group>
@@ -588,6 +563,71 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\bsp\uart.c</FilePath>
             </File>
+            <File>
+              <FileName>timer_count32.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\bsp\timer_count32.c</FilePath>
+            </File>
+            <File>
+              <FileName>delay.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\bsp\delay.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>Libs</GroupName>
+          <Files>
+            <File>
+              <FileName>backtrace.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\libs\backtrace.c</FilePath>
+            </File>
+            <File>
+              <FileName>circle_buffer.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\libs\circle_buffer.c</FilePath>
+            </File>
+            <File>
+              <FileName>logger.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\libs\logger.c</FilePath>
+            </File>
+            <File>
+              <FileName>crc16.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\libs\crc16.c</FilePath>
+            </File>
+            <File>
+              <FileName>byte_queue.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\libs\byte_queue.c</FilePath>
+            </File>
+            <File>
+              <FileName>time_measure.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\libs\time_measure.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>OS</GroupName>
+          <Files>
+            <File>
+              <FileName>heap_4.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\os\heap_4.c</FilePath>
+            </File>
+            <File>
+              <FileName>queue.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\os\queue.c</FilePath>
+            </File>
+            <File>
+              <FileName>os_task.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\os\os_task.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

Project/MC100_OS.uvprojx

@@ -1,783 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
-<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
-
-  <SchemaVersion>2.1</SchemaVersion>
-
-  <Header>### uVision Project, (C) Keil Software</Header>
-
-  <Targets>
-    <Target>
-      <TargetName>GD32F303CC</TargetName>
-      <ToolsetNumber>0x4</ToolsetNumber>
-      <ToolsetName>ARM-ADS</ToolsetName>
-      <pCCUsed>5060750::V5.06 update 6 (build 750)::.\ARMCC</pCCUsed>
-      <uAC6>0</uAC6>
-      <TargetOption>
-        <TargetCommonOption>
-          <Device>GD32F303RC</Device>
-          <Vendor>GigaDevice</Vendor>
-          <PackID>GigaDevice.GD32F30x_DFP.2.0.0</PackID>
-          <PackURL>http://gd32mcu.21ic.com/data/documents/yingyongruanjian/</PackURL>
-          <Cpu>IRAM(0x20000000,0x0C000) IROM(0x08000000,0x040000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
-          <FlashUtilSpec></FlashUtilSpec>
-          <StartupFile></StartupFile>
-          <FlashDriverDll>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD -FS08000000 -FL010000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM))</FlashDriverDll>
-          <DeviceId>0</DeviceId>
-          <RegisterFile>$$Device:GD32F303RC$Device\Include\gd32f30x.h</RegisterFile>
-          <MemoryEnv></MemoryEnv>
-          <Cmp></Cmp>
-          <Asm></Asm>
-          <Linker></Linker>
-          <OHString></OHString>
-          <InfinionOptionDll></InfinionOptionDll>
-          <SLE66CMisc></SLE66CMisc>
-          <SLE66AMisc></SLE66AMisc>
-          <SLE66LinkerMisc></SLE66LinkerMisc>
-          <SFDFile>$$Device:GD32F303RC$SVD\GD32F30x_HD.svd</SFDFile>
-          <bCustSvd>0</bCustSvd>
-          <UseEnv>0</UseEnv>
-          <BinPath></BinPath>
-          <IncludePath></IncludePath>
-          <LibPath></LibPath>
-          <RegisterFilePath></RegisterFilePath>
-          <DBRegisterFilePath></DBRegisterFilePath>
-          <TargetStatus>
-            <Error>0</Error>
-            <ExitCodeStop>0</ExitCodeStop>
-            <ButtonStop>0</ButtonStop>
-            <NotGenerated>0</NotGenerated>
-            <InvalidFlash>1</InvalidFlash>
-          </TargetStatus>
-          <OutputDirectory>.\Objects\</OutputDirectory>
-          <OutputName>MC100</OutputName>
-          <CreateExecutable>1</CreateExecutable>
-          <CreateLib>0</CreateLib>
-          <CreateHexFile>1</CreateHexFile>
-          <DebugInformation>1</DebugInformation>
-          <BrowseInformation>1</BrowseInformation>
-          <ListingPath>.\Listings\</ListingPath>
-          <HexFormatSelection>1</HexFormatSelection>
-          <Merge32K>0</Merge32K>
-          <CreateBatchFile>0</CreateBatchFile>
-          <BeforeCompile>
-            <RunUserProg1>0</RunUserProg1>
-            <RunUserProg2>0</RunUserProg2>
-            <UserProg1Name></UserProg1Name>
-            <UserProg2Name></UserProg2Name>
-            <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
-            <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
-            <nStopU1X>0</nStopU1X>
-            <nStopU2X>0</nStopU2X>
-          </BeforeCompile>
-          <BeforeMake>
-            <RunUserProg1>1</RunUserProg1>
-            <RunUserProg2>0</RunUserProg2>
-            <UserProg1Name>SharkFwVersion gen ..\Applications\version.h</UserProg1Name>
-            <UserProg2Name></UserProg2Name>
-            <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
-            <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
-            <nStopB1X>0</nStopB1X>
-            <nStopB2X>0</nStopB2X>
-          </BeforeMake>
-          <AfterMake>
-            <RunUserProg1>1</RunUserProg1>
-            <RunUserProg2>1</RunUserProg2>
-            <UserProg1Name>fromelf --bin --output ./Output/MC100.bin ./Objects/MC100.axf</UserProg1Name>
-            <UserProg2Name>SharkFwVersion mv  ./Output/MC100.bin</UserProg2Name>
-            <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
-            <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
-            <nStopA1X>0</nStopA1X>
-            <nStopA2X>0</nStopA2X>
-          </AfterMake>
-          <SelectedForBatchBuild>0</SelectedForBatchBuild>
-          <SVCSIdString></SVCSIdString>
-        </TargetCommonOption>
-        <CommonProperty>
-          <UseCPPCompiler>0</UseCPPCompiler>
-          <RVCTCodeConst>0</RVCTCodeConst>
-          <RVCTZI>0</RVCTZI>
-          <RVCTOtherData>0</RVCTOtherData>
-          <ModuleSelection>0</ModuleSelection>
-          <IncludeInBuild>1</IncludeInBuild>
-          <AlwaysBuild>0</AlwaysBuild>
-          <GenerateAssemblyFile>0</GenerateAssemblyFile>
-          <AssembleAssemblyFile>0</AssembleAssemblyFile>
-          <PublicsOnly>0</PublicsOnly>
-          <StopOnExitCode>3</StopOnExitCode>
-          <CustomArgument></CustomArgument>
-          <IncludeLibraryModules></IncludeLibraryModules>
-          <ComprImg>1</ComprImg>
-        </CommonProperty>
-        <DllOption>
-          <SimDllName>SARMCM3.DLL</SimDllName>
-          <SimDllArguments> -REMAP -MPU</SimDllArguments>
-          <SimDlgDll>DCM.DLL</SimDlgDll>
-          <SimDlgDllArguments>-pCM4</SimDlgDllArguments>
-          <TargetDllName>SARMCM3.DLL</TargetDllName>
-          <TargetDllArguments> -MPU</TargetDllArguments>
-          <TargetDlgDll>TCM.DLL</TargetDlgDll>
-          <TargetDlgDllArguments>-pCM4</TargetDlgDllArguments>
-        </DllOption>
-        <DebugOption>
-          <OPTHX>
-            <HexSelection>1</HexSelection>
-            <HexRangeLowAddress>0</HexRangeLowAddress>
-            <HexRangeHighAddress>0</HexRangeHighAddress>
-            <HexOffset>0</HexOffset>
-            <Oh166RecLen>16</Oh166RecLen>
-          </OPTHX>
-        </DebugOption>
-        <Utilities>
-          <Flash1>
-            <UseTargetDll>1</UseTargetDll>
-            <UseExternalTool>0</UseExternalTool>
-            <RunIndependent>0</RunIndependent>
-            <UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
-            <Capability>1</Capability>
-            <DriverSelection>-1</DriverSelection>
-          </Flash1>
-          <bUseTDR>1</bUseTDR>
-          <Flash2>BIN\UL2CM3.DLL</Flash2>
-          <Flash3></Flash3>
-          <Flash4></Flash4>
-          <pFcarmOut></pFcarmOut>
-          <pFcarmGrp></pFcarmGrp>
-          <pFcArmRoot></pFcArmRoot>
-          <FcArmLst>0</FcArmLst>
-        </Utilities>
-        <TargetArmAds>
-          <ArmAdsMisc>
-            <GenerateListings>0</GenerateListings>
-            <asHll>1</asHll>
-            <asAsm>1</asAsm>
-            <asMacX>1</asMacX>
-            <asSyms>1</asSyms>
-            <asFals>1</asFals>
-            <asDbgD>1</asDbgD>
-            <asForm>1</asForm>
-            <ldLst>0</ldLst>
-            <ldmm>1</ldmm>
-            <ldXref>1</ldXref>
-            <BigEnd>0</BigEnd>
-            <AdsALst>1</AdsALst>
-            <AdsACrf>1</AdsACrf>
-            <AdsANop>0</AdsANop>
-            <AdsANot>0</AdsANot>
-            <AdsLLst>1</AdsLLst>
-            <AdsLmap>1</AdsLmap>
-            <AdsLcgr>1</AdsLcgr>
-            <AdsLsym>1</AdsLsym>
-            <AdsLszi>1</AdsLszi>
-            <AdsLtoi>1</AdsLtoi>
-            <AdsLsun>1</AdsLsun>
-            <AdsLven>1</AdsLven>
-            <AdsLsxf>1</AdsLsxf>
-            <RvctClst>0</RvctClst>
-            <GenPPlst>0</GenPPlst>
-            <AdsCpuType>"Cortex-M4"</AdsCpuType>
-            <RvctDeviceName></RvctDeviceName>
-            <mOS>0</mOS>
-            <uocRom>0</uocRom>
-            <uocRam>0</uocRam>
-            <hadIROM>1</hadIROM>
-            <hadIRAM>1</hadIRAM>
-            <hadXRAM>0</hadXRAM>
-            <uocXRam>0</uocXRam>
-            <RvdsVP>2</RvdsVP>
-            <RvdsMve>0</RvdsMve>
-            <RvdsCdeCp>0</RvdsCdeCp>
-            <hadIRAM2>0</hadIRAM2>
-            <hadIROM2>0</hadIROM2>
-            <StupSel>8</StupSel>
-            <useUlib>1</useUlib>
-            <EndSel>0</EndSel>
-            <uLtcg>0</uLtcg>
-            <nSecure>0</nSecure>
-            <RoSelD>3</RoSelD>
-            <RwSelD>3</RwSelD>
-            <CodeSel>0</CodeSel>
-            <OptFeed>0</OptFeed>
-            <NoZi1>0</NoZi1>
-            <NoZi2>0</NoZi2>
-            <NoZi3>0</NoZi3>
-            <NoZi4>0</NoZi4>
-            <NoZi5>0</NoZi5>
-            <Ro1Chk>0</Ro1Chk>
-            <Ro2Chk>0</Ro2Chk>
-            <Ro3Chk>0</Ro3Chk>
-            <Ir1Chk>1</Ir1Chk>
-            <Ir2Chk>0</Ir2Chk>
-            <Ra1Chk>0</Ra1Chk>
-            <Ra2Chk>0</Ra2Chk>
-            <Ra3Chk>0</Ra3Chk>
-            <Im1Chk>1</Im1Chk>
-            <Im2Chk>0</Im2Chk>
-            <OnChipMemories>
-              <Ocm1>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </Ocm1>
-              <Ocm2>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </Ocm2>
-              <Ocm3>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </Ocm3>
-              <Ocm4>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </Ocm4>
-              <Ocm5>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </Ocm5>
-              <Ocm6>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </Ocm6>
-              <IRAM>
-                <Type>0</Type>
-                <StartAddress>0x20000000</StartAddress>
-                <Size>0xc000</Size>
-              </IRAM>
-              <IROM>
-                <Type>1</Type>
-                <StartAddress>0x8000000</StartAddress>
-                <Size>0x40000</Size>
-              </IROM>
-              <XRAM>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </XRAM>
-              <OCR_RVCT1>
-                <Type>1</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT1>
-              <OCR_RVCT2>
-                <Type>1</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT2>
-              <OCR_RVCT3>
-                <Type>1</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT3>
-              <OCR_RVCT4>
-                <Type>1</Type>
-                <StartAddress>0x8000000</StartAddress>
-                <Size>0x40000</Size>
-              </OCR_RVCT4>
-              <OCR_RVCT5>
-                <Type>1</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT5>
-              <OCR_RVCT6>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT6>
-              <OCR_RVCT7>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT7>
-              <OCR_RVCT8>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT8>
-              <OCR_RVCT9>
-                <Type>0</Type>
-                <StartAddress>0x20000000</StartAddress>
-                <Size>0xc000</Size>
-              </OCR_RVCT9>
-              <OCR_RVCT10>
-                <Type>0</Type>
-                <StartAddress>0x0</StartAddress>
-                <Size>0x0</Size>
-              </OCR_RVCT10>
-            </OnChipMemories>
-            <RvctStartVector></RvctStartVector>
-          </ArmAdsMisc>
-          <Cads>
-            <interw>1</interw>
-            <Optim>4</Optim>
-            <oTime>1</oTime>
-            <SplitLS>0</SplitLS>
-            <OneElfS>1</OneElfS>
-            <Strict>0</Strict>
-            <EnumInt>0</EnumInt>
-            <PlainCh>0</PlainCh>
-            <Ropi>0</Ropi>
-            <Rwpi>0</Rwpi>
-            <wLevel>2</wLevel>
-            <uThumb>0</uThumb>
-            <uSurpInc>0</uSurpInc>
-            <uC99>1</uC99>
-            <uGnu>1</uGnu>
-            <useXO>0</useXO>
-            <v6Lang>3</v6Lang>
-            <v6LangP>3</v6LangP>
-            <vShortEn>1</vShortEn>
-            <vShortWch>1</vShortWch>
-            <v6Lto>0</v6Lto>
-            <v6WtE>0</v6WtE>
-            <v6Rtti>0</v6Rtti>
-            <VariousControls>
-              <MiscControls>--gnu</MiscControls>
-              <Define>USE_STDPERIPH_DRIVER,GD32F30X_HD,BACK_TRACE,CONFIG_CAN_IAP</Define>
-              <Undefine></Undefine>
-              <IncludePath>..\Librarys\CMSIS\Include,..\Librarys\CMSIS\GD\GD32F30x\Include,..\Librarys\GD32F30x_Drivers\include,..\Applications</IncludePath>
-            </VariousControls>
-          </Cads>
-          <Aads>
-            <interw>1</interw>
-            <Ropi>0</Ropi>
-            <Rwpi>0</Rwpi>
-            <thumb>0</thumb>
-            <SplitLS>0</SplitLS>
-            <SwStkChk>0</SwStkChk>
-            <NoWarn>0</NoWarn>
-            <uSurpInc>0</uSurpInc>
-            <useXO>0</useXO>
-            <ClangAsOpt>1</ClangAsOpt>
-            <VariousControls>
-              <MiscControls></MiscControls>
-              <Define></Define>
-              <Undefine></Undefine>
-              <IncludePath></IncludePath>
-            </VariousControls>
-          </Aads>
-          <LDads>
-            <umfTarg>1</umfTarg>
-            <Ropi>0</Ropi>
-            <Rwpi>0</Rwpi>
-            <noStLib>0</noStLib>
-            <RepFail>1</RepFail>
-            <useFile>0</useFile>
-            <TextAddressRange>0x08000000</TextAddressRange>
-            <DataAddressRange>0x20000000</DataAddressRange>
-            <pXoBase></pXoBase>
-            <ScatterFile></ScatterFile>
-            <IncludeLibs></IncludeLibs>
-            <IncludeLibsPath></IncludeLibsPath>
-            <Misc></Misc>
-            <LinkerInputFile></LinkerInputFile>
-            <DisabledWarnings></DisabledWarnings>
-          </LDads>
-        </TargetArmAds>
-      </TargetOption>
-      <Groups>
-        <Group>
-          <GroupName>Application</GroupName>
-          <Files>
-            <File>
-              <FileName>main.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\main.c</FilePath>
-            </File>
-            <File>
-              <FileName>app.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\app\app.c</FilePath>
-            </File>
-            <File>
-              <FileName>nv_storage.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\app\nv_storage.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Foc</GroupName>
-          <Files>
-            <File>
-              <FileName>circle_limitation.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\circle_limitation.c</FilePath>
-            </File>
-            <File>
-              <FileName>foc_api.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\foc_api.c</FilePath>
-            </File>
-            <File>
-              <FileName>foc_core.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\foc_core.c</FilePath>
-            </File>
-            <File>
-              <FileName>foc_fsm.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\foc_fsm.c</FilePath>
-            </File>
-            <File>
-              <FileName>pi_controller.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\pi_controller.c</FilePath>
-            </File>
-            <File>
-              <FileName>ramp_ctrl.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\ramp_ctrl.c</FilePath>
-            </File>
-            <File>
-              <FileName>svpwm.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\svpwm.c</FilePath>
-            </File>
-            <File>
-              <FileName>commands.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\commands.c</FilePath>
-            </File>
-            <File>
-              <FileName>samples.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\samples.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Motor</GroupName>
-          <Files>
-            <File>
-              <FileName>current.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\motor\current.c</FilePath>
-            </File>
-            <File>
-              <FileName>hall.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\motor\hall.c</FilePath>
-            </File>
-            <File>
-              <FileName>motor.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\motor\motor.c</FilePath>
-            </File>
-            <File>
-              <FileName>ntc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\motor\ntc.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Proto</GroupName>
-          <Files>
-            <File>
-              <FileName>can_message.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\prot\can_message.c</FilePath>
-            </File>
-            <File>
-              <FileName>wait_queue.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\prot\wait_queue.c</FilePath>
-            </File>
-            <File>
-              <FileName>can_pc_message.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\prot\can_pc_message.c</FilePath>
-            </File>
-            <File>
-              <FileName>can_foc_msg.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\prot\can_foc_msg.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Math</GroupName>
-          <Files>
-            <File>
-              <FileName>fast_math.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\math\fast_math.c</FilePath>
-            </File>
-            <File>
-              <FileName>arm_cortexM4lf_math.lib</FileName>
-              <FileType>4</FileType>
-              <FilePath>..\Applications\math\arm_cortexM4lf_math.lib</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>BSP</GroupName>
-          <Files>
-            <File>
-              <FileName>bsp.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\bsp.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32_bkp.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\gd32_bkp.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32_rtc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\gd32_rtc.c</FilePath>
-            </File>
-            <File>
-              <FileName>gpio.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\gpio.c</FilePath>
-            </File>
-            <File>
-              <FileName>i2c.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\i2c.c</FilePath>
-            </File>
-            <File>
-              <FileName>can.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\can.c</FilePath>
-            </File>
-            <File>
-              <FileName>fmc_flash.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\fmc_flash.c</FilePath>
-            </File>
-            <File>
-              <FileName>pwm.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\pwm.c</FilePath>
-            </File>
-            <File>
-              <FileName>mc_irqs.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\mc_irqs.c</FilePath>
-            </File>
-            <File>
-              <FileName>mc_hall_gpio.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\mc_hall_gpio.c</FilePath>
-            </File>
-            <File>
-              <FileName>adc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\adc.c</FilePath>
-            </File>
-            <File>
-              <FileName>uart.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\uart.c</FilePath>
-            </File>
-            <File>
-              <FileName>timer_count32.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\bsp\timer_count32.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>Libs</GroupName>
-          <Files>
-            <File>
-              <FileName>backtrace.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\libs\backtrace.c</FilePath>
-            </File>
-            <File>
-              <FileName>circle_buffer.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\libs\circle_buffer.c</FilePath>
-            </File>
-            <File>
-              <FileName>logger.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\libs\logger.c</FilePath>
-            </File>
-            <File>
-              <FileName>crc16.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\libs\crc16.c</FilePath>
-            </File>
-            <File>
-              <FileName>byte_queue.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\libs\byte_queue.c</FilePath>
-            </File>
-            <File>
-              <FileName>gtime.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\libs\gtime.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>OS</GroupName>
-          <Files>
-            <File>
-              <FileName>cortex-m4.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\os\port\cortex-m4.c</FilePath>
-            </File>
-            <File>
-              <FileName>co_task.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\os\co_task.c</FilePath>
-            </File>
-            <File>
-              <FileName>cpu.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\os\cpu.c</FilePath>
-            </File>
-            <File>
-              <FileName>heap_4.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\os\heap_4.c</FilePath>
-            </File>
-            <File>
-              <FileName>timer.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\os\timer.c</FilePath>
-            </File>
-            <File>
-              <FileName>queue.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\os\queue.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>GD32F30x_Drivers</GroupName>
-          <Files>
-            <File>
-              <FileName>gd32f30x_adc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_adc.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_bkp.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_bkp.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_can.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_can.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_dma.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_dma.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_exti.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_exti.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_fmc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_fmc.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_fwdgt.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_fwdgt.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_gpio.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_gpio.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_i2c.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_i2c.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_misc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_misc.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_rcu.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_rcu.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_rtc.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_rtc.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_timer.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_timer.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_usart.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_usart.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_pmu.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_pmu.c</FilePath>
-            </File>
-            <File>
-              <FileName>gd32f30x_dbg.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\GD32F30x_Drivers\source\gd32f30x_dbg.c</FilePath>
-            </File>
-          </Files>
-        </Group>
-        <Group>
-          <GroupName>StartUp</GroupName>
-          <Files>
-            <File>
-              <FileName>system_gd32f30x.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Librarys\CMSIS\GD\GD32F30x\Source\system_gd32f30x.c</FilePath>
-            </File>
-            <File>
-              <FileName>startup_gd32f30x_hd.s</FileName>
-              <FileType>2</FileType>
-              <FilePath>..\Librarys\CMSIS\GD\GD32F30x\Source\ARM\startup_gd32f30x_hd.s</FilePath>
-            </File>
-          </Files>
-        </Group>
-      </Groups>
-    </Target>
-  </Targets>
-
-  <RTE>
-    <apis/>
-    <components/>
-    <files/>
-  </RTE>
-
-  <LayerInfo>
-    <Layers>
-      <Layer>
-        <LayName>&lt;Project Info&gt;</LayName>
-        <LayDesc></LayDesc>
-        <LayUrl></LayUrl>
-        <LayKeys></LayKeys>
-        <LayCat></LayCat>
-        <LayLic></LayLic>
-        <LayTarg>0</LayTarg>
-        <LayPrjMark>1</LayPrjMark>
-      </Layer>
-    </Layers>
-  </LayerInfo>
-
-</Project>

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.bat

@@ -0,0 +1,16 @@
+
+call "setup_msvc.bat"
+
+
+call  "\\DESKTOP-C3O4FD7\D$\Program Files\R2020b\bin\win64\checkMATLABRootForDriveMap.exe" "\\DESKTOP-C3O4FD7\D$\Program Files\R2020b"  > mlEnv.txt
+for /f %%a in (mlEnv.txt) do set "%%a"\n
+cd .
+
+if "%1"=="" (nmake MATLAB_ROOT=%MATLAB_ROOT% ALT_MATLAB_ROOT=%ALT_MATLAB_ROOT% MATLAB_BIN=%MATLAB_BIN% ALT_MATLAB_BIN=%ALT_MATLAB_BIN%  -f PMSM_Controller.mk all) else (nmake MATLAB_ROOT=%MATLAB_ROOT% ALT_MATLAB_ROOT=%ALT_MATLAB_ROOT% MATLAB_BIN=%MATLAB_BIN% ALT_MATLAB_BIN=%ALT_MATLAB_BIN%  -f PMSM_Controller.mk %1)
+@if errorlevel 1 goto error_exit
+
+exit 0
+
+:error_exit
+echo The make command returned an error of %errorlevel%
+exit 1

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.c

@@ -0,0 +1,2489 @@
+/*
+ * File: PMSM_Controller.c
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#include "PMSM_Controller.h"
+#include "PMSM_Controller_private.h"
+
+/* Named constants for Chart: '<S3>/Control_Mode_Manager' */
+#define IN_ACTIVE                      ((uint8_T)1U)
+#define IN_NO_ACTIVE_CHILD             ((uint8_T)0U)
+#define IN_OPEN                        ((uint8_T)2U)
+#define IN_SPEED_MODE                  ((uint8_T)1U)
+#define IN_TORQUE_MODE                 ((uint8_T)2U)
+#define OPEN_MODE                      ((uint8_T)0U)
+#define SPD_MODE                       ((uint8_T)1U)
+#define TRQ_MODE                       ((uint8_T)2U)
+
+uint16_T plook_u16s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T
+  maxIndex)
+{
+  uint16_T bpIndex;
+
+  /* Prelookup - Index only
+     Index Search method: 'even'
+     Extrapolation method: 'Clip'
+     Use previous index: 'off'
+     Use last breakpoint for index at or above upper limit: 'on'
+     Remove protection against out-of-range input in generated code: 'off'
+   */
+  if (u <= bp0) {
+    bpIndex = 0U;
+  } else {
+    bpIndex = (uint16_T)((uint32_T)(uint16_T)(u - bp0) / bpSpace);
+    if (bpIndex < maxIndex) {
+    } else {
+      bpIndex = (uint16_T)maxIndex;
+    }
+  }
+
+  return bpIndex;
+}
+
+uint8_T plook_u8u16_evencka(uint16_T u, uint16_T bp0, uint16_T bpSpace, uint32_T
+  maxIndex)
+{
+  uint16_T fbpIndex;
+  uint8_T bpIndex;
+
+  /* Prelookup - Index only
+     Index Search method: 'even'
+     Extrapolation method: 'Clip'
+     Use previous index: 'off'
+     Use last breakpoint for index at or above upper limit: 'on'
+     Remove protection against out-of-range input in generated code: 'off'
+   */
+  if (u <= bp0) {
+    bpIndex = 0U;
+  } else {
+    fbpIndex = (uint16_T)((uint32_T)(uint16_T)((uint32_T)u - bp0) / bpSpace);
+    if (fbpIndex < maxIndex) {
+      bpIndex = (uint8_T)fbpIndex;
+    } else {
+      bpIndex = (uint8_T)maxIndex;
+    }
+  }
+
+  return bpIndex;
+}
+
+int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator)
+{
+  return (((numerator < 0) != (denominator < 0)) && (numerator % denominator !=
+           0) ? -1 : 0) + numerator / denominator;
+}
+
+/*
+ * System initialize for atomic system:
+ *    '<S39>/Counter'
+ *    '<S38>/Counter'
+ */
+void Counter_Init(DW_Counter *localDW, uint16_T rtp_z_cntInit)
+{
+  /* InitializeConditions for UnitDelay: '<S44>/UnitDelay' */
+  localDW->UnitDelay_DSTATE = rtp_z_cntInit;
+}
+
+/*
+ * Output and update for atomic system:
+ *    '<S39>/Counter'
+ *    '<S38>/Counter'
+ */
+uint16_T Counter(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst,
+                 DW_Counter *localDW)
+{
+  uint16_T rty_cnt_0;
+  uint16_T rtu_rst_0;
+
+  /* Switch: '<S44>/Switch1' incorporates:
+   *  Constant: '<S44>/Constant23'
+   *  UnitDelay: '<S44>/UnitDelay'
+   */
+  if (rtu_rst) {
+    rtu_rst_0 = 0U;
+  } else {
+    rtu_rst_0 = localDW->UnitDelay_DSTATE;
+  }
+
+  /* End of Switch: '<S44>/Switch1' */
+
+  /* Sum: '<S43>/Sum1' */
+  rty_cnt_0 = (uint16_T)((uint32_T)rtu_inc + rtu_rst_0);
+
+  /* MinMax: '<S43>/MinMax' */
+  if (rty_cnt_0 < rtu_max) {
+    /* Update for UnitDelay: '<S44>/UnitDelay' */
+    localDW->UnitDelay_DSTATE = rty_cnt_0;
+  } else {
+    /* Update for UnitDelay: '<S44>/UnitDelay' */
+    localDW->UnitDelay_DSTATE = rtu_max;
+  }
+
+  /* End of MinMax: '<S43>/MinMax' */
+  return rty_cnt_0;
+}
+
+/*
+ * Output and update for atomic system:
+ *    '<S35>/either_edge'
+ *    '<S34>/either_edge'
+ */
+boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW)
+{
+  boolean_T rty_y_0;
+
+  /* RelationalOperator: '<S40>/Relational Operator' incorporates:
+   *  UnitDelay: '<S40>/UnitDelay'
+   */
+  rty_y_0 = (rtu_u != localDW->UnitDelay_DSTATE);
+
+  /* Update for UnitDelay: '<S40>/UnitDelay' */
+  localDW->UnitDelay_DSTATE = rtu_u;
+  return rty_y_0;
+}
+
+/* System initialize for atomic system: '<S34>/Debounce_Filter' */
+void Debounce_Filter_Init(DW_Debounce_Filter *localDW)
+{
+  /* SystemInitialize for IfAction SubSystem: '<S35>/Qualification' */
+  /* SystemInitialize for Atomic SubSystem: '<S39>/Counter' */
+  Counter_Init(&localDW->Counter_f, 0);
+
+  /* End of SystemInitialize for SubSystem: '<S39>/Counter' */
+  /* End of SystemInitialize for SubSystem: '<S35>/Qualification' */
+
+  /* SystemInitialize for IfAction SubSystem: '<S35>/Dequalification' */
+  /* SystemInitialize for Atomic SubSystem: '<S38>/Counter' */
+  Counter_Init(&localDW->Counter_d, 0);
+
+  /* End of SystemInitialize for SubSystem: '<S38>/Counter' */
+  /* End of SystemInitialize for SubSystem: '<S35>/Dequalification' */
+}
+
+/* Output and update for atomic system: '<S34>/Debounce_Filter' */
+void Debounce_Filter(boolean_T rtu_u, uint16_T rtu_tAcv, uint16_T rtu_tDeacv,
+                     boolean_T *rty_y, DW_Debounce_Filter *localDW)
+{
+  uint16_T rtb_Sum1_n;
+  boolean_T rtb_RelationalOperator_e;
+
+  /* Outputs for Atomic SubSystem: '<S35>/either_edge' */
+  rtb_RelationalOperator_e = either_edge(rtu_u, &localDW->either_edge_j);
+
+  /* End of Outputs for SubSystem: '<S35>/either_edge' */
+
+  /* If: '<S35>/If2' incorporates:
+   *  Constant: '<S38>/Constant6'
+   *  Constant: '<S39>/Constant6'
+   *  Inport: '<S37>/yPrev'
+   *  Logic: '<S35>/Logical Operator1'
+   *  Logic: '<S35>/Logical Operator2'
+   *  Logic: '<S35>/Logical Operator3'
+   *  Logic: '<S35>/Logical Operator4'
+   *  UnitDelay: '<S35>/UnitDelay'
+   */
+  if (rtu_u && (!localDW->UnitDelay_DSTATE)) {
+    /* Outputs for IfAction SubSystem: '<S35>/Qualification' incorporates:
+     *  ActionPort: '<S39>/Action Port'
+     */
+    /* Outputs for Atomic SubSystem: '<S39>/Counter' */
+    rtb_Sum1_n = Counter(1, rtu_tAcv, rtb_RelationalOperator_e,
+                         &localDW->Counter_f);
+
+    /* End of Outputs for SubSystem: '<S39>/Counter' */
+
+    /* Switch: '<S39>/Switch2' incorporates:
+     *  Constant: '<S39>/Constant6'
+     *  RelationalOperator: '<S39>/Relational Operator2'
+     */
+    *rty_y = ((rtb_Sum1_n > rtu_tAcv) || localDW->UnitDelay_DSTATE);
+
+    /* End of Outputs for SubSystem: '<S35>/Qualification' */
+  } else if ((!rtu_u) && localDW->UnitDelay_DSTATE) {
+    /* Outputs for IfAction SubSystem: '<S35>/Dequalification' incorporates:
+     *  ActionPort: '<S38>/Action Port'
+     */
+    /* Outputs for Atomic SubSystem: '<S38>/Counter' */
+    rtb_Sum1_n = Counter(1, rtu_tDeacv, rtb_RelationalOperator_e,
+                         &localDW->Counter_d);
+
+    /* End of Outputs for SubSystem: '<S38>/Counter' */
+
+    /* Switch: '<S38>/Switch2' incorporates:
+     *  Constant: '<S38>/Constant6'
+     *  RelationalOperator: '<S38>/Relational Operator2'
+     */
+    *rty_y = ((rtb_Sum1_n <= rtu_tDeacv) && localDW->UnitDelay_DSTATE);
+
+    /* End of Outputs for SubSystem: '<S35>/Dequalification' */
+  } else {
+    /* Outputs for IfAction SubSystem: '<S35>/Default' incorporates:
+     *  ActionPort: '<S37>/Action Port'
+     */
+    *rty_y = localDW->UnitDelay_DSTATE;
+
+    /* End of Outputs for SubSystem: '<S35>/Default' */
+  }
+
+  /* End of If: '<S35>/If2' */
+
+  /* Update for UnitDelay: '<S35>/UnitDelay' */
+  localDW->UnitDelay_DSTATE = *rty_y;
+}
+
+/* Output and update for atomic system: '<S45>/Low_Pass_Filter' */
+void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T rty_y[2],
+                     DW_Low_Pass_Filter *localDW)
+{
+  int32_T tmp;
+
+  /* Sum: '<S54>/Sum2' incorporates:
+   *  UnitDelay: '<S54>/UnitDelay1'
+   */
+  tmp = rtu_u[0] - localDW->UnitDelay1_DSTATE[0];
+  if (tmp > 32767) {
+    tmp = 32767;
+  } else {
+    if (tmp < -32768) {
+      tmp = -32768;
+    }
+  }
+
+  /* Product: '<S54>/Divide3' incorporates:
+   *  Sum: '<S54>/Sum2'
+   */
+  rty_y[0] = (int16_T)((rtu_coef * tmp) >> 16);
+
+  /* Sum: '<S54>/Sum3' incorporates:
+   *  UnitDelay: '<S54>/UnitDelay1'
+   */
+  rty_y[0] += localDW->UnitDelay1_DSTATE[0];
+
+  /* Update for UnitDelay: '<S54>/UnitDelay1' incorporates:
+   *  Sum: '<S54>/Sum3'
+   */
+  localDW->UnitDelay1_DSTATE[0] = rty_y[0];
+
+  /* Sum: '<S54>/Sum2' incorporates:
+   *  UnitDelay: '<S54>/UnitDelay1'
+   */
+  tmp = rtu_u[1] - localDW->UnitDelay1_DSTATE[1];
+  if (tmp > 32767) {
+    tmp = 32767;
+  } else {
+    if (tmp < -32768) {
+      tmp = -32768;
+    }
+  }
+
+  /* Product: '<S54>/Divide3' incorporates:
+   *  Sum: '<S54>/Sum2'
+   */
+  rty_y[1] = (int16_T)((rtu_coef * tmp) >> 16);
+
+  /* Sum: '<S54>/Sum3' incorporates:
+   *  UnitDelay: '<S54>/UnitDelay1'
+   */
+  rty_y[1] += localDW->UnitDelay1_DSTATE[1];
+
+  /* Update for UnitDelay: '<S54>/UnitDelay1' incorporates:
+   *  Sum: '<S54>/Sum3'
+   */
+  localDW->UnitDelay1_DSTATE[1] = rty_y[1];
+}
+
+/* Output and update for atomic system: '<S58>/PI_iq' */
+void PI_iq(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T rtu_Kb,
+           int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt,
+           int16_T *rty_pi_out, DW_PI_iq *localDW)
+{
+  int64_T tmp;
+  int32_T rtb_Divide4_k;
+  int32_T rtb_Sum1_hy;
+
+  /* Product: '<S63>/Divide4' */
+  rtb_Divide4_k = (rtu_err * rtu_P) >> 6;
+
+  /* Product: '<S63>/Divide1' incorporates:
+   *  Product: '<S63>/Divide4'
+   */
+  tmp = ((int64_T)rtb_Divide4_k * rtu_I) >> 10;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Sum: '<S63>/Sum2' incorporates:
+   *  Product: '<S63>/Divide1'
+   *  UnitDelay: '<S63>/UnitDelay'
+   */
+  tmp = (((int64_T)rtu_ext_limProt << 4) + (int32_T)tmp) +
+    localDW->UnitDelay_DSTATE;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Sum: '<S65>/Sum1' incorporates:
+   *  Sum: '<S63>/Sum2'
+   *  UnitDelay: '<S65>/UnitDelay'
+   */
+  rtb_Sum1_hy = (int32_T)tmp + localDW->UnitDelay_DSTATE_i;
+
+  /* Sum: '<S63>/Sum6' incorporates:
+   *  Product: '<S63>/Divide4'
+   *  Sum: '<S65>/Sum1'
+   */
+  tmp = (int64_T)rtb_Divide4_k + rtb_Sum1_hy;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Switch: '<S66>/Switch2' incorporates:
+   *  RelationalOperator: '<S66>/LowerRelop1'
+   *  RelationalOperator: '<S66>/UpperRelop'
+   *  Sum: '<S63>/Sum6'
+   *  Switch: '<S66>/Switch'
+   */
+  if ((int32_T)tmp > (rtu_satMax << 4)) {
+    *rty_pi_out = rtu_satMax;
+  } else if ((int32_T)tmp < (rtu_satMin << 4)) {
+    /* Switch: '<S66>/Switch' */
+    *rty_pi_out = rtu_satMin;
+  } else {
+    *rty_pi_out = (int16_T)((int32_T)tmp >> 4);
+  }
+
+  /* End of Switch: '<S66>/Switch2' */
+
+  /* Update for UnitDelay: '<S63>/UnitDelay' incorporates:
+   *  Product: '<S63>/Divide2'
+   *  Sum: '<S63>/Sum3'
+   *  Sum: '<S63>/Sum6'
+   */
+  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)((*rty_pi_out << 4) - (int32_T)
+    tmp) * rtu_Kb) >> 10);
+
+  /* Update for UnitDelay: '<S65>/UnitDelay' incorporates:
+   *  Sum: '<S65>/Sum1'
+   */
+  localDW->UnitDelay_DSTATE_i = rtb_Sum1_hy;
+}
+
+/* Output and update for atomic system: '<S57>/PI_id' */
+void PI_id(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T rtu_Kb,
+           int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt,
+           int16_T *rty_pi_out, DW_PI_id *localDW)
+{
+  int64_T tmp;
+  int32_T rtb_Divide4_e;
+  int32_T rtb_Sum1_l;
+
+  /* Product: '<S59>/Divide4' */
+  rtb_Divide4_e = (rtu_err * rtu_P) >> 6;
+
+  /* Product: '<S59>/Divide1' incorporates:
+   *  Product: '<S59>/Divide4'
+   */
+  tmp = ((int64_T)rtb_Divide4_e * rtu_I) >> 13;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Sum: '<S59>/Sum2' incorporates:
+   *  Product: '<S59>/Divide1'
+   *  UnitDelay: '<S59>/UnitDelay'
+   */
+  tmp = ((((int64_T)(int32_T)tmp << 3) + ((int64_T)rtu_ext_limProt << 3)) +
+         localDW->UnitDelay_DSTATE_j) >> 3;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Sum: '<S61>/Sum1' incorporates:
+   *  Sum: '<S59>/Sum2'
+   *  UnitDelay: '<S61>/UnitDelay'
+   */
+  rtb_Sum1_l = (int32_T)tmp + localDW->UnitDelay_DSTATE;
+
+  /* Sum: '<S59>/Sum6' incorporates:
+   *  Product: '<S59>/Divide4'
+   *  Sum: '<S61>/Sum1'
+   */
+  tmp = ((int64_T)rtb_Sum1_l << 3) + rtb_Divide4_e;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Switch: '<S62>/Switch2' incorporates:
+   *  RelationalOperator: '<S62>/LowerRelop1'
+   *  RelationalOperator: '<S62>/UpperRelop'
+   *  Sum: '<S59>/Sum6'
+   *  Switch: '<S62>/Switch'
+   */
+  if ((int32_T)tmp > (rtu_satMax << 4)) {
+    *rty_pi_out = rtu_satMax;
+  } else if ((int32_T)tmp < (rtu_satMin << 4)) {
+    /* Switch: '<S62>/Switch' */
+    *rty_pi_out = rtu_satMin;
+  } else {
+    *rty_pi_out = (int16_T)((int32_T)tmp >> 4);
+  }
+
+  /* End of Switch: '<S62>/Switch2' */
+
+  /* Update for UnitDelay: '<S59>/UnitDelay' incorporates:
+   *  Product: '<S59>/Divide2'
+   *  Sum: '<S59>/Sum3'
+   *  Sum: '<S59>/Sum6'
+   */
+  localDW->UnitDelay_DSTATE_j = (int32_T)(((int64_T)((*rty_pi_out << 4) -
+    (int32_T)tmp) * rtu_Kb) >> 10);
+
+  /* Update for UnitDelay: '<S61>/UnitDelay' incorporates:
+   *  Sum: '<S61>/Sum1'
+   */
+  localDW->UnitDelay_DSTATE = rtb_Sum1_l;
+}
+
+/* System initialize for atomic system: '<S73>/pi_speed' */
+void pi_speed_Init(DW_pi_speed *localDW)
+{
+  /* InitializeConditions for Delay: '<S77>/Resettable Delay' */
+  localDW->icLoad = 1U;
+}
+
+/* Output and update for atomic system: '<S73>/pi_speed' */
+int16_T pi_speed(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T rtu_Kb,
+                 int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt,
+                 uint8_T rtu_reset, const ConstB_pi_speed *localC, DW_pi_speed
+                 *localDW, ZCE_pi_speed *localZCE)
+{
+  int16_T rty_pi_out_0;
+  int64_T tmp;
+  int32_T rtb_Divide4_hl;
+  int32_T rtb_Sum1_b1;
+
+  /* Product: '<S76>/Divide4' */
+  rtb_Divide4_hl = (rtu_err * rtu_P) >> 2;
+
+  /* Delay: '<S77>/Resettable Delay' incorporates:
+   *  DataTypeConversion: '<S77>/Data Type Conversion2'
+   */
+  if ((rtu_reset > 0) && (localZCE->ResettableDelay_Reset_ZCE != POS_ZCSIG)) {
+    localDW->icLoad = 1U;
+  }
+
+  localZCE->ResettableDelay_Reset_ZCE = (ZCSigState)(rtu_reset > 0);
+  if (localDW->icLoad != 0) {
+    localDW->ResettableDelay_DSTATE = localC->DataTypeConversion2;
+  }
+
+  /* Product: '<S76>/Divide1' incorporates:
+   *  Product: '<S76>/Divide4'
+   */
+  tmp = ((int64_T)rtb_Divide4_hl * rtu_I) >> 10;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Sum: '<S76>/Sum2' incorporates:
+   *  Product: '<S76>/Divide1'
+   *  UnitDelay: '<S76>/UnitDelay'
+   */
+  tmp = (((int64_T)(int32_T)tmp + rtu_ext_limProt) + ((int64_T)
+          localDW->UnitDelay_DSTATE << 2)) >> 2;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Sum: '<S77>/Sum1' incorporates:
+   *  Delay: '<S77>/Resettable Delay'
+   *  Sum: '<S76>/Sum2'
+   */
+  rtb_Sum1_b1 = (int32_T)tmp + localDW->ResettableDelay_DSTATE;
+
+  /* Sum: '<S76>/Sum6' incorporates:
+   *  DataTypeConversion: '<S77>/Data Type Conversion1'
+   *  Product: '<S76>/Divide4'
+   *  Sum: '<S77>/Sum1'
+   */
+  tmp = ((int64_T)(rtb_Sum1_b1 >> 2) << 4) + rtb_Divide4_hl;
+  if (tmp > 2147483647LL) {
+    tmp = 2147483647LL;
+  } else {
+    if (tmp < -2147483648LL) {
+      tmp = -2147483648LL;
+    }
+  }
+
+  /* Switch: '<S78>/Switch2' incorporates:
+   *  RelationalOperator: '<S78>/LowerRelop1'
+   *  RelationalOperator: '<S78>/UpperRelop'
+   *  Sum: '<S76>/Sum6'
+   *  Switch: '<S78>/Switch'
+   */
+  if ((int32_T)tmp > (rtu_satMax << 4)) {
+    rty_pi_out_0 = rtu_satMax;
+  } else if ((int32_T)tmp < (rtu_satMin << 4)) {
+    /* Switch: '<S78>/Switch' */
+    rty_pi_out_0 = rtu_satMin;
+  } else {
+    rty_pi_out_0 = (int16_T)((int32_T)tmp >> 4);
+  }
+
+  /* End of Switch: '<S78>/Switch2' */
+
+  /* Update for UnitDelay: '<S76>/UnitDelay' incorporates:
+   *  Product: '<S76>/Divide2'
+   *  Sum: '<S76>/Sum3'
+   *  Sum: '<S76>/Sum6'
+   */
+  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)((rty_pi_out_0 << 4) -
+    (int32_T)tmp) * rtu_Kb) >> 12);
+
+  /* Update for Delay: '<S77>/Resettable Delay' incorporates:
+   *  Sum: '<S77>/Sum1'
+   */
+  localDW->icLoad = 0U;
+  localDW->ResettableDelay_DSTATE = rtb_Sum1_b1;
+  return rty_pi_out_0;
+}
+
+real_T rt_roundd_snf(real_T u)
+{
+  real_T y;
+  if (fabs(u) < 4.503599627370496E+15) {
+    if (u >= 0.5) {
+      y = floor(u + 0.5);
+    } else if (u > -0.5) {
+      y = u * 0.0;
+    } else {
+      y = ceil(u - 0.5);
+    }
+  } else {
+    y = u;
+  }
+
+  return y;
+}
+
+/* Model step function */
+void PMSM_Controller_step(RT_MODEL *const rtM)
+{
+  DW *rtDW = rtM->dwork;
+  PrevZCX *rtPrevZCX = rtM->prevZCSigState;
+  ExtU *rtU = (ExtU *) rtM->inputs;
+  ExtY *rtY = (ExtY *) rtM->outputs;
+  int32_T sigIdx;
+  int32_T tmp;
+  int32_T tmp_0;
+  int32_T tmp_1;
+  uint32_T tmp_2;
+  int16_T rtb_DataTypeConversion[2];
+  int16_T rtb_TmpSignalConversionAtLow_Pa[2];
+  int16_T rtb_Abs5;
+  int16_T rtb_Abs5_h;
+  int16_T rtb_Divide1_fi;
+  int16_T rtb_Divide4_c;
+  int16_T rtb_Gain1;
+  int16_T rtb_Gain4;
+  int16_T rtb_Max;
+  int16_T rtb_Switch2_ip;
+  int16_T rtb_Switch3_c;
+  int16_T rtb_Switch_b;
+  int16_T rtb_Switch_dr;
+  int16_T rtb_Switch_oi;
+  uint16_T rtb_LogicalOperator3;
+  uint16_T rtb_MultiportSwitch_idx_0;
+  uint16_T rtb_MultiportSwitch_idx_1;
+  uint16_T rtb_Switch2_idx_1;
+  uint16_T rtb_Switch2_idx_2;
+  int8_T UnitDelay3;
+  int8_T rtb_Sum2;
+  int8_T rtb_Sum2_tmp;
+  uint8_T rtb_Add_k;
+  uint8_T rtb_DataTypeConversion1_c;
+  uint8_T rtb_Switch2_fu;
+  uint8_T rtb_UnitDelay;
+  uint8_T rtb_z_ctrlMod;
+  boolean_T rtb_LogicalOperator2;
+  boolean_T rtb_LogicalOperator4;
+  boolean_T rtb_LogicalOperator_p;
+  boolean_T rtb_RelationalOperator4_f;
+  boolean_T rtb_n_commDeacv;
+
+  /* Outputs for Atomic SubSystem: '<Root>/PMSM_Controller' */
+  /* Logic: '<S7>/Edge_Detect' incorporates:
+   *  Delay: '<S7>/Delay'
+   *  Delay: '<S7>/Delay1'
+   *  Delay: '<S7>/Delay2'
+   *  Inport: '<Root>/hall_a'
+   *  Inport: '<Root>/hall_b'
+   *  Inport: '<Root>/hall_c'
+   */
+  rtb_LogicalOperator_p = (boolean_T)((rtU->hall_a != 0) ^ (rtDW->Delay_DSTATE
+    != 0) ^ (rtU->hall_b != 0) ^ (rtDW->Delay1_DSTATE != 0) ^ (rtU->hall_c != 0))
+    ^ (rtDW->Delay2_DSTATE != 0);
+
+  /* Sum: '<S9>/Add' incorporates:
+   *  Gain: '<S9>/Gain'
+   *  Gain: '<S9>/Gain1'
+   *  Inport: '<Root>/hall_a'
+   *  Inport: '<Root>/hall_b'
+   *  Inport: '<Root>/hall_c'
+   */
+  rtb_Add_k = (uint8_T)((uint32_T)(uint8_T)((uint32_T)(uint8_T)(rtU->hall_c << 2)
+    + (uint8_T)(rtU->hall_b << 1)) + rtU->hall_a);
+
+  /* If: '<S2>/If2' incorporates:
+   *  If: '<S10>/If2'
+   *  Inport: '<S15>/z_counterRawPrev'
+   *  UnitDelay: '<S10>/UnitDelay3'
+   */
+  if (rtb_LogicalOperator_p) {
+    /* Outputs for IfAction SubSystem: '<S2>/Direction_Detection' incorporates:
+     *  ActionPort: '<S6>/Action Port'
+     */
+    /* UnitDelay: '<S6>/UnitDelay3' */
+    UnitDelay3 = rtDW->Switch2_i;
+
+    /* End of Outputs for SubSystem: '<S2>/Direction_Detection' */
+
+    /* Selector: '<S9>/Selector' incorporates:
+     *  Constant: '<S9>/vec_hallToPos'
+     */
+    rtb_Sum2_tmp = rtConstP.vec_hallToPos_Value[rtb_Add_k];
+
+    /* Outputs for IfAction SubSystem: '<S2>/Direction_Detection' incorporates:
+     *  ActionPort: '<S6>/Action Port'
+     */
+    /* Sum: '<S6>/Sum2' incorporates:
+     *  Constant: '<S9>/vec_hallToPos'
+     *  Selector: '<S9>/Selector'
+     *  UnitDelay: '<S6>/UnitDelay2'
+     */
+    rtb_Sum2 = (int8_T)(rtb_Sum2_tmp - rtDW->UnitDelay2_DSTATE_j);
+
+    /* Switch: '<S6>/Switch2' incorporates:
+     *  Constant: '<S6>/Constant20'
+     *  Constant: '<S6>/Constant8'
+     *  Logic: '<S6>/Logical Operator3'
+     *  RelationalOperator: '<S6>/Relational Operator1'
+     *  RelationalOperator: '<S6>/Relational Operator6'
+     */
+    if ((rtb_Sum2 == 1) || (rtb_Sum2 == -5)) {
+      /* Switch: '<S6>/Switch2' incorporates:
+       *  Constant: '<S6>/Constant24'
+       */
+      rtDW->Switch2_i = 1;
+    } else {
+      /* Switch: '<S6>/Switch2' incorporates:
+       *  Constant: '<S6>/Constant23'
+       */
+      rtDW->Switch2_i = -1;
+    }
+
+    /* End of Switch: '<S6>/Switch2' */
+
+    /* Update for UnitDelay: '<S6>/UnitDelay2' */
+    rtDW->UnitDelay2_DSTATE_j = rtb_Sum2_tmp;
+
+    /* End of Outputs for SubSystem: '<S2>/Direction_Detection' */
+
+    /* Outputs for IfAction SubSystem: '<S10>/Raw_Motor_Speed_Estimation' incorporates:
+     *  ActionPort: '<S15>/Action Port'
+     */
+    /* RelationalOperator: '<S15>/Relational Operator4' */
+    rtb_RelationalOperator4_f = (rtDW->Switch2_i != UnitDelay3);
+    rtDW->z_counterRawPrev = rtDW->UnitDelay3_DSTATE;
+
+    /* Switch: '<S15>/Switch3' incorporates:
+     *  Constant: '<S15>/Constant4'
+     *  Inport: '<S15>/z_counterRawPrev'
+     *  Logic: '<S15>/Logical Operator1'
+     *  Switch: '<S15>/Switch2'
+     *  UnitDelay: '<S10>/UnitDelay3'
+     *  UnitDelay: '<S15>/UnitDelay1'
+     */
+    if (rtb_RelationalOperator4_f && rtDW->UnitDelay1_DSTATE_i) {
+      rtb_Switch3_c = 0;
+    } else if (rtb_RelationalOperator4_f) {
+      /* Switch: '<S15>/Switch3' incorporates:
+       *  Switch: '<S15>/Switch2'
+       *  UnitDelay: '<S10>/UnitDelay4'
+       */
+      rtb_Switch3_c = rtDW->UnitDelay4_DSTATE;
+    } else {
+      /* Product: '<S15>/Divide13' incorporates:
+       *  Sum: '<S15>/Sum13'
+       *  Switch: '<S15>/Switch2'
+       *  UnitDelay: '<S15>/UnitDelay2'
+       *  UnitDelay: '<S15>/UnitDelay3'
+       *  UnitDelay: '<S15>/UnitDelay5'
+       */
+      tmp_2 = 8000000U / (((rtDW->UnitDelay2_DSTATE + rtDW->UnitDelay3_DSTATE_l)
+                           + rtDW->UnitDelay5_DSTATE) + rtDW->z_counterRawPrev);
+      if (tmp_2 > 32767U) {
+        tmp_2 = 32767U;
+      }
+
+      /* Switch: '<S15>/Switch3' incorporates:
+       *  Product: '<S15>/Divide13'
+       *  Switch: '<S15>/Switch2'
+       */
+      rtb_Switch3_c = (int16_T)tmp_2;
+    }
+
+    /* End of Switch: '<S15>/Switch3' */
+
+    /* Product: '<S15>/Divide11' incorporates:
+     *  Switch: '<S15>/Switch3'
+     */
+    rtDW->Divide11 = (int16_T)(rtb_Switch3_c * rtDW->Switch2_i);
+
+    /* Update for UnitDelay: '<S15>/UnitDelay1' */
+    rtDW->UnitDelay1_DSTATE_i = rtb_RelationalOperator4_f;
+
+    /* Update for UnitDelay: '<S15>/UnitDelay2' incorporates:
+     *  UnitDelay: '<S15>/UnitDelay3'
+     */
+    rtDW->UnitDelay2_DSTATE = rtDW->UnitDelay3_DSTATE_l;
+
+    /* Update for UnitDelay: '<S15>/UnitDelay3' incorporates:
+     *  UnitDelay: '<S15>/UnitDelay5'
+     */
+    rtDW->UnitDelay3_DSTATE_l = rtDW->UnitDelay5_DSTATE;
+
+    /* Update for UnitDelay: '<S15>/UnitDelay5' */
+    rtDW->UnitDelay5_DSTATE = rtDW->z_counterRawPrev;
+
+    /* End of Outputs for SubSystem: '<S10>/Raw_Motor_Speed_Estimation' */
+  }
+
+  /* End of If: '<S2>/If2' */
+
+  /* Switch: '<S8>/Switch3' incorporates:
+   *  Constant: '<S8>/Constant16'
+   *  Constant: '<S8>/Constant2'
+   *  Constant: '<S9>/vec_hallToPos'
+   *  RelationalOperator: '<S8>/Relational Operator7'
+   *  Selector: '<S9>/Selector'
+   *  Sum: '<S8>/Sum1'
+   */
+  if (rtDW->Switch2_i == 1) {
+    rtb_Sum2 = rtConstP.vec_hallToPos_Value[rtb_Add_k];
+  } else {
+    rtb_Sum2 = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Add_k] + 1);
+  }
+
+  /* End of Switch: '<S8>/Switch3' */
+
+  /* MinMax: '<S8>/MinMax' incorporates:
+   *  Inport: '<Root>/hw_count'
+   */
+  if (rtU->hw_count < rtDW->z_counterRawPrev) {
+    tmp_2 = rtU->hw_count;
+  } else {
+    tmp_2 = rtDW->z_counterRawPrev;
+  }
+
+  /* End of MinMax: '<S8>/MinMax' */
+
+  /* Sum: '<S8>/Sum3' incorporates:
+   *  Product: '<S8>/Divide1'
+   *  Product: '<S8>/Divide3'
+   */
+  rtb_Switch3_c = (int16_T)(((int16_T)((int16_T)(((uint64_T)tmp_2 << 14) /
+    rtDW->z_counterRawPrev) * rtDW->Switch2_i) + (rtb_Sum2 << 14)) >> 2);
+
+  /* MinMax: '<S8>/MinMax1' incorporates:
+   *  Constant: '<S8>/Constant1'
+   *  Sum: '<S8>/Sum3'
+   *  Switch: '<S8>/Switch2'
+   */
+  if (rtb_Switch3_c <= 0) {
+    rtb_Switch3_c = 0;
+  }
+
+  /* End of MinMax: '<S8>/MinMax1' */
+
+  /* Sum: '<S11>/Add2' incorporates:
+   *  Constant: '<S11>/Constant2'
+   *  Product: '<S8>/Divide2'
+   */
+  rtb_Switch3_c = (int16_T)((((15 * rtb_Switch3_c) >> 4) + 3840) >> 2);
+
+  /* If: '<S11>/If' incorporates:
+   *  Constant: '<S11>/Constant3'
+   *  DataTypeConversion: '<S11>/Data Type Conversion'
+   *  Inport: '<S12>/In1'
+   *  Merge: '<S11>/Merge'
+   *  Sum: '<S11>/Add'
+   *  Sum: '<S11>/Add2'
+   */
+  if ((int16_T)(rtb_Switch3_c >> 4) >= 360) {
+    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem' incorporates:
+     *  ActionPort: '<S12>/Action Port'
+     */
+    rtb_Switch3_c = (int16_T)(rtb_Switch3_c - 5760);
+
+    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem' */
+  }
+
+  /* End of If: '<S11>/If' */
+
+  /* Switch: '<S10>/Switch2' incorporates:
+   *  Constant: '<S10>/Constant4'
+   *  Inport: '<Root>/hw_count'
+   *  Product: '<S15>/Divide11'
+   *  RelationalOperator: '<S10>/Relational Operator2'
+   */
+  if (rtU->hw_count >= 400000U) {
+    rtb_Switch2_ip = 0;
+  } else {
+    rtb_Switch2_ip = rtDW->Divide11;
+  }
+
+  /* End of Switch: '<S10>/Switch2' */
+
+  /* Abs: '<S10>/Abs5' incorporates:
+   *  Switch: '<S10>/Switch2'
+   */
+  if (rtb_Switch2_ip < 0) {
+    rtb_Abs5 = (int16_T)-rtb_Switch2_ip;
+  } else {
+    rtb_Abs5 = rtb_Switch2_ip;
+  }
+
+  /* End of Abs: '<S10>/Abs5' */
+
+  /* If: '<S10>/If1' */
+  if (rtb_LogicalOperator_p) {
+    /* Outputs for IfAction SubSystem: '<S10>/Subsystem' incorporates:
+     *  ActionPort: '<S16>/Action Port'
+     */
+    /* Relay: '<S16>/n_commDeacv' incorporates:
+     *  Abs: '<S10>/Abs5'
+     */
+    rtDW->n_commDeacv_Mode = ((rtb_Abs5 >= 120) || ((rtb_Abs5 > 60) &&
+      rtDW->n_commDeacv_Mode));
+
+    /* RelationalOperator: '<S18>/Compare' incorporates:
+     *  Constant: '<S18>/Constant'
+     *  Relay: '<S16>/n_commDeacv'
+     *  Sum: '<S16>/Sum13'
+     *  UnitDelay: '<S16>/UnitDelay2'
+     *  UnitDelay: '<S16>/UnitDelay3'
+     *  UnitDelay: '<S16>/UnitDelay5'
+     */
+    rtDW->Compare = ((uint16_T)((uint32_T)(uint16_T)((uint32_T)(uint16_T)
+      ((uint32_T)rtDW->UnitDelay2_DSTATE_f + rtDW->UnitDelay3_DSTATE_lh) +
+      rtDW->UnitDelay5_DSTATE_f) + rtDW->n_commDeacv_Mode) >= 4);
+
+    /* Update for UnitDelay: '<S16>/UnitDelay2' incorporates:
+     *  UnitDelay: '<S16>/UnitDelay3'
+     */
+    rtDW->UnitDelay2_DSTATE_f = rtDW->UnitDelay3_DSTATE_lh;
+
+    /* Update for UnitDelay: '<S16>/UnitDelay3' incorporates:
+     *  UnitDelay: '<S16>/UnitDelay5'
+     */
+    rtDW->UnitDelay3_DSTATE_lh = rtDW->UnitDelay5_DSTATE_f;
+
+    /* Update for UnitDelay: '<S16>/UnitDelay5' incorporates:
+     *  Logic: '<S16>/Logical Operator3'
+     *  Relay: '<S16>/n_commDeacv'
+     */
+    rtDW->UnitDelay5_DSTATE_f = rtDW->n_commDeacv_Mode;
+
+    /* End of Outputs for SubSystem: '<S10>/Subsystem' */
+  }
+
+  /* End of If: '<S10>/If1' */
+
+  /* Switch: '<S2>/Switch' incorporates:
+   *  Inport: '<Root>/b_hall_calibrate'
+   *  Inport: '<Root>/open_theta'
+   *  Merge: '<S11>/Merge'
+   */
+  if (rtU->b_hall_calibrate) {
+    rtb_Switch_b = (int16_T)(rtU->open_theta << 4);
+  } else {
+    rtb_Switch_b = rtb_Switch3_c;
+  }
+
+  /* End of Switch: '<S2>/Switch' */
+
+  /* Abs: '<S3>/Abs2' incorporates:
+   *  Switch: '<S10>/Switch2'
+   */
+  if (rtb_Switch2_ip < 0) {
+    rtb_LogicalOperator3 = (uint16_T)((uint32_T)-rtb_Switch2_ip >> 2);
+  } else {
+    rtb_LogicalOperator3 = (uint16_T)((uint32_T)rtb_Switch2_ip >> 2);
+  }
+
+  /* End of Abs: '<S3>/Abs2' */
+
+  /* UnitDelay: '<S34>/UnitDelay' */
+  rtb_UnitDelay = rtDW->UnitDelay_DSTATE_j;
+
+  /* Outport: '<Root>/VqPrev' incorporates:
+   *  UnitDelay: '<S5>/UnitDelay2'
+   */
+  rtY->VqPrev = rtDW->UnitDelay2_DSTATE_p;
+
+  /* Switch: '<S34>/Switch3' incorporates:
+   *  Abs: '<S10>/Abs5'
+   *  Abs: '<S34>/Abs4'
+   *  Constant: '<S34>/CTRL_COMM4'
+   *  Inport: '<Root>/b_motEna'
+   *  Logic: '<S34>/Logical Operator1'
+   *  RelationalOperator: '<S10>/Relational Operator9'
+   *  RelationalOperator: '<S34>/Relational Operator7'
+   *  S-Function (sfix_bitop): '<S34>/Bitwise Operator1'
+   *  UnitDelay: '<S5>/UnitDelay2'
+   */
+  if ((rtb_UnitDelay & 4U) != 0U) {
+    rtb_LogicalOperator_p = true;
+  } else {
+    if (rtDW->UnitDelay2_DSTATE_p < 0) {
+      /* Abs: '<S34>/Abs4' incorporates:
+       *  UnitDelay: '<S5>/UnitDelay2'
+       */
+      rtb_Divide1_fi = (int16_T)-rtDW->UnitDelay2_DSTATE_p;
+    } else {
+      /* Abs: '<S34>/Abs4' incorporates:
+       *  UnitDelay: '<S5>/UnitDelay2'
+       */
+      rtb_Divide1_fi = rtDW->UnitDelay2_DSTATE_p;
+    }
+
+    rtb_LogicalOperator_p = (rtU->b_motEna && (rtb_Abs5 < 12) && (rtb_Divide1_fi
+      > 960));
+  }
+
+  /* End of Switch: '<S34>/Switch3' */
+
+  /* Sum: '<S34>/Sum' incorporates:
+   *  Constant: '<S34>/CTRL_COMM'
+   *  Constant: '<S34>/CTRL_COMM1'
+   *  DataTypeConversion: '<S34>/Data Type Conversion3'
+   *  Gain: '<S34>/g_Hb'
+   *  Gain: '<S34>/g_Hb1'
+   *  RelationalOperator: '<S34>/Relational Operator1'
+   *  RelationalOperator: '<S34>/Relational Operator3'
+   */
+  rtb_DataTypeConversion1_c = (uint8_T)(((uint32_T)((rtb_Add_k == 7) << 1) +
+    (rtb_Add_k == 0)) + (rtb_LogicalOperator_p << 2));
+
+  /* Outputs for Atomic SubSystem: '<S34>/Debounce_Filter' */
+  /* RelationalOperator: '<S34>/Relational Operator2' incorporates:
+   *  Constant: '<S34>/CTRL_COMM2'
+   *  Constant: '<S34>/t_errDequal'
+   *  Constant: '<S34>/t_errQual'
+   */
+  Debounce_Filter(rtb_DataTypeConversion1_c != 0, 1600, 12000,
+                  &rtb_RelationalOperator4_f, &rtDW->Debounce_Filter_i);
+
+  /* End of Outputs for SubSystem: '<S34>/Debounce_Filter' */
+
+  /* Logic: '<S21>/Logical Operator12' incorporates:
+   *  Inport: '<Root>/b_motEna'
+   *  Logic: '<S21>/Logical Operator7'
+   */
+  rtb_n_commDeacv = ((!rtb_RelationalOperator4_f) && rtU->b_motEna);
+
+  /* Logic: '<S21>/Logical Operator4' incorporates:
+   *  Constant: '<S21>/constant8'
+   *  Inport: '<Root>/b_hall_calibrate'
+   *  Inport: '<Root>/n_ctrlModReq'
+   *  Logic: '<S21>/Logical Operator11'
+   *  Logic: '<S21>/Logical Operator8'
+   *  RelationalOperator: '<S21>/Relational Operator10'
+   */
+  rtb_LogicalOperator4 = (rtU->b_hall_calibrate || (!rtDW->Compare) ||
+    (!rtb_n_commDeacv) || (rtU->n_ctrlModReq == 0));
+
+  /* Relay: '<S21>/n_SpeedCtrl' */
+  rtDW->n_SpeedCtrl_Mode = ((rtb_LogicalOperator3 >= 300) ||
+    ((rtb_LogicalOperator3 > 200) && rtDW->n_SpeedCtrl_Mode));
+  rtb_LogicalOperator_p = rtDW->n_SpeedCtrl_Mode;
+
+  /* Logic: '<S21>/Logical Operator10' incorporates:
+   *  Inport: '<Root>/b_cruiseEna'
+   */
+  rtb_LogicalOperator_p = (rtb_LogicalOperator_p && rtU->b_cruiseEna);
+
+  /* Logic: '<S21>/Logical Operator2' incorporates:
+   *  Constant: '<S21>/constant'
+   *  Inport: '<Root>/n_ctrlModReq'
+   *  Logic: '<S21>/Logical Operator5'
+   *  RelationalOperator: '<S21>/Relational Operator4'
+   */
+  rtb_LogicalOperator2 = ((rtU->n_ctrlModReq == 2) && (!rtb_LogicalOperator_p));
+
+  /* Logic: '<S21>/Logical Operator1' incorporates:
+   *  Constant: '<S21>/constant1'
+   *  Inport: '<Root>/n_ctrlModReq'
+   *  RelationalOperator: '<S21>/Relational Operator1'
+   */
+  rtb_LogicalOperator_p = ((rtU->n_ctrlModReq == 1) || rtb_LogicalOperator_p);
+
+  /* Chart: '<S3>/Control_Mode_Manager' incorporates:
+   *  Logic: '<S21>/Logical Operator3'
+   *  Logic: '<S21>/Logical Operator6'
+   *  Logic: '<S21>/Logical Operator9'
+   */
+  if (rtDW->is_active_c5_PMSM_Controller == 0U) {
+    rtDW->is_active_c5_PMSM_Controller = 1U;
+    rtDW->is_c5_PMSM_Controller = IN_OPEN;
+    rtb_z_ctrlMod = OPEN_MODE;
+  } else if (rtDW->is_c5_PMSM_Controller == 1) {
+    if (rtb_LogicalOperator4) {
+      rtDW->is_ACTIVE = IN_NO_ACTIVE_CHILD;
+      rtDW->is_c5_PMSM_Controller = IN_OPEN;
+      rtb_z_ctrlMod = OPEN_MODE;
+    } else if (rtDW->is_ACTIVE == 1) {
+      rtb_z_ctrlMod = SPD_MODE;
+      if (!rtb_LogicalOperator_p) {
+        if (rtb_LogicalOperator2) {
+          rtDW->is_ACTIVE = IN_TORQUE_MODE;
+          rtb_z_ctrlMod = TRQ_MODE;
+        } else {
+          rtDW->is_ACTIVE = IN_SPEED_MODE;
+        }
+      }
+    } else {
+      /* case IN_TORQUE_MODE: */
+      rtb_z_ctrlMod = TRQ_MODE;
+      if (!rtb_LogicalOperator2) {
+        rtDW->is_ACTIVE = IN_SPEED_MODE;
+        rtb_z_ctrlMod = SPD_MODE;
+      }
+    }
+  } else {
+    /* case IN_OPEN: */
+    rtb_z_ctrlMod = OPEN_MODE;
+    if ((!rtb_LogicalOperator4) && (rtb_LogicalOperator2 ||
+         rtb_LogicalOperator_p)) {
+      rtDW->is_c5_PMSM_Controller = IN_ACTIVE;
+      if (rtb_LogicalOperator2) {
+        rtDW->is_ACTIVE = IN_TORQUE_MODE;
+        rtb_z_ctrlMod = TRQ_MODE;
+      } else {
+        rtDW->is_ACTIVE = IN_SPEED_MODE;
+        rtb_z_ctrlMod = SPD_MODE;
+      }
+    }
+  }
+
+  /* End of Chart: '<S3>/Control_Mode_Manager' */
+
+  /* Switch: '<S22>/Switch' incorporates:
+   *  Constant: '<S22>/Constant3'
+   *  Inport: '<Root>/input_target'
+   */
+  if (rtU->input_target > 60) {
+    /* Switch: '<S22>/Switch1' incorporates:
+     *  Constant: '<S22>/Constant1'
+     *  DataTypeConversion: '<S22>/Data Type Conversion'
+     *  Switch: '<S22>/Switch'
+     */
+    if (rtb_n_commDeacv) {
+      rtb_Switch_oi = rtU->input_target;
+    } else {
+      rtb_Switch_oi = 0;
+    }
+
+    /* End of Switch: '<S22>/Switch1' */
+  } else {
+    rtb_Switch_oi = 0;
+  }
+
+  /* End of Switch: '<S22>/Switch' */
+
+  /* Switch: '<S22>/Switch3' incorporates:
+   *  Constant: '<S22>/Constant4'
+   *  DataTypeConversion: '<S22>/Data Type Conversion2'
+   *  Inport: '<Root>/vq_open_target'
+   */
+  if (rtb_n_commDeacv) {
+    rtb_Abs5_h = rtU->vq_open_target;
+  } else {
+    rtb_Abs5_h = 0;
+  }
+
+  /* End of Switch: '<S22>/Switch3' */
+
+  /* If: '<S23>/If' incorporates:
+   *  Inport: '<Root>/b_hall_calibrate'
+   *  Inport: '<S27>/vq_in'
+   *  Switch: '<S22>/Switch3'
+   */
+  if (rtU->b_hall_calibrate) {
+    /* Switch: '<S22>/Switch2' incorporates:
+     *  Constant: '<S22>/Constant2'
+     *  DataTypeConversion: '<S22>/Data Type Conversion1'
+     *  Inport: '<Root>/vd_open_target'
+     *  Inport: '<S27>/vd_in'
+     */
+    if (rtb_n_commDeacv) {
+      /* Outputs for IfAction SubSystem: '<S23>/If Action Subsystem' incorporates:
+       *  ActionPort: '<S27>/Action Port'
+       */
+      rtDW->Merge[0] = rtU->vd_open_target;
+
+      /* End of Outputs for SubSystem: '<S23>/If Action Subsystem' */
+    } else {
+      /* Outputs for IfAction SubSystem: '<S23>/If Action Subsystem' incorporates:
+       *  ActionPort: '<S27>/Action Port'
+       */
+      rtDW->Merge[0] = 0;
+
+      /* End of Outputs for SubSystem: '<S23>/If Action Subsystem' */
+    }
+
+    /* End of Switch: '<S22>/Switch2' */
+
+    /* Outputs for IfAction SubSystem: '<S23>/If Action Subsystem' incorporates:
+     *  ActionPort: '<S27>/Action Port'
+     */
+    rtDW->Merge[1] = rtb_Abs5_h;
+
+    /* End of Outputs for SubSystem: '<S23>/If Action Subsystem' */
+  } else if (rtb_z_ctrlMod == 0) {
+    /* Outputs for IfAction SubSystem: '<S23>/open_mode' incorporates:
+     *  ActionPort: '<S28>/Action Port'
+     */
+    /* RelationalOperator: '<S28>/Equal1' incorporates:
+     *  Switch: '<S22>/Switch3'
+     *  UnitDelay: '<S28>/Unit Delay'
+     */
+    rtb_LogicalOperator_p = (rtDW->UnitDelay_DSTATE != rtb_Abs5_h);
+
+    /* If: '<S30>/If' */
+    if (rtb_LogicalOperator_p) {
+      /* Outputs for IfAction SubSystem: '<S30>/Subsystem' incorporates:
+       *  ActionPort: '<S32>/Action Port'
+       */
+      /* Sum: '<S32>/Add' incorporates:
+       *  Signum: '<S32>/Sign'
+       *  Switch: '<S22>/Switch3'
+       *  UnitDelay: '<S5>/UnitDelay2'
+       */
+      rtb_Divide1_fi = (int16_T)((rtb_Abs5_h - rtDW->UnitDelay2_DSTATE_p) >> 2);
+
+      /* Signum: '<S32>/Sign' */
+      if (rtb_Divide1_fi < 0) {
+        rtb_Divide1_fi = -1;
+      } else {
+        rtb_Divide1_fi = (int16_T)(rtb_Divide1_fi > 0);
+      }
+
+      /* End of Signum: '<S32>/Sign' */
+
+      /* Product: '<S32>/Divide' incorporates:
+       *  Constant: '<S28>/Constant5'
+       */
+      rtDW->Divide = (int16_T)(rtb_Divide1_fi * 6);
+
+      /* Switch: '<S32>/Switch' incorporates:
+       *  Switch: '<S32>/Switch1'
+       */
+      if (rtb_Divide1_fi > 0) {
+        /* Switch: '<S32>/Switch' incorporates:
+         *  Switch: '<S22>/Switch3'
+         */
+        rtDW->Switch = rtb_Abs5_h;
+
+        /* Switch: '<S32>/Switch1' incorporates:
+         *  UnitDelay: '<S5>/UnitDelay2'
+         */
+        rtDW->Switch1 = rtDW->UnitDelay2_DSTATE_p;
+      } else {
+        /* Switch: '<S32>/Switch' incorporates:
+         *  UnitDelay: '<S5>/UnitDelay2'
+         */
+        rtDW->Switch = rtDW->UnitDelay2_DSTATE_p;
+
+        /* Switch: '<S32>/Switch1' incorporates:
+         *  Switch: '<S22>/Switch3'
+         */
+        rtDW->Switch1 = rtb_Abs5_h;
+      }
+
+      /* End of Switch: '<S32>/Switch' */
+      /* End of Outputs for SubSystem: '<S30>/Subsystem' */
+
+      /* Switch: '<S33>/Switch1' incorporates:
+       *  UnitDelay: '<S5>/UnitDelay2'
+       */
+      rtb_Switch_dr = rtDW->UnitDelay2_DSTATE_p;
+    } else {
+      /* Switch: '<S33>/Switch1' incorporates:
+       *  UnitDelay: '<S33>/UnitDelay'
+       */
+      rtb_Switch_dr = rtDW->UnitDelay_DSTATE_d;
+    }
+
+    /* End of If: '<S30>/If' */
+
+    /* Sum: '<S30>/Add2' incorporates:
+     *  Product: '<S32>/Divide'
+     */
+    sigIdx = ((rtb_Switch_dr << 1) + rtDW->Divide) >> 1;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    /* Switch: '<S28>/Switch' incorporates:
+     *  Switch: '<S22>/Switch'
+     */
+    if (rtb_Switch_oi > 0) {
+      /* Switch: '<S31>/Switch2' incorporates:
+       *  RelationalOperator: '<S31>/LowerRelop1'
+       *  RelationalOperator: '<S31>/UpperRelop'
+       *  Sum: '<S30>/Add2'
+       *  Switch: '<S31>/Switch'
+       *  Switch: '<S32>/Switch'
+       *  Switch: '<S32>/Switch1'
+       */
+      if ((int16_T)sigIdx > rtDW->Switch) {
+        /* Merge: '<S23>/Merge' incorporates:
+         *  Switch: '<S28>/Switch'
+         */
+        rtDW->Merge[1] = rtDW->Switch;
+      } else if ((int16_T)sigIdx < rtDW->Switch1) {
+        /* Merge: '<S23>/Merge' incorporates:
+         *  Switch: '<S28>/Switch'
+         *  Switch: '<S31>/Switch'
+         *  Switch: '<S32>/Switch1'
+         */
+        rtDW->Merge[1] = rtDW->Switch1;
+      } else {
+        /* Merge: '<S23>/Merge' incorporates:
+         *  Switch: '<S28>/Switch'
+         */
+        rtDW->Merge[1] = (int16_T)sigIdx;
+      }
+
+      /* End of Switch: '<S31>/Switch2' */
+    } else {
+      /* Merge: '<S23>/Merge' incorporates:
+       *  Constant: '<S28>/Constant1'
+       */
+      rtDW->Merge[1] = 0;
+    }
+
+    /* End of Switch: '<S28>/Switch' */
+
+    /* Merge: '<S23>/Merge' incorporates:
+     *  Constant: '<S28>/Constant3'
+     *  SignalConversion generated from: '<S28>/open_voltage'
+     */
+    rtDW->Merge[0] = 0;
+
+    /* Update for UnitDelay: '<S28>/Unit Delay' incorporates:
+     *  Switch: '<S22>/Switch3'
+     */
+    rtDW->UnitDelay_DSTATE = rtb_Abs5_h;
+
+    /* Switch: '<S33>/Switch2' */
+    if (rtb_LogicalOperator_p) {
+      /* Update for UnitDelay: '<S33>/UnitDelay' incorporates:
+       *  UnitDelay: '<S5>/UnitDelay2'
+       */
+      rtDW->UnitDelay_DSTATE_d = rtDW->UnitDelay2_DSTATE_p;
+    } else {
+      /* Update for UnitDelay: '<S33>/UnitDelay' incorporates:
+       *  Sum: '<S30>/Add2'
+       */
+      rtDW->UnitDelay_DSTATE_d = (int16_T)sigIdx;
+    }
+
+    /* End of Switch: '<S33>/Switch2' */
+    /* End of Outputs for SubSystem: '<S23>/open_mode' */
+  } else {
+    /* Outputs for IfAction SubSystem: '<S23>/torque_mode' incorporates:
+     *  ActionPort: '<S29>/Action Port'
+     */
+    /* Product: '<S29>/Divide1' incorporates:
+     *  Inport: '<Root>/i_dc_limit'
+     *  Inport: '<Root>/speed_limit'
+     *  Product: '<S29>/Divide4'
+     *  Switch: '<S22>/Switch'
+     */
+    sigIdx = ((uint16_T)((rtU->i_dc_limit << 8) / rtU->speed_limit) *
+              rtb_Switch_oi) >> 8;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    /* Product: '<S29>/Divide1' */
+    rtDW->Divide1 = (int16_T)sigIdx;
+
+    /* End of Outputs for SubSystem: '<S23>/torque_mode' */
+  }
+
+  /* End of If: '<S23>/If' */
+
+  /* Outputs for Atomic SubSystem: '<S34>/either_edge' */
+  rtb_LogicalOperator_p = either_edge(rtb_RelationalOperator4_f,
+    &rtDW->either_edge_f);
+
+  /* End of Outputs for SubSystem: '<S34>/either_edge' */
+
+  /* Switch: '<S34>/Switch1' */
+  if (rtb_LogicalOperator_p) {
+    rtb_UnitDelay = rtb_DataTypeConversion1_c;
+  }
+
+  /* End of Switch: '<S34>/Switch1' */
+
+  /* Gain: '<S51>/Multiply' incorporates:
+   *  DataTypeConversion: '<S54>/Data Type Conversion'
+   *  Inport: '<Root>/adc_a'
+   *  Inport: '<Root>/adc_b'
+   */
+  sigIdx = (12351 * rtU->adc_a) >> 11;
+  if (sigIdx > 32767) {
+    sigIdx = 32767;
+  } else {
+    if (sigIdx < -32768) {
+      sigIdx = -32768;
+    }
+  }
+
+  rtb_DataTypeConversion[0] = (int16_T)sigIdx;
+  tmp_1 = (12351 * rtU->adc_b) >> 11;
+  if (tmp_1 > 32767) {
+    tmp_1 = 32767;
+  } else {
+    if (tmp_1 < -32768) {
+      tmp_1 = -32768;
+    }
+  }
+
+  rtb_DataTypeConversion[1] = (int16_T)tmp_1;
+
+  /* Sum: '<S45>/Add' incorporates:
+   *  Gain: '<S51>/Multiply'
+   */
+  tmp = (int16_T)sigIdx + (int16_T)tmp_1;
+  if (tmp > 32767) {
+    tmp = 32767;
+  } else {
+    if (tmp < -32768) {
+      tmp = -32768;
+    }
+  }
+
+  /* Sum: '<S45>/Add1' incorporates:
+   *  Sum: '<S45>/Add'
+   */
+  tmp_0 = -tmp;
+  if (-tmp > 32767) {
+    tmp_0 = 32767;
+  }
+
+  /* Sum: '<S53>/Add3' incorporates:
+   *  Gain: '<S51>/Multiply'
+   *  Sum: '<S45>/Add1'
+   */
+  tmp = (int16_T)tmp_1 + (int16_T)tmp_0;
+  if (tmp > 32767) {
+    tmp = 32767;
+  } else {
+    if (tmp < -32768) {
+      tmp = -32768;
+    }
+  }
+
+  /* Sum: '<S53>/Add' incorporates:
+   *  Gain: '<S51>/Multiply'
+   *  Sum: '<S53>/Add3'
+   */
+  sigIdx = (((int16_T)sigIdx << 1) - tmp) >> 1;
+  if (sigIdx > 32767) {
+    sigIdx = 32767;
+  } else {
+    if (sigIdx < -32768) {
+      sigIdx = -32768;
+    }
+  }
+
+  /* Gain: '<S53>/Gain1' incorporates:
+   *  Product: '<S55>/Divide1'
+   *  Sum: '<S53>/Add'
+   */
+  rtb_Divide1_fi = (int16_T)((21845 * sigIdx) >> 15);
+
+  /* Gain: '<S53>/Gain2' incorporates:
+   *  Gain: '<S51>/Multiply'
+   *  Sum: '<S45>/Add1'
+   *  Sum: '<S53>/Add2'
+   */
+  sigIdx = ((((int16_T)tmp_1 - (int16_T)tmp_0) >> 1) * 18919) >> 14;
+  if (sigIdx > 32767) {
+    sigIdx = 32767;
+  } else {
+    if (sigIdx < -32768) {
+      sigIdx = -32768;
+    }
+  }
+
+  /* PreLookup: '<S56>/a_elecAngle_XA' incorporates:
+   *  Switch: '<S2>/Switch'
+   */
+  rtb_LogicalOperator3 = plook_u16s16_evencka(rtb_Switch_b, 0, 4U, 1440U);
+
+  /* Interpolation_n-D: '<S56>/r_cos_M1' */
+  rtb_Switch_dr = rtConstP.r_cos_M1_Table[rtb_LogicalOperator3];
+
+  /* Interpolation_n-D: '<S56>/r_sin_M1' incorporates:
+   *  Product: '<S67>/Divide4'
+   */
+  rtb_Abs5_h = rtConstP.r_sin_M1_Table[rtb_LogicalOperator3];
+
+  /* Sum: '<S55>/Sum1' incorporates:
+   *  Gain: '<S53>/Gain2'
+   *  Interpolation_n-D: '<S56>/r_cos_M1'
+   *  Interpolation_n-D: '<S56>/r_sin_M1'
+   *  Product: '<S55>/Divide1'
+   *  Product: '<S55>/Divide2'
+   *  Product: '<S55>/Divide3'
+   */
+  tmp_1 = (int16_T)((rtb_Divide1_fi *
+                     rtConstP.r_cos_M1_Table[rtb_LogicalOperator3]) >> 14) +
+    (int16_T)(((int16_T)sigIdx * rtConstP.r_sin_M1_Table[rtb_LogicalOperator3]) >>
+              14);
+  if (tmp_1 > 32767) {
+    tmp_1 = 32767;
+  } else {
+    if (tmp_1 < -32768) {
+      tmp_1 = -32768;
+    }
+  }
+
+  /* SignalConversion generated from: '<S45>/Low_Pass_Filter' incorporates:
+   *  Sum: '<S55>/Sum1'
+   */
+  rtb_TmpSignalConversionAtLow_Pa[0] = (int16_T)tmp_1;
+
+  /* Sum: '<S55>/Sum6' incorporates:
+   *  Gain: '<S53>/Gain2'
+   *  Interpolation_n-D: '<S56>/r_cos_M1'
+   *  Interpolation_n-D: '<S56>/r_sin_M1'
+   *  Product: '<S55>/Divide1'
+   *  Product: '<S55>/Divide4'
+   */
+  sigIdx = (int16_T)(((int16_T)sigIdx *
+                      rtConstP.r_cos_M1_Table[rtb_LogicalOperator3]) >> 14) -
+    (int16_T)((rtb_Divide1_fi * rtConstP.r_sin_M1_Table[rtb_LogicalOperator3]) >>
+              14);
+  if (sigIdx > 32767) {
+    sigIdx = 32767;
+  } else {
+    if (sigIdx < -32768) {
+      sigIdx = -32768;
+    }
+  }
+
+  /* SignalConversion generated from: '<S45>/Low_Pass_Filter' incorporates:
+   *  Sum: '<S55>/Sum6'
+   */
+  rtb_TmpSignalConversionAtLow_Pa[1] = (int16_T)sigIdx;
+
+  /* Outputs for Atomic SubSystem: '<S45>/Low_Pass_Filter' */
+  /* Constant: '<S45>/Constant' */
+  Low_Pass_Filter(rtb_TmpSignalConversionAtLow_Pa, 26214, rtb_DataTypeConversion,
+                  &rtDW->Low_Pass_Filter_d);
+
+  /* End of Outputs for SubSystem: '<S45>/Low_Pass_Filter' */
+
+  /* Outport: '<Root>/VdPrev' incorporates:
+   *  UnitDelay: '<S5>/UnitDelay1'
+   */
+  rtY->VdPrev = rtDW->UnitDelay1_DSTATE;
+
+  /* Interpolation_n-D: '<S46>/Vq_max_M1' incorporates:
+   *  Abs: '<S46>/Abs5'
+   *  PreLookup: '<S46>/Vq_max_XA'
+   *  UnitDelay: '<S5>/UnitDelay1'
+   */
+  if (rtDW->UnitDelay1_DSTATE < 0) {
+    rtb_Divide1_fi = (int16_T)-rtDW->UnitDelay1_DSTATE;
+  } else {
+    rtb_Divide1_fi = rtDW->UnitDelay1_DSTATE;
+  }
+
+  rtb_Divide1_fi = rtConstP.Vq_max_M1_Table[plook_u16s16_evencka(rtb_Divide1_fi,
+    0, 64U, 45U)];
+
+  /* End of Interpolation_n-D: '<S46>/Vq_max_M1' */
+
+  /* Product: '<S46>/Divide4' incorporates:
+   *  Inport: '<Root>/i_dc_limit'
+   *  Product: '<S24>/Divide3'
+   */
+  sigIdx = rtDW->Divide3 << 16;
+  sigIdx = (sigIdx == MIN_int32_T) && (rtU->i_dc_limit == -1) ? MAX_int32_T :
+    sigIdx / rtU->i_dc_limit;
+  if (sigIdx < 0) {
+    sigIdx = 0;
+  } else {
+    if (sigIdx > 65535) {
+      sigIdx = 65535;
+    }
+  }
+
+  /* PreLookup: '<S46>/iq_maxSca_XA' incorporates:
+   *  Product: '<S46>/Divide4'
+   */
+  rtb_DataTypeConversion1_c = plook_u8u16_evencka((uint16_T)sigIdx, 0U, 1311U,
+    49U);
+
+  /* Outport: '<Root>/PWM' incorporates:
+   *  Interpolation_n-D: '<S46>/iq_maxSca_M1'
+   */
+  rtY->PWM[6] = rtConstP.iq_maxSca_M1_Table[rtb_DataTypeConversion1_c];
+
+  /* Product: '<S46>/Divide1' incorporates:
+   *  Inport: '<Root>/i_dc_limit'
+   *  Interpolation_n-D: '<S46>/iq_maxSca_M1'
+   */
+  rtb_Divide4_c = (int16_T)
+    ((rtConstP.iq_maxSca_M1_Table[rtb_DataTypeConversion1_c] * rtU->i_dc_limit) >>
+     16);
+
+  /* Switch: '<S52>/Switch2' */
+  rtb_Switch2_fu = (uint8_T)(rtb_z_ctrlMod != 0);
+
+  /* Delay: '<S80>/Delay' */
+  rtb_RelationalOperator4_f = rtDW->Delay_DSTATE_n[0];
+
+  /* DataTypeConversion: '<S52>/Data Type Conversion1' incorporates:
+   *  Delay: '<S80>/Delay'
+   *  Logic: '<S52>/Logical Operator'
+   *  Logic: '<S80>/Logical Operator'
+   *  UnitDelay: '<S80>/Unit Delay'
+   */
+  rtb_DataTypeConversion1_c = (uint8_T)((rtb_Switch2_fu != 0) && ((boolean_T)
+    (rtDW->UnitDelay_DSTATE_f ^ rtDW->Delay_DSTATE_n[0])));
+
+  /* If: '<S50>/If' incorporates:
+   *  Constant: '<S73>/Constant1'
+   *  Constant: '<S73>/Constant11'
+   *  Constant: '<S73>/Constant2'
+   *  Constant: '<S73>/Constant4'
+   *  Gain: '<S46>/Gain1'
+   *  Product: '<S46>/Divide1'
+   *  Sum: '<S73>/Add2'
+   *  Switch: '<S10>/Switch2'
+   *  Switch: '<S78>/Switch2'
+   */
+  if ((rtb_DataTypeConversion1_c > 0) && (rtb_z_ctrlMod == 1)) {
+    /* Outputs for IfAction SubSystem: '<S50>/speed_mode' incorporates:
+     *  ActionPort: '<S73>/Action Port'
+     */
+    /* Switch: '<S75>/Switch2' incorporates:
+     *  Inport: '<Root>/speed_limit'
+     *  RelationalOperator: '<S75>/LowerRelop1'
+     *  RelationalOperator: '<S75>/UpperRelop'
+     *  Switch: '<S22>/Switch'
+     *  Switch: '<S75>/Switch'
+     *  Switch: '<S78>/Switch2'
+     */
+    if (rtb_Switch_oi > rtU->speed_limit) {
+      rtb_Switch_oi = rtU->speed_limit;
+    } else {
+      if (rtb_Switch_oi < 0) {
+        /* Switch: '<S75>/Switch' incorporates:
+         *  Constant: '<S73>/Constant5'
+         *  Switch: '<S78>/Switch2'
+         */
+        rtb_Switch_oi = 0;
+      }
+    }
+
+    /* End of Switch: '<S75>/Switch2' */
+
+    /* Outputs for Atomic SubSystem: '<S73>/pi_speed' */
+    rtb_Switch_oi = pi_speed((int16_T)(rtb_Switch_oi - rtb_Switch2_ip), 3174, 10,
+      20, rtb_Divide4_c, (int16_T)-rtb_Divide4_c, 0, rtb_Switch2_fu,
+      &rtConstB.pi_speed_g, &rtDW->pi_speed_g, &rtPrevZCX->pi_speed_g);
+
+    /* End of Outputs for SubSystem: '<S73>/pi_speed' */
+
+    /* Merge: '<S50>/Merge' incorporates:
+     *  Constant: '<S73>/Constant1'
+     *  Constant: '<S73>/Constant11'
+     *  Constant: '<S73>/Constant2'
+     *  Constant: '<S73>/Constant4'
+     *  Gain: '<S46>/Gain1'
+     *  Product: '<S46>/Divide1'
+     *  SignalConversion generated from: '<S73>/iq_target'
+     *  Sum: '<S73>/Add2'
+     *  Switch: '<S10>/Switch2'
+     *  Switch: '<S78>/Switch2'
+     */
+    rtDW->Merge_b = rtb_Switch_oi;
+
+    /* End of Outputs for SubSystem: '<S50>/speed_mode' */
+  } else {
+    if ((rtb_DataTypeConversion1_c > 0) && (rtb_z_ctrlMod == 2)) {
+      /* Outputs for IfAction SubSystem: '<S50>/torque_mode' incorporates:
+       *  ActionPort: '<S74>/Action Port'
+       */
+      /* Product: '<S74>/Divide' incorporates:
+       *  Constant: '<S74>/Constant2'
+       *  Sum: '<S74>/Sum2'
+       *  Switch: '<S10>/Switch2'
+       *  Switch: '<S22>/Switch'
+       */
+      sigIdx = ((int16_T)(rtb_Switch_oi - rtb_Switch2_ip) * 819) >> 6;
+      if (sigIdx > 32767) {
+        sigIdx = 32767;
+      } else {
+        if (sigIdx < -32768) {
+          sigIdx = -32768;
+        }
+      }
+
+      /* Product: '<S74>/Divide1' incorporates:
+       *  Sum: '<S74>/Sum3'
+       *  Switch: '<S10>/Switch2'
+       *  Switch: '<S22>/Switch'
+       */
+      tmp_1 = ((int16_T)(rtb_Switch2_ip - rtb_Switch_oi) * -51) >> 5;
+      if (tmp_1 > 32767) {
+        tmp_1 = 32767;
+      } else {
+        if (tmp_1 < -32768) {
+          tmp_1 = -32768;
+        }
+      }
+
+      rtb_Switch_oi = (int16_T)tmp_1;
+
+      /* End of Product: '<S74>/Divide1' */
+
+      /* MinMax: '<S74>/Max' incorporates:
+       *  Product: '<S74>/Divide'
+       *  Product: '<S74>/Divide1'
+       */
+      if ((int16_T)sigIdx > rtb_Switch_oi) {
+        rtb_Max = (int16_T)sigIdx;
+      } else {
+        rtb_Max = rtb_Switch_oi;
+      }
+
+      /* End of MinMax: '<S74>/Max' */
+
+      /* MinMax: '<S74>/Max3' incorporates:
+       *  MinMax: '<S74>/Max'
+       *  Product: '<S46>/Divide1'
+       *  Switch: '<S79>/Switch2'
+       */
+      if (rtb_Divide4_c < rtb_Max) {
+        rtb_Max = rtb_Divide4_c;
+      }
+
+      /* End of MinMax: '<S74>/Max3' */
+
+      /* Switch: '<S79>/Switch2' incorporates:
+       *  Product: '<S29>/Divide1'
+       *  RelationalOperator: '<S79>/LowerRelop1'
+       */
+      if (rtDW->Divide1 <= rtb_Max) {
+        /* MinMax: '<S74>/Max1' incorporates:
+         *  Product: '<S74>/Divide'
+         *  Product: '<S74>/Divide1'
+         */
+        if ((int16_T)sigIdx < rtb_Switch_oi) {
+          rtb_Switch_oi = (int16_T)sigIdx;
+        }
+
+        /* End of MinMax: '<S74>/Max1' */
+
+        /* MinMax: '<S74>/Max2' incorporates:
+         *  Gain: '<S46>/Gain1'
+         *  MinMax: '<S74>/Max1'
+         *  Product: '<S46>/Divide1'
+         */
+        if (rtb_Switch_oi <= (int16_T)-rtb_Divide4_c) {
+          rtb_Switch_oi = (int16_T)-rtb_Divide4_c;
+        }
+
+        /* End of MinMax: '<S74>/Max2' */
+
+        /* Switch: '<S79>/Switch' incorporates:
+         *  MinMax: '<S74>/Max2'
+         *  RelationalOperator: '<S79>/UpperRelop'
+         */
+        if (rtDW->Divide1 < rtb_Switch_oi) {
+          rtb_Max = rtb_Switch_oi;
+        } else {
+          rtb_Max = rtDW->Divide1;
+        }
+
+        /* End of Switch: '<S79>/Switch' */
+      }
+
+      /* End of Switch: '<S79>/Switch2' */
+
+      /* Merge: '<S50>/Merge' incorporates:
+       *  SignalConversion generated from: '<S74>/torque_iq'
+       *  Switch: '<S79>/Switch2'
+       */
+      rtDW->Merge_b = rtb_Max;
+
+      /* End of Outputs for SubSystem: '<S50>/torque_mode' */
+    }
+  }
+
+  /* End of If: '<S50>/If' */
+
+  /* If: '<S47>/If' incorporates:
+   *  Constant: '<S47>/Constant3'
+   *  Constant: '<S57>/Constant3'
+   *  Constant: '<S57>/Constant4'
+   *  Constant: '<S57>/Constant6'
+   *  Constant: '<S57>/Constant9'
+   *  Constant: '<S58>/Constant1'
+   *  Constant: '<S58>/Constant7'
+   *  Constant: '<S58>/Constant8'
+   *  Gain: '<S46>/Gain3'
+   *  Gain: '<S46>/Gain5'
+   *  If: '<S47>/If1'
+   *  Inport: '<Root>/vbus_voltage'
+   *  Interpolation_n-D: '<S46>/Vq_max_M1'
+   *  Sum: '<S57>/Add'
+   *  Sum: '<S58>/Add1'
+   *  Switch: '<S60>/Switch2'
+   *  Switch: '<S64>/Switch2'
+   */
+  if (rtb_Switch2_fu == 1) {
+    /* Outputs for IfAction SubSystem: '<S47>/iq_ctrl' incorporates:
+     *  ActionPort: '<S58>/Action Port'
+     */
+    /* Switch: '<S64>/Switch2' incorporates:
+     *  Merge: '<S50>/Merge'
+     *  Product: '<S46>/Divide1'
+     *  RelationalOperator: '<S64>/LowerRelop1'
+     */
+    if (rtDW->Merge_b <= rtb_Divide4_c) {
+      /* Switch: '<S64>/Switch' incorporates:
+       *  Gain: '<S46>/Gain1'
+       *  RelationalOperator: '<S64>/UpperRelop'
+       *  Switch: '<S64>/Switch2'
+       */
+      if (rtDW->Merge_b < (int16_T)-rtb_Divide4_c) {
+        rtb_Divide4_c = (int16_T)-rtb_Divide4_c;
+      } else {
+        rtb_Divide4_c = rtDW->Merge_b;
+      }
+
+      /* End of Switch: '<S64>/Switch' */
+    }
+
+    /* End of Switch: '<S64>/Switch2' */
+
+    /* Outputs for Atomic SubSystem: '<S58>/PI_iq' */
+    PI_iq((int16_T)(rtb_Divide4_c - rtb_DataTypeConversion[1]), 4096, 51, 1024,
+          rtb_Divide1_fi, (int16_T)-rtb_Divide1_fi, 0, &rtDW->Switch2_m,
+          &rtDW->PI_iq_g);
+
+    /* End of Outputs for SubSystem: '<S58>/PI_iq' */
+    /* End of Outputs for SubSystem: '<S47>/iq_ctrl' */
+
+    /* Outputs for IfAction SubSystem: '<S47>/id_ctrl' incorporates:
+     *  ActionPort: '<S57>/Action Port'
+     */
+    /* Switch: '<S60>/Switch2' incorporates:
+     *  Constant: '<S47>/Constant3'
+     *  Constant: '<S58>/Constant1'
+     *  Constant: '<S58>/Constant7'
+     *  Constant: '<S58>/Constant8'
+     *  Gain: '<S46>/Gain4'
+     *  Gain: '<S46>/Gain5'
+     *  Inport: '<Root>/i_dc_limit'
+     *  Interpolation_n-D: '<S46>/Vq_max_M1'
+     *  Product: '<S24>/Divide3'
+     *  RelationalOperator: '<S60>/LowerRelop1'
+     *  RelationalOperator: '<S60>/UpperRelop'
+     *  Sum: '<S58>/Add1'
+     *  Switch: '<S60>/Switch'
+     *  Switch: '<S64>/Switch2'
+     */
+    if (rtDW->Divide3 > rtU->i_dc_limit) {
+      rtb_Switch_oi = rtU->i_dc_limit;
+    } else if (rtDW->Divide3 < (int16_T)-rtU->i_dc_limit) {
+      /* Switch: '<S60>/Switch' incorporates:
+       *  Gain: '<S46>/Gain4'
+       *  Switch: '<S60>/Switch2'
+       */
+      rtb_Switch_oi = (int16_T)-rtU->i_dc_limit;
+    } else {
+      rtb_Switch_oi = rtDW->Divide3;
+    }
+
+    /* End of Switch: '<S60>/Switch2' */
+
+    /* Outputs for Atomic SubSystem: '<S57>/PI_id' */
+    PI_id((int16_T)(rtb_Switch_oi - rtb_DataTypeConversion[0]), 4096, 51, 1024,
+          rtU->vbus_voltage, (int16_T)-rtU->vbus_voltage, 0, &rtDW->Switch2,
+          &rtDW->PI_id_b);
+
+    /* End of Outputs for SubSystem: '<S57>/PI_id' */
+    /* End of Outputs for SubSystem: '<S47>/id_ctrl' */
+  }
+
+  /* End of If: '<S47>/If' */
+
+  /* Switch: '<S5>/Switch1' incorporates:
+   *  Switch: '<S5>/Switch'
+   *  Switch: '<S62>/Switch2'
+   *  Switch: '<S66>/Switch2'
+   */
+  if (rtb_z_ctrlMod != 0) {
+    rtb_Switch_oi = rtDW->Switch2_m;
+    rtb_Divide1_fi = rtDW->Switch2;
+  } else {
+    rtb_Switch_oi = rtDW->Merge[1];
+    rtb_Divide1_fi = rtDW->Merge[0];
+  }
+
+  /* End of Switch: '<S5>/Switch1' */
+
+  /* Sum: '<S48>/Sum1' incorporates:
+   *  Interpolation_n-D: '<S56>/r_cos_M1'
+   *  Product: '<S48>/Divide2'
+   *  Product: '<S48>/Divide3'
+   *  Product: '<S67>/Divide4'
+   *  Switch: '<S5>/Switch'
+   *  Switch: '<S5>/Switch1'
+   */
+  sigIdx = (int16_T)((rtb_Divide1_fi * rtb_Abs5_h) >> 14) + (int16_T)
+    ((rtb_Switch_oi * rtb_Switch_dr) >> 14);
+  if (sigIdx > 32767) {
+    sigIdx = 32767;
+  } else {
+    if (sigIdx < -32768) {
+      sigIdx = -32768;
+    }
+  }
+
+  /* Sum: '<S48>/Sum6' incorporates:
+   *  Interpolation_n-D: '<S56>/r_cos_M1'
+   *  Product: '<S48>/Divide1'
+   *  Product: '<S48>/Divide4'
+   *  Product: '<S67>/Divide4'
+   *  Switch: '<S5>/Switch'
+   *  Switch: '<S5>/Switch1'
+   */
+  tmp_1 = (int16_T)((rtb_Divide1_fi * rtb_Switch_dr) >> 14) - (int16_T)
+    ((rtb_Switch_oi * rtb_Abs5_h) >> 14);
+  if (tmp_1 > 32767) {
+    tmp_1 = 32767;
+  } else {
+    if (tmp_1 < -32768) {
+      tmp_1 = -32768;
+    }
+  }
+
+  /* Product: '<S67>/Divide3' incorporates:
+   *  Constant: '<S67>/Constant1'
+   *  Product: '<S67>/Divide'
+   *  Sum: '<S48>/Sum6'
+   */
+  rtb_Switch_dr = (int16_T)((3547 * (int16_T)tmp_1) >> 12);
+
+  /* Product: '<S67>/Divide2' incorporates:
+   *  Constant: '<S67>/Constant'
+   *  Sum: '<S48>/Sum1'
+   */
+  rtb_Max = (int16_T)((3547 * (int16_T)sigIdx) >> 12);
+
+  /* Product: '<S67>/Divide4' incorporates:
+   *  Constant: '<S67>/Constant2'
+   *  Product: '<S67>/Divide2'
+   */
+  rtb_Abs5_h = (int16_T)((2365 * rtb_Max) >> 12);
+
+  /* Sum: '<S67>/Add' incorporates:
+   *  Product: '<S67>/Divide'
+   *  Product: '<S67>/Divide4'
+   */
+  rtb_Gain1 = (int16_T)((rtb_Switch_dr + rtb_Abs5_h) >> 1);
+
+  /* Sum: '<S67>/Add1' incorporates:
+   *  Product: '<S67>/Divide'
+   *  Product: '<S67>/Divide4'
+   */
+  rtb_Gain4 = (int16_T)((rtb_Abs5_h - rtb_Switch_dr) >> 1);
+
+  /* Product: '<S67>/Divide7' incorporates:
+   *  Constant: '<S67>/Constant3'
+   *  Sum: '<S48>/Sum1'
+   */
+  rtb_Abs5_h = (int16_T)((2365 * (int16_T)sigIdx) >> 12);
+
+  /* MATLAB Function: '<S67>/sector_select' incorporates:
+   *  Product: '<S67>/Divide7'
+   *  Sum: '<S48>/Sum1'
+   *  Sum: '<S48>/Sum6'
+   */
+  if ((int16_T)sigIdx >= 0) {
+    if ((int16_T)tmp_1 >= 0) {
+      if (rtb_Abs5_h > (int16_T)tmp_1) {
+        /* DataTypeConversion: '<S67>/Data Type Conversion' */
+        rtb_DataTypeConversion1_c = 2U;
+      } else {
+        /* DataTypeConversion: '<S67>/Data Type Conversion' */
+        rtb_DataTypeConversion1_c = 1U;
+      }
+    } else if (-rtb_Abs5_h > (int16_T)tmp_1) {
+      /* DataTypeConversion: '<S67>/Data Type Conversion' */
+      rtb_DataTypeConversion1_c = 3U;
+    } else {
+      /* DataTypeConversion: '<S67>/Data Type Conversion' */
+      rtb_DataTypeConversion1_c = 2U;
+    }
+  } else if ((int16_T)tmp_1 >= 0) {
+    if (-rtb_Abs5_h > (int16_T)tmp_1) {
+      /* DataTypeConversion: '<S67>/Data Type Conversion' */
+      rtb_DataTypeConversion1_c = 5U;
+    } else {
+      /* DataTypeConversion: '<S67>/Data Type Conversion' */
+      rtb_DataTypeConversion1_c = 6U;
+    }
+  } else if (rtb_Abs5_h > (int16_T)tmp_1) {
+    /* DataTypeConversion: '<S67>/Data Type Conversion' */
+    rtb_DataTypeConversion1_c = 4U;
+  } else {
+    /* DataTypeConversion: '<S67>/Data Type Conversion' */
+    rtb_DataTypeConversion1_c = 5U;
+  }
+
+  /* End of MATLAB Function: '<S67>/sector_select' */
+
+  /* Product: '<S67>/Divide' incorporates:
+   *  Inport: '<Root>/vbus_voltage'
+   */
+  rtb_Switch_dr = (int16_T)div_nde_s32_floor(40960000, rtU->vbus_voltage);
+
+  /* Product: '<S67>/Divide1' incorporates:
+   *  Product: '<S67>/Divide'
+   *  Product: '<S67>/Divide2'
+   *  Product: '<S67>/Divide8'
+   */
+  rtb_Abs5_h = (int16_T)((((2365 * rtb_Max) >> 13) * rtb_Switch_dr) >> 10);
+
+  /* Product: '<S67>/Divide5' incorporates:
+   *  Product: '<S67>/Divide'
+   *  Sum: '<S67>/Add'
+   */
+  rtb_Divide4_c = (int16_T)((rtb_Gain1 * rtb_Switch_dr) >> 11);
+
+  /* Product: '<S67>/Divide6' incorporates:
+   *  Product: '<S67>/Divide'
+   *  Sum: '<S67>/Add1'
+   */
+  rtb_Gain1 = (int16_T)((rtb_Gain4 * rtb_Switch_dr) >> 11);
+
+  /* MATLAB Function: '<S67>/phase_time' incorporates:
+   *  DataTypeConversion: '<S67>/Data Type Conversion1'
+   */
+  switch ((int8_T)rtb_DataTypeConversion1_c) {
+   case 1:
+    sigIdx = -rtb_Gain1;
+    if (-rtb_Gain1 > 32767) {
+      sigIdx = 32767;
+    }
+
+    tmp_1 = 10000 - (int16_T)sigIdx;
+    if (10000 - (int16_T)sigIdx > 32767) {
+      tmp_1 = 32767;
+    }
+
+    tmp_1 -= rtb_Abs5_h;
+    if (tmp_1 > 32767) {
+      tmp_1 = 32767;
+    } else {
+      if (tmp_1 < -32768) {
+        tmp_1 = -32768;
+      }
+    }
+
+    rtb_Switch_dr = (int16_T)rt_roundd_snf((real_T)tmp_1 / 4.0);
+    rtb_Abs5_h = (int16_T)((int32_T)rt_roundd_snf((real_T)rtb_Abs5_h / 2.0) +
+      rtb_Switch_dr);
+    sigIdx = (int32_T)rt_roundd_snf((real_T)(int16_T)sigIdx / 2.0) + rtb_Abs5_h;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Divide4_c = (int16_T)sigIdx;
+    break;
+
+   case 2:
+    sigIdx = 10000 - rtb_Divide4_c;
+    if (10000 - rtb_Divide4_c > 32767) {
+      sigIdx = 32767;
+    }
+
+    sigIdx -= rtb_Gain1;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Switch_dr = (int16_T)rt_roundd_snf((real_T)sigIdx / 4.0);
+    rtb_Divide4_c = (int16_T)((int32_T)rt_roundd_snf((real_T)rtb_Divide4_c / 2.0)
+      + rtb_Switch_dr);
+    sigIdx = (int32_T)rt_roundd_snf((real_T)rtb_Gain1 / 2.0) + rtb_Divide4_c;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Abs5_h = (int16_T)sigIdx;
+    break;
+
+   case 3:
+    sigIdx = -rtb_Divide4_c;
+    if (-rtb_Divide4_c > 32767) {
+      sigIdx = 32767;
+    }
+
+    tmp_1 = 10000 - rtb_Abs5_h;
+    if (10000 - rtb_Abs5_h > 32767) {
+      tmp_1 = 32767;
+    }
+
+    tmp_1 -= (int16_T)sigIdx;
+    if (tmp_1 > 32767) {
+      tmp_1 = 32767;
+    } else {
+      if (tmp_1 < -32768) {
+        tmp_1 = -32768;
+      }
+    }
+
+    rtb_Divide4_c = (int16_T)rt_roundd_snf((real_T)tmp_1 / 4.0);
+    rtb_Switch_dr = (int16_T)((int32_T)rt_roundd_snf((real_T)(int16_T)sigIdx /
+      2.0) + rtb_Divide4_c);
+    sigIdx = (int32_T)rt_roundd_snf((real_T)rtb_Abs5_h / 2.0) + rtb_Switch_dr;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Abs5_h = (int16_T)sigIdx;
+    break;
+
+   case 4:
+    sigIdx = -rtb_Abs5_h;
+    if (-rtb_Abs5_h > 32767) {
+      sigIdx = 32767;
+    }
+
+    tmp_1 = 10000 - (int16_T)sigIdx;
+    if (10000 - (int16_T)sigIdx > 32767) {
+      tmp_1 = 32767;
+    }
+
+    tmp_1 -= rtb_Gain1;
+    if (tmp_1 > 32767) {
+      tmp_1 = 32767;
+    } else {
+      if (tmp_1 < -32768) {
+        tmp_1 = -32768;
+      }
+    }
+
+    rtb_Divide4_c = (int16_T)rt_roundd_snf((real_T)tmp_1 / 4.0);
+    rtb_Abs5_h = (int16_T)((int32_T)rt_roundd_snf((real_T)rtb_Gain1 / 2.0) +
+      rtb_Divide4_c);
+    sigIdx = (int32_T)rt_roundd_snf((real_T)(int16_T)sigIdx / 2.0) + rtb_Abs5_h;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Switch_dr = (int16_T)sigIdx;
+    break;
+
+   case 5:
+    sigIdx = -rtb_Divide4_c;
+    if (-rtb_Divide4_c > 32767) {
+      sigIdx = 32767;
+    }
+
+    tmp_1 = -rtb_Gain1;
+    if (-rtb_Gain1 > 32767) {
+      tmp_1 = 32767;
+    }
+
+    tmp = 10000 - (int16_T)sigIdx;
+    if (10000 - (int16_T)sigIdx > 32767) {
+      tmp = 32767;
+    }
+
+    tmp -= (int16_T)tmp_1;
+    if (tmp > 32767) {
+      tmp = 32767;
+    } else {
+      if (tmp < -32768) {
+        tmp = -32768;
+      }
+    }
+
+    rtb_Abs5_h = (int16_T)rt_roundd_snf((real_T)tmp / 4.0);
+    rtb_Divide4_c = (int16_T)((int32_T)rt_roundd_snf((real_T)(int16_T)tmp_1 /
+      2.0) + rtb_Abs5_h);
+    sigIdx = (int32_T)rt_roundd_snf((real_T)(int16_T)sigIdx / 2.0) +
+      rtb_Divide4_c;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Switch_dr = (int16_T)sigIdx;
+    break;
+
+   default:
+    sigIdx = -rtb_Abs5_h;
+    if (-rtb_Abs5_h > 32767) {
+      sigIdx = 32767;
+    }
+
+    tmp_1 = 10000 - rtb_Divide4_c;
+    if (10000 - rtb_Divide4_c > 32767) {
+      tmp_1 = 32767;
+    }
+
+    tmp_1 -= (int16_T)sigIdx;
+    if (tmp_1 > 32767) {
+      tmp_1 = 32767;
+    } else {
+      if (tmp_1 < -32768) {
+        tmp_1 = -32768;
+      }
+    }
+
+    rtb_Abs5_h = (int16_T)rt_roundd_snf((real_T)tmp_1 / 4.0);
+    rtb_Switch_dr = (int16_T)((int32_T)rt_roundd_snf((real_T)(int16_T)sigIdx /
+      2.0) + rtb_Abs5_h);
+    sigIdx = (int32_T)rt_roundd_snf((real_T)rtb_Divide4_c / 2.0) + rtb_Switch_dr;
+    if (sigIdx > 32767) {
+      sigIdx = 32767;
+    } else {
+      if (sigIdx < -32768) {
+        sigIdx = -32768;
+      }
+    }
+
+    rtb_Divide4_c = (int16_T)sigIdx;
+    break;
+  }
+
+  /* Switch: '<S69>/Switch2' incorporates:
+   *  Constant: '<S67>/Constant6'
+   *  MATLAB Function: '<S67>/phase_time'
+   *  RelationalOperator: '<S69>/LowerRelop1'
+   *  RelationalOperator: '<S69>/UpperRelop'
+   *  Switch: '<S69>/Switch'
+   */
+  if (rtb_Divide4_c > 5000) {
+    rtb_LogicalOperator3 = 5000U;
+  } else if (rtb_Divide4_c < 0) {
+    /* Switch: '<S69>/Switch' incorporates:
+     *  Constant: '<S67>/Constant5'
+     */
+    rtb_LogicalOperator3 = 0U;
+  } else {
+    rtb_LogicalOperator3 = (uint16_T)rtb_Divide4_c;
+  }
+
+  if (rtb_Abs5_h > 5000) {
+    rtb_Switch2_idx_1 = 5000U;
+  } else if (rtb_Abs5_h < 0) {
+    /* Switch: '<S69>/Switch' incorporates:
+     *  Constant: '<S67>/Constant5'
+     */
+    rtb_Switch2_idx_1 = 0U;
+  } else {
+    rtb_Switch2_idx_1 = (uint16_T)rtb_Abs5_h;
+  }
+
+  if (rtb_Switch_dr > 5000) {
+    rtb_Switch2_idx_2 = 5000U;
+  } else if (rtb_Switch_dr < 0) {
+    /* Switch: '<S69>/Switch' incorporates:
+     *  Constant: '<S67>/Constant5'
+     */
+    rtb_Switch2_idx_2 = 0U;
+  } else {
+    rtb_Switch2_idx_2 = (uint16_T)rtb_Switch_dr;
+  }
+
+  /* End of Switch: '<S69>/Switch2' */
+
+  /* MultiPortSwitch: '<S68>/Multiport Switch' */
+  switch (rtb_DataTypeConversion1_c) {
+   case 1:
+    rtb_MultiportSwitch_idx_0 = rtb_LogicalOperator3;
+    rtb_MultiportSwitch_idx_1 = rtb_Switch2_idx_1;
+    break;
+
+   case 2:
+    rtb_MultiportSwitch_idx_0 = rtb_Switch2_idx_1;
+    rtb_MultiportSwitch_idx_1 = rtb_LogicalOperator3;
+    break;
+
+   case 3:
+    rtb_MultiportSwitch_idx_0 = rtb_Switch2_idx_1;
+    rtb_MultiportSwitch_idx_1 = rtb_Switch2_idx_2;
+    break;
+
+   case 4:
+    rtb_MultiportSwitch_idx_0 = rtb_Switch2_idx_2;
+    rtb_MultiportSwitch_idx_1 = rtb_Switch2_idx_1;
+    break;
+
+   case 5:
+    rtb_MultiportSwitch_idx_0 = rtb_Switch2_idx_2;
+    rtb_MultiportSwitch_idx_1 = rtb_LogicalOperator3;
+    break;
+
+   default:
+    rtb_MultiportSwitch_idx_0 = rtb_LogicalOperator3;
+    rtb_MultiportSwitch_idx_1 = rtb_Switch2_idx_2;
+    break;
+  }
+
+  /* End of MultiPortSwitch: '<S68>/Multiport Switch' */
+
+  /* Outport: '<Root>/PWM' incorporates:
+   *  Constant: '<S68>/Constant'
+   *  Constant: '<S68>/Constant1'
+   *  Constant: '<S68>/Constant2'
+   *  Constant: '<S68>/Constant3'
+   *  MATLAB Function: '<S68>/Shunt_Three_Sample_Point'
+   *  Outport: '<Root>/sector'
+   */
+  Shunt_Three_Sample_Point(rtb_MultiportSwitch_idx_0, rtb_MultiportSwitch_idx_1,
+    rtb_DataTypeConversion1_c, 5000, 10, 10, 10, &rtY->PWM[6], &rtY->PWM[7],
+    &rtY->sector);
+
+  /* Update for Delay: '<S7>/Delay' incorporates:
+   *  Inport: '<Root>/hall_a'
+   */
+  rtDW->Delay_DSTATE = rtU->hall_a;
+
+  /* Update for Delay: '<S7>/Delay1' incorporates:
+   *  Inport: '<Root>/hall_b'
+   */
+  rtDW->Delay1_DSTATE = rtU->hall_b;
+
+  /* Update for Delay: '<S7>/Delay2' incorporates:
+   *  Inport: '<Root>/hall_c'
+   */
+  rtDW->Delay2_DSTATE = rtU->hall_c;
+
+  /* Update for UnitDelay: '<S10>/UnitDelay3' incorporates:
+   *  Inport: '<Root>/hw_count'
+   */
+  rtDW->UnitDelay3_DSTATE = rtU->hw_count;
+
+  /* Update for UnitDelay: '<S10>/UnitDelay4' incorporates:
+   *  Abs: '<S10>/Abs5'
+   */
+  rtDW->UnitDelay4_DSTATE = rtb_Abs5;
+
+  /* Update for UnitDelay: '<S34>/UnitDelay' */
+  rtDW->UnitDelay_DSTATE_j = rtb_UnitDelay;
+
+  /* Update for UnitDelay: '<S5>/UnitDelay2' incorporates:
+   *  Switch: '<S5>/Switch1'
+   */
+  rtDW->UnitDelay2_DSTATE_p = rtb_Switch_oi;
+
+  /* Update for UnitDelay: '<S5>/UnitDelay1' incorporates:
+   *  Switch: '<S5>/Switch'
+   */
+  rtDW->UnitDelay1_DSTATE = rtb_Divide1_fi;
+
+  /* Update for UnitDelay: '<S80>/Unit Delay' incorporates:
+   *  Delay: '<S80>/Delay'
+   */
+  rtDW->UnitDelay_DSTATE_f = rtDW->Delay_DSTATE_n[0];
+
+  /* Update for Delay: '<S80>/Delay' incorporates:
+   *  Logic: '<S80>/Logical Operator1'
+   */
+  for (sigIdx = 0; sigIdx < 19; sigIdx++) {
+    rtDW->Delay_DSTATE_n[sigIdx] = rtDW->Delay_DSTATE_n[sigIdx + 1];
+  }
+
+  rtDW->Delay_DSTATE_n[19] = !rtb_RelationalOperator4_f;
+
+  /* End of Update for Delay: '<S80>/Delay' */
+  /* End of Outputs for SubSystem: '<Root>/PMSM_Controller' */
+
+  /* Outport: '<Root>/PWM' */
+  rtY->PWM[0] = rtb_LogicalOperator3;
+  rtY->PWM[3] = rtb_LogicalOperator3;
+  rtY->PWM[1] = rtb_Switch2_idx_1;
+  rtY->PWM[4] = rtb_Switch2_idx_1;
+  rtY->PWM[2] = rtb_Switch2_idx_2;
+  rtY->PWM[5] = rtb_Switch2_idx_2;
+
+  /* Outport: '<Root>/n_MotError' */
+  rtY->n_MotError = rtb_UnitDelay;
+
+  /* Outport: '<Root>/iq' */
+  rtY->iq = rtb_DataTypeConversion[1];
+
+  /* Outport: '<Root>/id' */
+  rtY->id = rtb_DataTypeConversion[0];
+
+  /* Outport: '<Root>/angle' incorporates:
+   *  Switch: '<S2>/Switch'
+   */
+  rtY->angle = rtb_Switch_b;
+
+  /* Outport: '<Root>/rpm' incorporates:
+   *  Switch: '<S10>/Switch2'
+   */
+  rtY->rpm = rtb_Switch2_ip;
+
+  /* Outport: '<Root>/hall_angle' incorporates:
+   *  Merge: '<S11>/Merge'
+   */
+  rtY->hall_angle = rtb_Switch3_c;
+
+  /* Outport: '<Root>/hall_state' */
+  rtY->hall_state = rtb_Add_k;
+}
+
+/* Model initialize function */
+void PMSM_Controller_initialize(RT_MODEL *const rtM)
+{
+  DW *rtDW = rtM->dwork;
+  PrevZCX *rtPrevZCX = rtM->prevZCSigState;
+
+  {
+    int32_T i;
+    rtPrevZCX->pi_speed_g.ResettableDelay_Reset_ZCE = POS_ZCSIG;
+
+    /* SystemInitialize for Atomic SubSystem: '<Root>/PMSM_Controller' */
+    /* InitializeConditions for Delay: '<S80>/Delay' */
+    for (i = 0; i < 20; i++) {
+      rtDW->Delay_DSTATE_n[i] = true;
+    }
+
+    /* End of InitializeConditions for Delay: '<S80>/Delay' */
+
+    /* SystemInitialize for IfAction SubSystem: '<S10>/Raw_Motor_Speed_Estimation' */
+    /* SystemInitialize for Outport: '<S15>/z_counter' incorporates:
+     *  Inport: '<S15>/z_counterRawPrev'
+     */
+    rtDW->z_counterRawPrev = 200000U;
+
+    /* End of SystemInitialize for SubSystem: '<S10>/Raw_Motor_Speed_Estimation' */
+
+    /* SystemInitialize for Atomic SubSystem: '<S34>/Debounce_Filter' */
+    Debounce_Filter_Init(&rtDW->Debounce_Filter_i);
+
+    /* End of SystemInitialize for SubSystem: '<S34>/Debounce_Filter' */
+
+    /* SystemInitialize for IfAction SubSystem: '<S50>/speed_mode' */
+    /* SystemInitialize for Atomic SubSystem: '<S73>/pi_speed' */
+    pi_speed_Init(&rtDW->pi_speed_g);
+
+    /* End of SystemInitialize for SubSystem: '<S73>/pi_speed' */
+    /* End of SystemInitialize for SubSystem: '<S50>/speed_mode' */
+    /* End of SystemInitialize for SubSystem: '<Root>/PMSM_Controller' */
+  }
+}
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.h

@@ -0,0 +1,424 @@
+/*
+ * File: PMSM_Controller.h
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#ifndef RTW_HEADER_PMSM_Controller_h_
+#define RTW_HEADER_PMSM_Controller_h_
+#include <math.h>
+#ifndef PMSM_Controller_COMMON_INCLUDES_
+#define PMSM_Controller_COMMON_INCLUDES_
+#include "rtwtypes.h"
+#include "zero_crossing_types.h"
+#endif                                 /* PMSM_Controller_COMMON_INCLUDES_ */
+
+#include "PMSM_Controller_types.h"
+
+/* Child system includes */
+#include "Shunt_Three_Sample_Point.h"
+
+/* Macros for accessing real-time model data structure */
+#ifndef rtmGetErrorStatus
+#define rtmGetErrorStatus(rtm)         ((rtm)->errorStatus)
+#endif
+
+#ifndef rtmSetErrorStatus
+#define rtmSetErrorStatus(rtm, val)    ((rtm)->errorStatus = (val))
+#endif
+
+/* Block signals and states (default storage) for system '<S39>/Counter' */
+typedef struct {
+  uint16_T UnitDelay_DSTATE;           /* '<S44>/UnitDelay' */
+} DW_Counter;
+
+/* Block signals and states (default storage) for system '<S35>/either_edge' */
+typedef struct {
+  boolean_T UnitDelay_DSTATE;          /* '<S40>/UnitDelay' */
+} DW_either_edge;
+
+/* Block signals and states (default storage) for system '<S34>/Debounce_Filter' */
+typedef struct {
+  DW_either_edge either_edge_j;        /* '<S35>/either_edge' */
+  DW_Counter Counter_d;                /* '<S38>/Counter' */
+  DW_Counter Counter_f;                /* '<S39>/Counter' */
+  boolean_T UnitDelay_DSTATE;          /* '<S35>/UnitDelay' */
+} DW_Debounce_Filter;
+
+/* Block signals and states (default storage) for system '<S45>/Low_Pass_Filter' */
+typedef struct {
+  int16_T UnitDelay1_DSTATE[2];        /* '<S54>/UnitDelay1' */
+} DW_Low_Pass_Filter;
+
+/* Block signals and states (default storage) for system '<S58>/PI_iq' */
+typedef struct {
+  int32_T UnitDelay_DSTATE;            /* '<S63>/UnitDelay' */
+  int32_T UnitDelay_DSTATE_i;          /* '<S65>/UnitDelay' */
+} DW_PI_iq;
+
+/* Block signals and states (default storage) for system '<S57>/PI_id' */
+typedef struct {
+  int32_T UnitDelay_DSTATE;            /* '<S61>/UnitDelay' */
+  int32_T UnitDelay_DSTATE_j;          /* '<S59>/UnitDelay' */
+} DW_PI_id;
+
+/* Block signals and states (default storage) for system '<S73>/pi_speed' */
+typedef struct {
+  int32_T UnitDelay_DSTATE;            /* '<S76>/UnitDelay' */
+  int32_T ResettableDelay_DSTATE;      /* '<S77>/Resettable Delay' */
+  uint8_T icLoad;                      /* '<S77>/Resettable Delay' */
+} DW_pi_speed;
+
+/* Zero-crossing (trigger) state for system '<S73>/pi_speed' */
+typedef struct {
+  ZCSigState ResettableDelay_Reset_ZCE;/* '<S77>/Resettable Delay' */
+} ZCE_pi_speed;
+
+/* Block signals and states (default storage) for system '<Root>' */
+typedef struct {
+  DW_pi_speed pi_speed_g;              /* '<S73>/pi_speed' */
+  DW_PI_id PI_id_b;                    /* '<S57>/PI_id' */
+  DW_PI_iq PI_iq_g;                    /* '<S58>/PI_iq' */
+  DW_Low_Pass_Filter Low_Pass_Filter_d;/* '<S45>/Low_Pass_Filter' */
+  DW_either_edge either_edge_f;        /* '<S34>/either_edge' */
+  DW_Debounce_Filter Debounce_Filter_i;/* '<S34>/Debounce_Filter' */
+  uint32_T z_counterRawPrev;           /* '<S15>/z_counterRawPrev' */
+  uint32_T UnitDelay3_DSTATE;          /* '<S10>/UnitDelay3' */
+  uint32_T UnitDelay2_DSTATE;          /* '<S15>/UnitDelay2' */
+  uint32_T UnitDelay3_DSTATE_l;        /* '<S15>/UnitDelay3' */
+  uint32_T UnitDelay5_DSTATE;          /* '<S15>/UnitDelay5' */
+  int16_T Merge[2];                    /* '<S23>/Merge' */
+  int16_T Divide;                      /* '<S32>/Divide' */
+  int16_T Merge_b;                     /* '<S50>/Merge' */
+  int16_T Switch2;                     /* '<S62>/Switch2' */
+  int16_T Switch2_m;                   /* '<S66>/Switch2' */
+  int16_T Divide1;                     /* '<S29>/Divide1' */
+  int16_T Switch;                      /* '<S32>/Switch' */
+  int16_T Switch1;                     /* '<S32>/Switch1' */
+  int16_T Divide3;                     /* '<S24>/Divide3' */
+  int16_T Divide11;                    /* '<S15>/Divide11' */
+  int16_T UnitDelay2_DSTATE_p;         /* '<S5>/UnitDelay2' */
+  int16_T UnitDelay1_DSTATE;           /* '<S5>/UnitDelay1' */
+  int16_T UnitDelay_DSTATE;            /* '<S28>/Unit Delay' */
+  int16_T UnitDelay_DSTATE_d;          /* '<S33>/UnitDelay' */
+  int16_T UnitDelay4_DSTATE;           /* '<S10>/UnitDelay4' */
+  uint16_T UnitDelay2_DSTATE_f;        /* '<S16>/UnitDelay2' */
+  uint16_T UnitDelay3_DSTATE_lh;       /* '<S16>/UnitDelay3' */
+  uint16_T UnitDelay5_DSTATE_f;        /* '<S16>/UnitDelay5' */
+  int8_T Switch2_i;                    /* '<S6>/Switch2' */
+  int8_T UnitDelay2_DSTATE_j;          /* '<S6>/UnitDelay2' */
+  uint8_T Delay_DSTATE;                /* '<S7>/Delay' */
+  uint8_T Delay1_DSTATE;               /* '<S7>/Delay1' */
+  uint8_T Delay2_DSTATE;               /* '<S7>/Delay2' */
+  uint8_T UnitDelay_DSTATE_j;          /* '<S34>/UnitDelay' */
+  uint8_T is_active_c5_PMSM_Controller;/* '<S3>/Control_Mode_Manager' */
+  uint8_T is_c5_PMSM_Controller;       /* '<S3>/Control_Mode_Manager' */
+  uint8_T is_ACTIVE;                   /* '<S3>/Control_Mode_Manager' */
+  boolean_T Delay_DSTATE_n[20];        /* '<S80>/Delay' */
+  boolean_T Compare;                   /* '<S18>/Compare' */
+  boolean_T UnitDelay_DSTATE_f;        /* '<S80>/Unit Delay' */
+  boolean_T UnitDelay1_DSTATE_i;       /* '<S15>/UnitDelay1' */
+  boolean_T n_SpeedCtrl_Mode;          /* '<S21>/n_SpeedCtrl' */
+  boolean_T n_commDeacv_Mode;          /* '<S16>/n_commDeacv' */
+} DW;
+
+/* Zero-crossing (trigger) state */
+typedef struct {
+  ZCE_pi_speed pi_speed_g;             /* '<S73>/pi_speed' */
+} PrevZCX;
+
+/* Invariant block signals for system '<S73>/pi_speed' */
+typedef struct {
+  const int32_T DataTypeConversion2;   /* '<S77>/Data Type Conversion2' */
+} ConstB_pi_speed;
+
+/* Invariant block signals (default storage) */
+typedef struct {
+  ConstB_pi_speed pi_speed_g;          /* '<S73>/pi_speed' */
+} ConstB;
+
+/* Constant parameters (default storage) */
+typedef struct {
+  /* Computed Parameter: r_cos_M1_Table
+   * Referenced by: '<S56>/r_cos_M1'
+   */
+  int16_T r_cos_M1_Table[1441];
+
+  /* Computed Parameter: r_sin_M1_Table
+   * Referenced by: '<S56>/r_sin_M1'
+   */
+  int16_T r_sin_M1_Table[1441];
+
+  /* Computed Parameter: Vq_max_M1_Table
+   * Referenced by: '<S46>/Vq_max_M1'
+   */
+  int16_T Vq_max_M1_Table[46];
+
+  /* Computed Parameter: iq_maxSca_M1_Table
+   * Referenced by: '<S46>/iq_maxSca_M1'
+   */
+  uint16_T iq_maxSca_M1_Table[50];
+
+  /* Computed Parameter: vec_hallToPos_Value
+   * Referenced by: '<S9>/vec_hallToPos'
+   */
+  int8_T vec_hallToPos_Value[8];
+} ConstP;
+
+/* External inputs (root inport signals with default storage) */
+typedef struct {
+  int16_T adc_a;                       /* '<Root>/adc_a' */
+  int16_T adc_b;                       /* '<Root>/adc_b' */
+  int16_T open_theta;                  /* '<Root>/open_theta' */
+  int16_T rotor_speed;                 /* '<Root>/rotor_speed' */
+  int16_T input_target;                /* '<Root>/input_target' */
+  uint8_T hall_a;                      /* '<Root>/hall_a' */
+  uint8_T hall_b;                      /* '<Root>/hall_b' */
+  uint8_T hall_c;                      /* '<Root>/hall_c' */
+  uint32_T hw_count;                   /* '<Root>/hw_count' */
+  boolean_T b_motEna;                  /* '<Root>/b_motEna' */
+  boolean_T b_cruiseEna;               /* '<Root>/b_cruiseEna' */
+  uint8_T n_ctrlModReq;                /* '<Root>/n_ctrlModReq' */
+  int16_T i_dc_limit;                  /* '<Root>/i_dc_limit' */
+  int16_T speed_limit;                 /* '<Root>/speed_limit' */
+  int16_T vbus_voltage;                /* '<Root>/vbus_voltage' */
+  boolean_T b_hall_calibrate;          /* '<Root>/b_hall_calibrate' */
+  int16_T vd_open_target;              /* '<Root>/vd_open_target' */
+  int16_T vq_open_target;              /* '<Root>/vq_open_target' */
+} ExtU;
+
+/* External outputs (root outports fed by signals with default storage) */
+typedef struct {
+  uint16_T PWM[8];                     /* '<Root>/PWM' */
+  uint8_T sector;                      /* '<Root>/sector' */
+  uint8_T n_MotError;                  /* '<Root>/n_MotError' */
+  int16_T VqPrev;                      /* '<Root>/VqPrev' */
+  int16_T VdPrev;                      /* '<Root>/VdPrev' */
+  int16_T iq;                          /* '<Root>/iq' */
+  int16_T id;                          /* '<Root>/id' */
+  int16_T angle;                       /* '<Root>/angle' */
+  int16_T rpm;                         /* '<Root>/rpm' */
+  int16_T hall_angle;                  /* '<Root>/hall_angle' */
+  uint8_T hall_state;                  /* '<Root>/hall_state' */
+} ExtY;
+
+/* Real-time Model Data Structure */
+struct tag_RTM {
+  const char_T * volatile errorStatus;
+  PrevZCX *prevZCSigState;
+  ExtU *inputs;
+  ExtY *outputs;
+  DW *dwork;
+};
+
+extern const ConstB rtConstB;          /* constant block i/o */
+
+/* Constant parameters (default storage) */
+extern const ConstP rtConstP;
+
+/* Model entry point functions */
+extern void PMSM_Controller_initialize(RT_MODEL *const rtM);
+extern void PMSM_Controller_step(RT_MODEL *const rtM);
+
+/*-
+ * These blocks were eliminated from the model due to optimizations:
+ *
+ * Block '<S6>/Scope' : Unused code path elimination
+ * Block '<S8>/Scope' : Unused code path elimination
+ * Block '<S9>/Scope' : Unused code path elimination
+ * Block '<S2>/Scope' : Unused code path elimination
+ * Block '<S10>/Logical Operator4' : Unused code path elimination
+ * Block '<S15>/Abs2' : Unused code path elimination
+ * Block '<S15>/Add' : Unused code path elimination
+ * Block '<S17>/Compare' : Unused code path elimination
+ * Block '<S17>/Constant' : Unused code path elimination
+ * Block '<S15>/Sum7' : Unused code path elimination
+ * Block '<S15>/UnitDelay4' : Unused code path elimination
+ * Block '<S15>/UnitDelay6' : Unused code path elimination
+ * Block '<S15>/UnitDelay7' : Unused code path elimination
+ * Block '<S15>/UnitDelay8' : Unused code path elimination
+ * Block '<S15>/dz_cntTrnsDet' : Unused code path elimination
+ * Block '<S10>/Scope1' : Unused code path elimination
+ * Block '<S10>/Scope2' : Unused code path elimination
+ * Block '<S25>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S25>/Data Type Propagation' : Unused code path elimination
+ * Block '<S26>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S26>/Data Type Propagation' : Unused code path elimination
+ * Block '<S3>/Scope' : Unused code path elimination
+ * Block '<S23>/Scope' : Unused code path elimination
+ * Block '<S31>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S31>/Data Type Propagation' : Unused code path elimination
+ * Block '<S30>/Scope' : Unused code path elimination
+ * Block '<S30>/Scope1' : Unused code path elimination
+ * Block '<S28>/Scope' : Unused code path elimination
+ * Block '<S29>/Scope' : Unused code path elimination
+ * Block '<S1>/Scope1' : Unused code path elimination
+ * Block '<S1>/Scope2' : Unused code path elimination
+ * Block '<S53>/Scope' : Unused code path elimination
+ * Block '<S45>/Scope' : Unused code path elimination
+ * Block '<S45>/Scope1' : Unused code path elimination
+ * Block '<S5>/Gain6' : Unused code path elimination
+ * Block '<S46>/Scope' : Unused code path elimination
+ * Block '<S47>/Constant1' : Unused code path elimination
+ * Block '<S47>/Constant2' : Unused code path elimination
+ * Block '<S47>/Constant4' : Unused code path elimination
+ * Block '<S47>/Scope1' : Unused code path elimination
+ * Block '<S62>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S62>/Data Type Propagation' : Unused code path elimination
+ * Block '<S60>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S60>/Data Type Propagation' : Unused code path elimination
+ * Block '<S57>/Scope' : Unused code path elimination
+ * Block '<S66>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S66>/Data Type Propagation' : Unused code path elimination
+ * Block '<S63>/Scope' : Unused code path elimination
+ * Block '<S64>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S64>/Data Type Propagation' : Unused code path elimination
+ * Block '<S58>/Scope' : Unused code path elimination
+ * Block '<S69>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S69>/Data Type Propagation' : Unused code path elimination
+ * Block '<S67>/Scope' : Unused code path elimination
+ * Block '<S68>/Scope' : Unused code path elimination
+ * Block '<S5>/Scope1' : Unused code path elimination
+ * Block '<S5>/Scope12' : Unused code path elimination
+ * Block '<S75>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S75>/Data Type Propagation' : Unused code path elimination
+ * Block '<S73>/Scope' : Unused code path elimination
+ * Block '<S73>/Scope2' : Unused code path elimination
+ * Block '<S78>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S78>/Data Type Propagation' : Unused code path elimination
+ * Block '<S76>/Scope' : Unused code path elimination
+ * Block '<S79>/Data Type Duplicate' : Unused code path elimination
+ * Block '<S79>/Data Type Propagation' : Unused code path elimination
+ * Block '<S74>/Scope' : Unused code path elimination
+ * Block '<S52>/Scope' : Unused code path elimination
+ * Block '<S52>/Scope1' : Unused code path elimination
+ * Block '<S8>/Manual Switch' : Eliminated due to constant selection input
+ * Block '<S2>/hall_motor_speed_choice' : Eliminated due to constant selection input
+ * Block '<S23>/Data Type Conversion' : Eliminate redundant data type conversion
+ * Block '<S5>/Data Type Conversion' : Eliminate redundant data type conversion
+ * Block '<S46>/Data Type Conversion' : Eliminate redundant data type conversion
+ * Block '<S73>/Manual Switch' : Eliminated due to constant selection input
+ * Block '<S80>/Data Type Conversion1' : Eliminate redundant data type conversion
+ * Block '<S8>/Constant' : Unused code path elimination
+ * Block '<S15>/Divide14' : Unused code path elimination
+ * Block '<S15>/smooth' : Unused code path elimination
+ * Block '<S73>/Constant' : Unused code path elimination
+ */
+
+/*-
+ * The generated code includes comments that allow you to trace directly
+ * back to the appropriate location in the model.  The basic format
+ * is <system>/block_name, where system is the system number (uniquely
+ * assigned by Simulink) and block_name is the name of the block.
+ *
+ * Note that this particular code originates from a subsystem build,
+ * and has its own system numbers different from the parent model.
+ * Refer to the system hierarchy for this subsystem below, and use the
+ * MATLAB hilite_system command to trace the generated code back
+ * to the parent model.  For example,
+ *
+ * hilite_system('MotorController_FOC/PMSM_Controller')    - opens subsystem MotorController_FOC/PMSM_Controller
+ * hilite_system('MotorController_FOC/PMSM_Controller/Kp') - opens and selects block Kp
+ *
+ * Here is the system hierarchy for this model
+ *
+ * '<Root>' : 'MotorController_FOC'
+ * '<S1>'   : 'MotorController_FOC/PMSM_Controller'
+ * '<S2>'   : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation'
+ * '<S3>'   : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager'
+ * '<S4>'   : 'MotorController_FOC/PMSM_Controller/Diagnostics'
+ * '<S5>'   : 'MotorController_FOC/PMSM_Controller/controller'
+ * '<S6>'   : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Direction_Detection'
+ * '<S7>'   : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Edge_Detect'
+ * '<S8>'   : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Electrical_Angle_Estimation'
+ * '<S9>'   : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Hall_Angle_Raw'
+ * '<S10>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Speed_Estimation'
+ * '<S11>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Electrical_Angle_Estimation/degree_rad'
+ * '<S12>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Electrical_Angle_Estimation/degree_rad/If Action Subsystem'
+ * '<S13>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Electrical_Angle_Estimation/degree_rad/If Action Subsystem1'
+ * '<S14>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Electrical_Angle_Estimation/degree_rad/If Action Subsystem2'
+ * '<S15>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Speed_Estimation/Raw_Motor_Speed_Estimation'
+ * '<S16>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Speed_Estimation/Subsystem'
+ * '<S17>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Speed_Estimation/Raw_Motor_Speed_Estimation/Compare To Constant'
+ * '<S18>'  : 'MotorController_FOC/PMSM_Controller/Angle_Speed_Estimation/Speed_Estimation/Subsystem/Compare To Constant'
+ * '<S19>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Control_Mode_Manager'
+ * '<S20>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Field_Weakening_Enabled'
+ * '<S21>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Mode_Transition_Calculation'
+ * '<S22>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Subsystem'
+ * '<S23>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale'
+ * '<S24>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Field_Weakening_Enabled/Field_Weakening'
+ * '<S25>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Field_Weakening_Enabled/Field_Weakening/Saturation Dynamic'
+ * '<S26>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/Field_Weakening_Enabled/Field_Weakening/Saturation Dynamic1'
+ * '<S27>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/If Action Subsystem'
+ * '<S28>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/open_mode'
+ * '<S29>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/torque_mode'
+ * '<S30>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/open_mode/Rate_Control'
+ * '<S31>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/open_mode/Rate_Control/Saturation Dynamic'
+ * '<S32>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/open_mode/Rate_Control/Subsystem'
+ * '<S33>'  : 'MotorController_FOC/PMSM_Controller/Control_Mode_Manager/target_scale/open_mode/Rate_Control/delayUnit'
+ * '<S34>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled'
+ * '<S35>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter'
+ * '<S36>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/either_edge'
+ * '<S37>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Default'
+ * '<S38>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Dequalification'
+ * '<S39>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Qualification'
+ * '<S40>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/either_edge'
+ * '<S41>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Dequalification/Counter'
+ * '<S42>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Dequalification/Counter/rst_Delay'
+ * '<S43>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Qualification/Counter'
+ * '<S44>'  : 'MotorController_FOC/PMSM_Controller/Diagnostics/Diagnostics_Enabled/Debounce_Filter/Qualification/Counter/rst_Delay'
+ * '<S45>'  : 'MotorController_FOC/PMSM_Controller/controller/Clarke_Park_Transform_Forward'
+ * '<S46>'  : 'MotorController_FOC/PMSM_Controller/controller/Motor_Limitations_Enabled'
+ * '<S47>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller'
+ * '<S48>'  : 'MotorController_FOC/PMSM_Controller/controller/Park_Transform_Inverse'
+ * '<S49>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM'
+ * '<S50>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller'
+ * '<S51>'  : 'MotorController_FOC/PMSM_Controller/controller/current_sample'
+ * '<S52>'  : 'MotorController_FOC/PMSM_Controller/controller/pid_schdule'
+ * '<S53>'  : 'MotorController_FOC/PMSM_Controller/controller/Clarke_Park_Transform_Forward/Clarke'
+ * '<S54>'  : 'MotorController_FOC/PMSM_Controller/controller/Clarke_Park_Transform_Forward/Low_Pass_Filter'
+ * '<S55>'  : 'MotorController_FOC/PMSM_Controller/controller/Clarke_Park_Transform_Forward/Park'
+ * '<S56>'  : 'MotorController_FOC/PMSM_Controller/controller/Clarke_Park_Transform_Forward/Sine_Cosine_Approximation'
+ * '<S57>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/id_ctrl'
+ * '<S58>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/iq_ctrl'
+ * '<S59>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/id_ctrl/PI_id'
+ * '<S60>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/id_ctrl/Saturation Dynamic1'
+ * '<S61>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/id_ctrl/PI_id/Integrator'
+ * '<S62>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/id_ctrl/PI_id/Saturation Dynamic1'
+ * '<S63>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/iq_ctrl/PI_iq'
+ * '<S64>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/iq_ctrl/Saturation Dynamic2'
+ * '<S65>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/iq_ctrl/PI_iq/Integrator'
+ * '<S66>'  : 'MotorController_FOC/PMSM_Controller/controller/PID_Controller/iq_ctrl/PI_iq/Saturation Dynamic1'
+ * '<S67>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM/SVPWM'
+ * '<S68>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM/Sample_Control'
+ * '<S69>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM/SVPWM/Saturation Dynamic1'
+ * '<S70>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM/SVPWM/phase_time'
+ * '<S71>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM/SVPWM/sector_select'
+ * '<S72>'  : 'MotorController_FOC/PMSM_Controller/controller/SVM/Sample_Control/Shunt_Three_Sample_Point'
+ * '<S73>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/speed_mode'
+ * '<S74>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/torque_mode'
+ * '<S75>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/speed_mode/Saturation Dynamic3'
+ * '<S76>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/speed_mode/pi_speed'
+ * '<S77>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/speed_mode/pi_speed/Integrator'
+ * '<S78>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/speed_mode/pi_speed/Saturation Dynamic1'
+ * '<S79>'  : 'MotorController_FOC/PMSM_Controller/controller/Speed_Controller/torque_mode/Saturation Dynamic'
+ * '<S80>'  : 'MotorController_FOC/PMSM_Controller/controller/pid_schdule/Call_Scheduler'
+ */
+#endif                                 /* RTW_HEADER_PMSM_Controller_h_ */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.mk

@@ -0,0 +1,414 @@
+###########################################################################
+## Makefile generated for component 'PMSM_Controller'. 
+## 
+## Makefile     : PMSM_Controller.mk
+## Generated on : Sun Apr 03 14:02:45 2022
+## Final product: $(RELATIVE_PATH_TO_ANCHOR)\PMSM_Controller.exe
+## Product type : executable
+## 
+###########################################################################
+
+###########################################################################
+## MACROS
+###########################################################################
+
+# Macro Descriptions:
+# PRODUCT_NAME            Name of the system to build
+# MAKEFILE                Name of this makefile
+# COMPILER_COMMAND_FILE   Compiler command listing model reference header paths
+# CMD_FILE                Command file
+
+PRODUCT_NAME              = PMSM_Controller
+MAKEFILE                  = PMSM_Controller.mk
+MATLAB_ROOT               = D:\Program Files\R2020b
+MATLAB_BIN                = D:\Program Files\R2020b\bin
+MATLAB_ARCH_BIN           = $(MATLAB_BIN)\win64
+START_DIR                 = E:\works\mcu\MC100\Simulink
+SOLVER                    = 
+SOLVER_OBJ                = 
+CLASSIC_INTERFACE         = 0
+TGT_FCN_LIB               = ISO_C
+MODEL_HAS_DYNAMICALLY_LOADED_SFCNS = 0
+RELATIVE_PATH_TO_ANCHOR   = ..
+COMPILER_COMMAND_FILE     = PMSM_Controller_comp.rsp
+CMD_FILE                  = PMSM_Controller.rsp
+C_STANDARD_OPTS           = 
+CPP_STANDARD_OPTS         = 
+NODEBUG                   = 1
+
+###########################################################################
+## TOOLCHAIN SPECIFICATIONS
+###########################################################################
+
+# Toolchain Name:          Microsoft Visual C++ 2019 v16.0 | nmake (64-bit Windows)
+# Supported Version(s):    16.0
+# ToolchainInfo Version:   2020b
+# Specification Revision:  1.0
+# 
+#-------------------------------------------
+# Macros assumed to be defined elsewhere
+#-------------------------------------------
+
+# NODEBUG
+# cvarsdll
+# cvarsmt
+# conlibsmt
+# ldebug
+# conflags
+# cflags
+
+#-----------
+# MACROS
+#-----------
+
+MW_EXTERNLIB_DIR    = $(MATLAB_ROOT)\extern\lib\win64\microsoft
+MW_LIB_DIR          = $(MATLAB_ROOT)\lib\win64
+CPU                 = AMD64
+APPVER              = 5.02
+CVARSFLAG           = $(cvarsmt)
+CFLAGS_ADDITIONAL   = -D_CRT_SECURE_NO_WARNINGS
+CPPFLAGS_ADDITIONAL = -EHs -D_CRT_SECURE_NO_WARNINGS /wd4251
+LIBS_TOOLCHAIN      = $(conlibs)
+
+TOOLCHAIN_SRCS = 
+TOOLCHAIN_INCS = 
+TOOLCHAIN_LIBS = 
+
+#------------------------
+# BUILD TOOL COMMANDS
+#------------------------
+
+# C Compiler: Microsoft Visual C Compiler
+CC = cl
+
+# Linker: Microsoft Visual C Linker
+LD = link
+
+# C++ Compiler: Microsoft Visual C++ Compiler
+CPP = cl
+
+# C++ Linker: Microsoft Visual C++ Linker
+CPP_LD = link
+
+# Archiver: Microsoft Visual C/C++ Archiver
+AR = lib
+
+# MEX Tool: MEX Tool
+MEX_PATH = $(MATLAB_ARCH_BIN)
+MEX = "$(MEX_PATH)\mex"
+
+# Download: Download
+DOWNLOAD =
+
+# Execute: Execute
+EXECUTE = $(PRODUCT)
+
+# Builder: NMAKE Utility
+MAKE = nmake
+
+
+#-------------------------
+# Directives/Utilities
+#-------------------------
+
+CDEBUG              = -Zi
+C_OUTPUT_FLAG       = -Fo
+LDDEBUG             = /DEBUG
+OUTPUT_FLAG         = -out:
+CPPDEBUG            = -Zi
+CPP_OUTPUT_FLAG     = -Fo
+CPPLDDEBUG          = /DEBUG
+OUTPUT_FLAG         = -out:
+ARDEBUG             =
+STATICLIB_OUTPUT_FLAG = -out:
+MEX_DEBUG           = -g
+RM                  = @del
+ECHO                = @echo
+MV                  = @ren
+RUN                 = @cmd /C
+
+#----------------------------------------
+# "Faster Builds" Build Configuration
+#----------------------------------------
+
+ARFLAGS              = /nologo
+CFLAGS               = $(cflags) $(CVARSFLAG) $(CFLAGS_ADDITIONAL) \
+                       /Od /Oy-
+CPPFLAGS             = /TP $(cflags) $(CVARSFLAG) $(CPPFLAGS_ADDITIONAL) \
+                       /Od /Oy-
+CPP_LDFLAGS          = $(ldebug) $(conflags) $(LIBS_TOOLCHAIN)
+CPP_SHAREDLIB_LDFLAGS  = $(ldebug) $(conflags) $(LIBS_TOOLCHAIN) \
+                         -dll -def:$(DEF_FILE)
+DOWNLOAD_FLAGS       =
+EXECUTE_FLAGS        =
+LDFLAGS              = $(ldebug) $(conflags) $(LIBS_TOOLCHAIN)
+MEX_CPPFLAGS         =
+MEX_CPPLDFLAGS       =
+MEX_CFLAGS           =
+MEX_LDFLAGS          =
+MAKE_FLAGS           = -f $(MAKEFILE)
+SHAREDLIB_LDFLAGS    = $(ldebug) $(conflags) $(LIBS_TOOLCHAIN) \
+                       -dll -def:$(DEF_FILE)
+
+
+
+###########################################################################
+## OUTPUT INFO
+###########################################################################
+
+PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\PMSM_Controller.exe
+PRODUCT_TYPE = "executable"
+BUILD_TYPE = "Top-Level Standalone Executable"
+
+###########################################################################
+## INCLUDE PATHS
+###########################################################################
+
+INCLUDES_BUILDINFO = 
+
+INCLUDES = $(INCLUDES_BUILDINFO)
+
+###########################################################################
+## DEFINES
+###########################################################################
+
+DEFINES_BUILD_ARGS = -DCLASSIC_INTERFACE=0 -DALLOCATIONFCN=0 -DTERMFCN=0 -DONESTEPFCN=1 -DMAT_FILE=0 -DMULTI_INSTANCE_CODE=1 -DINTEGER_CODE=0 -DMT=0
+DEFINES_CUSTOM = 
+DEFINES_OPTS = -DTID01EQ=0
+DEFINES_STANDARD = -DMODEL=PMSM_Controller -DNUMST=1 -DNCSTATES=0 -DHAVESTDIO -DMODEL_HAS_DYNAMICALLY_LOADED_SFCNS=0
+
+DEFINES = $(DEFINES_BUILD_ARGS) $(DEFINES_CUSTOM) $(DEFINES_OPTS) $(DEFINES_STANDARD)
+
+###########################################################################
+## SOURCE FILES
+###########################################################################
+
+SRCS = $(START_DIR)\PMSM_Controller_ert_rtw\PMSM_Controller.c $(START_DIR)\PMSM_Controller_ert_rtw\PMSM_Controller_data.c $(START_DIR)\PMSM_Controller_ert_rtw\Shunt_Three_Sample_Point.c
+
+MAIN_SRC = $(START_DIR)\PMSM_Controller_ert_rtw\ert_main.c
+
+ALL_SRCS = $(SRCS) $(MAIN_SRC)
+
+###########################################################################
+## OBJECTS
+###########################################################################
+
+OBJS = PMSM_Controller.obj PMSM_Controller_data.obj Shunt_Three_Sample_Point.obj
+
+MAIN_OBJ = ert_main.obj
+
+ALL_OBJS = $(OBJS) $(MAIN_OBJ)
+
+###########################################################################
+## PREBUILT OBJECT FILES
+###########################################################################
+
+PREBUILT_OBJS = 
+
+###########################################################################
+## LIBRARIES
+###########################################################################
+
+LIBS = 
+
+###########################################################################
+## SYSTEM LIBRARIES
+###########################################################################
+
+SYSTEM_LIBS = 
+
+###########################################################################
+## ADDITIONAL TOOLCHAIN FLAGS
+###########################################################################
+
+#---------------
+# C Compiler
+#---------------
+
+CFLAGS_BASIC = $(DEFINES) @$(COMPILER_COMMAND_FILE)
+
+CFLAGS = $(CFLAGS) $(CFLAGS_BASIC)
+
+#-----------------
+# C++ Compiler
+#-----------------
+
+CPPFLAGS_BASIC = $(DEFINES) @$(COMPILER_COMMAND_FILE)
+
+CPPFLAGS = $(CPPFLAGS) $(CPPFLAGS_BASIC)
+
+###########################################################################
+## INLINED COMMANDS
+###########################################################################
+
+
+!include $(MATLAB_ROOT)\rtw\c\tools\vcdefs.mak
+
+
+###########################################################################
+## PHONY TARGETS
+###########################################################################
+
+.PHONY : all build buildobj clean info prebuild download execute set_environment_variables
+
+
+all : build
+	@cmd /C "@echo ### Successfully generated all binary outputs."
+
+
+build : set_environment_variables prebuild $(PRODUCT)
+
+
+buildobj : set_environment_variables prebuild $(OBJS) $(PREBUILT_OBJS)
+	@cmd /C "@echo ### Successfully generated all binary outputs."
+
+
+prebuild : 
+
+
+download : $(PRODUCT)
+
+
+execute : download
+	@cmd /C "@echo ### Invoking postbuild tool "Execute" ..."
+	$(EXECUTE) $(EXECUTE_FLAGS)
+	@cmd /C "@echo ### Done invoking postbuild tool."
+
+
+set_environment_variables : 
+	@set INCLUDE=$(INCLUDES);$(INCLUDE)
+	@set LIB=$(LIB)
+
+
+###########################################################################
+## FINAL TARGET
+###########################################################################
+
+#-------------------------------------------
+# Create a standalone executable            
+#-------------------------------------------
+
+$(PRODUCT) : $(OBJS) $(PREBUILT_OBJS) $(MAIN_OBJ)
+	@cmd /C "@echo ### Creating standalone executable "$(PRODUCT)" ..."
+	$(LD) $(LDFLAGS) -out:$(PRODUCT) @$(CMD_FILE) $(SYSTEM_LIBS) $(TOOLCHAIN_LIBS)
+	@cmd /C "@echo ### Created: $(PRODUCT)"
+
+
+###########################################################################
+## INTERMEDIATE TARGETS
+###########################################################################
+
+#---------------------
+# SOURCE-TO-OBJECT
+#---------------------
+
+.c.obj :
+	$(CC) $(CFLAGS) -Fo"$@" "$<"
+
+
+.cpp.obj :
+	$(CPP) $(CPPFLAGS) -Fo"$@" "$<"
+
+
+{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
+	$(CC) $(CFLAGS) -Fo"$@" "$<"
+
+
+{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
+	$(CPP) $(CPPFLAGS) -Fo"$@" "$<"
+
+
+{$(START_DIR)}.c.obj :
+	$(CC) $(CFLAGS) -Fo"$@" "$<"
+
+
+{$(START_DIR)}.cpp.obj :
+	$(CPP) $(CPPFLAGS) -Fo"$@" "$<"
+
+
+{$(START_DIR)\PMSM_Controller_ert_rtw}.c.obj :
+	$(CC) $(CFLAGS) -Fo"$@" "$<"
+
+
+{$(START_DIR)\PMSM_Controller_ert_rtw}.cpp.obj :
+	$(CPP) $(CPPFLAGS) -Fo"$@" "$<"
+
+
+{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
+	$(CC) $(CFLAGS) -Fo"$@" "$<"
+
+
+{$(MATLAB_ROOT)\rtw\c\src}.cpp.obj :
+	$(CPP) $(CPPFLAGS) -Fo"$@" "$<"
+
+
+{$(MATLAB_ROOT)\simulink\src}.c.obj :
+	$(CC) $(CFLAGS) -Fo"$@" "$<"
+
+
+{$(MATLAB_ROOT)\simulink\src}.cpp.obj :
+	$(CPP) $(CPPFLAGS) -Fo"$@" "$<"
+
+
+PMSM_Controller.obj : $(START_DIR)\PMSM_Controller_ert_rtw\PMSM_Controller.c
+	$(CC) $(CFLAGS) -Fo"$@" $(START_DIR)\PMSM_Controller_ert_rtw\PMSM_Controller.c
+
+
+PMSM_Controller_data.obj : $(START_DIR)\PMSM_Controller_ert_rtw\PMSM_Controller_data.c
+	$(CC) $(CFLAGS) -Fo"$@" $(START_DIR)\PMSM_Controller_ert_rtw\PMSM_Controller_data.c
+
+
+Shunt_Three_Sample_Point.obj : $(START_DIR)\PMSM_Controller_ert_rtw\Shunt_Three_Sample_Point.c
+	$(CC) $(CFLAGS) -Fo"$@" $(START_DIR)\PMSM_Controller_ert_rtw\Shunt_Three_Sample_Point.c
+
+
+ert_main.obj : $(START_DIR)\PMSM_Controller_ert_rtw\ert_main.c
+	$(CC) $(CFLAGS) -Fo"$@" $(START_DIR)\PMSM_Controller_ert_rtw\ert_main.c
+
+
+###########################################################################
+## DEPENDENCIES
+###########################################################################
+
+$(ALL_OBJS) : rtw_proj.tmw $(COMPILER_COMMAND_FILE) $(MAKEFILE)
+
+
+###########################################################################
+## MISCELLANEOUS TARGETS
+###########################################################################
+
+info : 
+	@cmd /C "@echo ### PRODUCT = $(PRODUCT)"
+	@cmd /C "@echo ### PRODUCT_TYPE = $(PRODUCT_TYPE)"
+	@cmd /C "@echo ### BUILD_TYPE = $(BUILD_TYPE)"
+	@cmd /C "@echo ### INCLUDES = $(INCLUDES)"
+	@cmd /C "@echo ### DEFINES = $(DEFINES)"
+	@cmd /C "@echo ### ALL_SRCS = $(ALL_SRCS)"
+	@cmd /C "@echo ### ALL_OBJS = $(ALL_OBJS)"
+	@cmd /C "@echo ### LIBS = $(LIBS)"
+	@cmd /C "@echo ### MODELREF_LIBS = $(MODELREF_LIBS)"
+	@cmd /C "@echo ### SYSTEM_LIBS = $(SYSTEM_LIBS)"
+	@cmd /C "@echo ### TOOLCHAIN_LIBS = $(TOOLCHAIN_LIBS)"
+	@cmd /C "@echo ### CFLAGS = $(CFLAGS)"
+	@cmd /C "@echo ### LDFLAGS = $(LDFLAGS)"
+	@cmd /C "@echo ### SHAREDLIB_LDFLAGS = $(SHAREDLIB_LDFLAGS)"
+	@cmd /C "@echo ### CPPFLAGS = $(CPPFLAGS)"
+	@cmd /C "@echo ### CPP_LDFLAGS = $(CPP_LDFLAGS)"
+	@cmd /C "@echo ### CPP_SHAREDLIB_LDFLAGS = $(CPP_SHAREDLIB_LDFLAGS)"
+	@cmd /C "@echo ### ARFLAGS = $(ARFLAGS)"
+	@cmd /C "@echo ### MEX_CFLAGS = $(MEX_CFLAGS)"
+	@cmd /C "@echo ### MEX_CPPFLAGS = $(MEX_CPPFLAGS)"
+	@cmd /C "@echo ### MEX_LDFLAGS = $(MEX_LDFLAGS)"
+	@cmd /C "@echo ### MEX_CPPLDFLAGS = $(MEX_CPPLDFLAGS)"
+	@cmd /C "@echo ### DOWNLOAD_FLAGS = $(DOWNLOAD_FLAGS)"
+	@cmd /C "@echo ### EXECUTE_FLAGS = $(EXECUTE_FLAGS)"
+	@cmd /C "@echo ### MAKE_FLAGS = $(MAKE_FLAGS)"
+
+
+clean : 
+	$(ECHO) "### Deleting all derived files..."
+	@if exist $(PRODUCT) $(RM) $(PRODUCT)
+	$(RM) $(ALL_OBJS)
+	$(ECHO) "### Deleted all derived files."
+
+

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.rsp

@@ -0,0 +1,4 @@
+PMSM_Controller.obj
+PMSM_Controller_data.obj
+Shunt_Three_Sample_Point.obj
+ert_main.obj

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller_comp.rsp

@@ -0,0 +1,7 @@
+-IE:\works\mcu\MC100\Simulink
+-IE:\works\mcu\MC100\Simulink\PMSM_Controller_ert_rtw
+-I"D:\Program Files\R2020b\extern\include"
+-I"D:\Program Files\R2020b\simulink\include"
+-I"D:\Program Files\R2020b\rtw\c\src"
+-I"D:\Program Files\R2020b\rtw\c\src\ext_mode\common"
+-I"D:\Program Files\R2020b\rtw\c\ert"

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller_data.c

@@ -0,0 +1,338 @@
+/*
+ * File: PMSM_Controller_data.c
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#include "PMSM_Controller.h"
+#include "PMSM_Controller_private.h"
+
+/* Invariant block signals (default storage) */
+const ConstB rtConstB = {
+  /* Start of '<S73>/pi_speed' */
+  {
+    0                                  /* '<S77>/Data Type Conversion2' */
+  }
+  /* End of '<S73>/pi_speed' */
+};
+
+/* Constant parameters (default storage) */
+const ConstP rtConstP = {
+  /* Computed Parameter: r_cos_M1_Table
+   * Referenced by: '<S56>/r_cos_M1'
+   */
+  { 16384, 16384, 16383, 16383, 16382, 16380, 16378, 16376, 16374, 16371, 16368,
+    16365, 16362, 16358, 16353, 16349, 16344, 16339, 16333, 16328, 16322, 16315,
+    16309, 16302, 16294, 16287, 16279, 16270, 16262, 16253, 16244, 16234, 16225,
+    16214, 16204, 16193, 16182, 16171, 16159, 16147, 16135, 16123, 16110, 16096,
+    16083, 16069, 16055, 16041, 16026, 16011, 15996, 15980, 15964, 15948, 15931,
+    15914, 15897, 15880, 15862, 15844, 15826, 15807, 15788, 15769, 15749, 15729,
+    15709, 15689, 15668, 15647, 15626, 15604, 15582, 15560, 15537, 15515, 15491,
+    15468, 15444, 15420, 15396, 15371, 15346, 15321, 15296, 15270, 15244, 15218,
+    15191, 15164, 15137, 15109, 15082, 15053, 15025, 14996, 14968, 14938, 14909,
+    14879, 14849, 14819, 14788, 14757, 14726, 14694, 14663, 14631, 14598, 14566,
+    14533, 14500, 14466, 14433, 14399, 14364, 14330, 14295, 14260, 14225, 14189,
+    14153, 14117, 14081, 14044, 14007, 13970, 13932, 13894, 13856, 13818, 13780,
+    13741, 13702, 13662, 13623, 13583, 13543, 13502, 13462, 13421, 13380, 13338,
+    13297, 13255, 13213, 13170, 13128, 13085, 13042, 12998, 12955, 12911, 12867,
+    12822, 12778, 12733, 12688, 12642, 12597, 12551, 12505, 12458, 12412, 12365,
+    12318, 12271, 12223, 12176, 12128, 12080, 12031, 11982, 11934, 11885, 11835,
+    11786, 11736, 11686, 11636, 11585, 11535, 11484, 11433, 11381, 11330, 11278,
+    11226, 11174, 11121, 11069, 11016, 10963, 10910, 10856, 10803, 10749, 10695,
+    10641, 10586, 10531, 10477, 10422, 10366, 10311, 10255, 10199, 10143, 10087,
+    10031, 9974, 9917, 9860, 9803, 9746, 9688, 9630, 9572, 9514, 9456, 9397,
+    9339, 9280, 9221, 9162, 9102, 9043, 8983, 8923, 8863, 8803, 8743, 8682, 8621,
+    8561, 8500, 8438, 8377, 8316, 8254, 8192, 8130, 8068, 8006, 7943, 7881, 7818,
+    7755, 7692, 7629, 7565, 7502, 7438, 7374, 7311, 7246, 7182, 7118, 7053, 6989,
+    6924, 6859, 6794, 6729, 6664, 6599, 6533, 6467, 6402, 6336, 6270, 6204, 6138,
+    6071, 6005, 5938, 5872, 5805, 5738, 5671, 5604, 5536, 5469, 5402, 5334, 5266,
+    5199, 5131, 5063, 4995, 4927, 4859, 4790, 4722, 4653, 4585, 4516, 4447, 4378,
+    4310, 4240, 4171, 4102, 4033, 3964, 3894, 3825, 3755, 3686, 3616, 3546, 3476,
+    3406, 3336, 3266, 3196, 3126, 3056, 2986, 2915, 2845, 2775, 2704, 2634, 2563,
+    2492, 2422, 2351, 2280, 2209, 2139, 2068, 1997, 1926, 1855, 1784, 1713, 1641,
+    1570, 1499, 1428, 1357, 1285, 1214, 1143, 1072, 1000, 929, 857, 786, 715,
+    643, 572, 500, 429, 357, 286, 214, 143, 71, 0, -71, -143, -214, -286, -357,
+    -429, -500, -572, -643, -715, -786, -857, -929, -1000, -1072, -1143, -1214,
+    -1285, -1357, -1428, -1499, -1570, -1641, -1713, -1784, -1855, -1926, -1997,
+    -2068, -2139, -2209, -2280, -2351, -2422, -2492, -2563, -2634, -2704, -2775,
+    -2845, -2915, -2986, -3056, -3126, -3196, -3266, -3336, -3406, -3476, -3546,
+    -3616, -3686, -3755, -3825, -3894, -3964, -4033, -4102, -4171, -4240, -4310,
+    -4378, -4447, -4516, -4585, -4653, -4722, -4790, -4859, -4927, -4995, -5063,
+    -5131, -5199, -5266, -5334, -5402, -5469, -5536, -5604, -5671, -5738, -5805,
+    -5872, -5938, -6005, -6071, -6138, -6204, -6270, -6336, -6402, -6467, -6533,
+    -6599, -6664, -6729, -6794, -6859, -6924, -6989, -7053, -7118, -7182, -7246,
+    -7311, -7374, -7438, -7502, -7565, -7629, -7692, -7755, -7818, -7881, -7943,
+    -8006, -8068, -8130, -8192, -8254, -8316, -8377, -8438, -8500, -8561, -8621,
+    -8682, -8743, -8803, -8863, -8923, -8983, -9043, -9102, -9162, -9221, -9280,
+    -9339, -9397, -9456, -9514, -9572, -9630, -9688, -9746, -9803, -9860, -9917,
+    -9974, -10031, -10087, -10143, -10199, -10255, -10311, -10366, -10422,
+    -10477, -10531, -10586, -10641, -10695, -10749, -10803, -10856, -10910,
+    -10963, -11016, -11069, -11121, -11174, -11226, -11278, -11330, -11381,
+    -11433, -11484, -11535, -11585, -11636, -11686, -11736, -11786, -11835,
+    -11885, -11934, -11982, -12031, -12080, -12128, -12176, -12223, -12271,
+    -12318, -12365, -12412, -12458, -12505, -12551, -12597, -12642, -12688,
+    -12733, -12778, -12822, -12867, -12911, -12955, -12998, -13042, -13085,
+    -13128, -13170, -13213, -13255, -13297, -13338, -13380, -13421, -13462,
+    -13502, -13543, -13583, -13623, -13662, -13702, -13741, -13780, -13818,
+    -13856, -13894, -13932, -13970, -14007, -14044, -14081, -14117, -14153,
+    -14189, -14225, -14260, -14295, -14330, -14364, -14399, -14433, -14466,
+    -14500, -14533, -14566, -14598, -14631, -14663, -14694, -14726, -14757,
+    -14788, -14819, -14849, -14879, -14909, -14938, -14968, -14996, -15025,
+    -15053, -15082, -15109, -15137, -15164, -15191, -15218, -15244, -15270,
+    -15296, -15321, -15346, -15371, -15396, -15420, -15444, -15468, -15491,
+    -15515, -15537, -15560, -15582, -15604, -15626, -15647, -15668, -15689,
+    -15709, -15729, -15749, -15769, -15788, -15807, -15826, -15844, -15862,
+    -15880, -15897, -15914, -15931, -15948, -15964, -15980, -15996, -16011,
+    -16026, -16041, -16055, -16069, -16083, -16096, -16110, -16123, -16135,
+    -16147, -16159, -16171, -16182, -16193, -16204, -16214, -16225, -16234,
+    -16244, -16253, -16262, -16270, -16279, -16287, -16294, -16302, -16309,
+    -16315, -16322, -16328, -16333, -16339, -16344, -16349, -16353, -16358,
+    -16362, -16365, -16368, -16371, -16374, -16376, -16378, -16380, -16382,
+    -16383, -16383, -16384, -16384, -16384, -16383, -16383, -16382, -16380,
+    -16378, -16376, -16374, -16371, -16368, -16365, -16362, -16358, -16353,
+    -16349, -16344, -16339, -16333, -16328, -16322, -16315, -16309, -16302,
+    -16294, -16287, -16279, -16270, -16262, -16253, -16244, -16234, -16225,
+    -16214, -16204, -16193, -16182, -16171, -16159, -16147, -16135, -16123,
+    -16110, -16096, -16083, -16069, -16055, -16041, -16026, -16011, -15996,
+    -15980, -15964, -15948, -15931, -15914, -15897, -15880, -15862, -15844,
+    -15826, -15807, -15788, -15769, -15749, -15729, -15709, -15689, -15668,
+    -15647, -15626, -15604, -15582, -15560, -15537, -15515, -15491, -15468,
+    -15444, -15420, -15396, -15371, -15346, -15321, -15296, -15270, -15244,
+    -15218, -15191, -15164, -15137, -15109, -15082, -15053, -15025, -14996,
+    -14968, -14938, -14909, -14879, -14849, -14819, -14788, -14757, -14726,
+    -14694, -14663, -14631, -14598, -14566, -14533, -14500, -14466, -14433,
+    -14399, -14364, -14330, -14295, -14260, -14225, -14189, -14153, -14117,
+    -14081, -14044, -14007, -13970, -13932, -13894, -13856, -13818, -13780,
+    -13741, -13702, -13662, -13623, -13583, -13543, -13502, -13462, -13421,
+    -13380, -13338, -13297, -13255, -13213, -13170, -13128, -13085, -13042,
+    -12998, -12955, -12911, -12867, -12822, -12778, -12733, -12688, -12642,
+    -12597, -12551, -12505, -12458, -12412, -12365, -12318, -12271, -12223,
+    -12176, -12128, -12080, -12031, -11982, -11934, -11885, -11835, -11786,
+    -11736, -11686, -11636, -11585, -11535, -11484, -11433, -11381, -11330,
+    -11278, -11226, -11174, -11121, -11069, -11016, -10963, -10910, -10856,
+    -10803, -10749, -10695, -10641, -10586, -10531, -10477, -10422, -10366,
+    -10311, -10255, -10199, -10143, -10087, -10031, -9974, -9917, -9860, -9803,
+    -9746, -9688, -9630, -9572, -9514, -9456, -9397, -9339, -9280, -9221, -9162,
+    -9102, -9043, -8983, -8923, -8863, -8803, -8743, -8682, -8621, -8561, -8500,
+    -8438, -8377, -8316, -8254, -8192, -8130, -8068, -8006, -7943, -7881, -7818,
+    -7755, -7692, -7629, -7565, -7502, -7438, -7374, -7311, -7246, -7182, -7118,
+    -7053, -6989, -6924, -6859, -6794, -6729, -6664, -6599, -6533, -6467, -6402,
+    -6336, -6270, -6204, -6138, -6071, -6005, -5938, -5872, -5805, -5738, -5671,
+    -5604, -5536, -5469, -5402, -5334, -5266, -5199, -5131, -5063, -4995, -4927,
+    -4859, -4790, -4722, -4653, -4585, -4516, -4447, -4378, -4310, -4240, -4171,
+    -4102, -4033, -3964, -3894, -3825, -3755, -3686, -3616, -3546, -3476, -3406,
+    -3336, -3266, -3196, -3126, -3056, -2986, -2915, -2845, -2775, -2704, -2634,
+    -2563, -2492, -2422, -2351, -2280, -2209, -2139, -2068, -1997, -1926, -1855,
+    -1784, -1713, -1641, -1570, -1499, -1428, -1357, -1285, -1214, -1143, -1072,
+    -1000, -929, -857, -786, -715, -643, -572, -500, -429, -357, -286, -214,
+    -143, -71, 0, 71, 143, 214, 286, 357, 429, 500, 572, 643, 715, 786, 857, 929,
+    1000, 1072, 1143, 1214, 1285, 1357, 1428, 1499, 1570, 1641, 1713, 1784, 1855,
+    1926, 1997, 2068, 2139, 2209, 2280, 2351, 2422, 2492, 2563, 2634, 2704, 2775,
+    2845, 2915, 2986, 3056, 3126, 3196, 3266, 3336, 3406, 3476, 3546, 3616, 3686,
+    3755, 3825, 3894, 3964, 4033, 4102, 4171, 4240, 4310, 4378, 4447, 4516, 4585,
+    4653, 4722, 4790, 4859, 4927, 4995, 5063, 5131, 5199, 5266, 5334, 5402, 5469,
+    5536, 5604, 5671, 5738, 5805, 5872, 5938, 6005, 6071, 6138, 6204, 6270, 6336,
+    6402, 6467, 6533, 6599, 6664, 6729, 6794, 6859, 6924, 6989, 7053, 7118, 7182,
+    7246, 7311, 7374, 7438, 7502, 7565, 7629, 7692, 7755, 7818, 7881, 7943, 8006,
+    8068, 8130, 8192, 8254, 8316, 8377, 8438, 8500, 8561, 8621, 8682, 8743, 8803,
+    8863, 8923, 8983, 9043, 9102, 9162, 9221, 9280, 9339, 9397, 9456, 9514, 9572,
+    9630, 9688, 9746, 9803, 9860, 9917, 9974, 10031, 10087, 10143, 10199, 10255,
+    10311, 10366, 10422, 10477, 10531, 10586, 10641, 10695, 10749, 10803, 10856,
+    10910, 10963, 11016, 11069, 11121, 11174, 11226, 11278, 11330, 11381, 11433,
+    11484, 11535, 11585, 11636, 11686, 11736, 11786, 11835, 11885, 11934, 11982,
+    12031, 12080, 12128, 12176, 12223, 12271, 12318, 12365, 12412, 12458, 12505,
+    12551, 12597, 12642, 12688, 12733, 12778, 12822, 12867, 12911, 12955, 12998,
+    13042, 13085, 13128, 13170, 13213, 13255, 13297, 13338, 13380, 13421, 13462,
+    13502, 13543, 13583, 13623, 13662, 13702, 13741, 13780, 13818, 13856, 13894,
+    13932, 13970, 14007, 14044, 14081, 14117, 14153, 14189, 14225, 14260, 14295,
+    14330, 14364, 14399, 14433, 14466, 14500, 14533, 14566, 14598, 14631, 14663,
+    14694, 14726, 14757, 14788, 14819, 14849, 14879, 14909, 14938, 14968, 14996,
+    15025, 15053, 15082, 15109, 15137, 15164, 15191, 15218, 15244, 15270, 15296,
+    15321, 15346, 15371, 15396, 15420, 15444, 15468, 15491, 15515, 15537, 15560,
+    15582, 15604, 15626, 15647, 15668, 15689, 15709, 15729, 15749, 15769, 15788,
+    15807, 15826, 15844, 15862, 15880, 15897, 15914, 15931, 15948, 15964, 15980,
+    15996, 16011, 16026, 16041, 16055, 16069, 16083, 16096, 16110, 16123, 16135,
+    16147, 16159, 16171, 16182, 16193, 16204, 16214, 16225, 16234, 16244, 16253,
+    16262, 16270, 16279, 16287, 16294, 16302, 16309, 16315, 16322, 16328, 16333,
+    16339, 16344, 16349, 16353, 16358, 16362, 16365, 16368, 16371, 16374, 16376,
+    16378, 16380, 16382, 16383, 16383, 16384, 16384 },
+
+  /* Computed Parameter: r_sin_M1_Table
+   * Referenced by: '<S56>/r_sin_M1'
+   */
+  { 0, 71, 143, 214, 286, 357, 429, 500, 572, 643, 715, 786, 857, 929, 1000,
+    1072, 1143, 1214, 1285, 1357, 1428, 1499, 1570, 1641, 1713, 1784, 1855, 1926,
+    1997, 2068, 2139, 2209, 2280, 2351, 2422, 2492, 2563, 2634, 2704, 2775, 2845,
+    2915, 2986, 3056, 3126, 3196, 3266, 3336, 3406, 3476, 3546, 3616, 3686, 3755,
+    3825, 3894, 3964, 4033, 4102, 4171, 4240, 4310, 4378, 4447, 4516, 4585, 4653,
+    4722, 4790, 4859, 4927, 4995, 5063, 5131, 5199, 5266, 5334, 5402, 5469, 5536,
+    5604, 5671, 5738, 5805, 5872, 5938, 6005, 6071, 6138, 6204, 6270, 6336, 6402,
+    6467, 6533, 6599, 6664, 6729, 6794, 6859, 6924, 6989, 7053, 7118, 7182, 7246,
+    7311, 7374, 7438, 7502, 7565, 7629, 7692, 7755, 7818, 7881, 7943, 8006, 8068,
+    8130, 8192, 8254, 8316, 8377, 8438, 8500, 8561, 8621, 8682, 8743, 8803, 8863,
+    8923, 8983, 9043, 9102, 9162, 9221, 9280, 9339, 9397, 9456, 9514, 9572, 9630,
+    9688, 9746, 9803, 9860, 9917, 9974, 10031, 10087, 10143, 10199, 10255, 10311,
+    10366, 10422, 10477, 10531, 10586, 10641, 10695, 10749, 10803, 10856, 10910,
+    10963, 11016, 11069, 11121, 11174, 11226, 11278, 11330, 11381, 11433, 11484,
+    11535, 11585, 11636, 11686, 11736, 11786, 11835, 11885, 11934, 11982, 12031,
+    12080, 12128, 12176, 12223, 12271, 12318, 12365, 12412, 12458, 12505, 12551,
+    12597, 12642, 12688, 12733, 12778, 12822, 12867, 12911, 12955, 12998, 13042,
+    13085, 13128, 13170, 13213, 13255, 13297, 13338, 13380, 13421, 13462, 13502,
+    13543, 13583, 13623, 13662, 13702, 13741, 13780, 13818, 13856, 13894, 13932,
+    13970, 14007, 14044, 14081, 14117, 14153, 14189, 14225, 14260, 14295, 14330,
+    14364, 14399, 14433, 14466, 14500, 14533, 14566, 14598, 14631, 14663, 14694,
+    14726, 14757, 14788, 14819, 14849, 14879, 14909, 14938, 14968, 14996, 15025,
+    15053, 15082, 15109, 15137, 15164, 15191, 15218, 15244, 15270, 15296, 15321,
+    15346, 15371, 15396, 15420, 15444, 15468, 15491, 15515, 15537, 15560, 15582,
+    15604, 15626, 15647, 15668, 15689, 15709, 15729, 15749, 15769, 15788, 15807,
+    15826, 15844, 15862, 15880, 15897, 15914, 15931, 15948, 15964, 15980, 15996,
+    16011, 16026, 16041, 16055, 16069, 16083, 16096, 16110, 16123, 16135, 16147,
+    16159, 16171, 16182, 16193, 16204, 16214, 16225, 16234, 16244, 16253, 16262,
+    16270, 16279, 16287, 16294, 16302, 16309, 16315, 16322, 16328, 16333, 16339,
+    16344, 16349, 16353, 16358, 16362, 16365, 16368, 16371, 16374, 16376, 16378,
+    16380, 16382, 16383, 16383, 16384, 16384, 16384, 16383, 16383, 16382, 16380,
+    16378, 16376, 16374, 16371, 16368, 16365, 16362, 16358, 16353, 16349, 16344,
+    16339, 16333, 16328, 16322, 16315, 16309, 16302, 16294, 16287, 16279, 16270,
+    16262, 16253, 16244, 16234, 16225, 16214, 16204, 16193, 16182, 16171, 16159,
+    16147, 16135, 16123, 16110, 16096, 16083, 16069, 16055, 16041, 16026, 16011,
+    15996, 15980, 15964, 15948, 15931, 15914, 15897, 15880, 15862, 15844, 15826,
+    15807, 15788, 15769, 15749, 15729, 15709, 15689, 15668, 15647, 15626, 15604,
+    15582, 15560, 15537, 15515, 15491, 15468, 15444, 15420, 15396, 15371, 15346,
+    15321, 15296, 15270, 15244, 15218, 15191, 15164, 15137, 15109, 15082, 15053,
+    15025, 14996, 14968, 14938, 14909, 14879, 14849, 14819, 14788, 14757, 14726,
+    14694, 14663, 14631, 14598, 14566, 14533, 14500, 14466, 14433, 14399, 14364,
+    14330, 14295, 14260, 14225, 14189, 14153, 14117, 14081, 14044, 14007, 13970,
+    13932, 13894, 13856, 13818, 13780, 13741, 13702, 13662, 13623, 13583, 13543,
+    13502, 13462, 13421, 13380, 13338, 13297, 13255, 13213, 13170, 13128, 13085,
+    13042, 12998, 12955, 12911, 12867, 12822, 12778, 12733, 12688, 12642, 12597,
+    12551, 12505, 12458, 12412, 12365, 12318, 12271, 12223, 12176, 12128, 12080,
+    12031, 11982, 11934, 11885, 11835, 11786, 11736, 11686, 11636, 11585, 11535,
+    11484, 11433, 11381, 11330, 11278, 11226, 11174, 11121, 11069, 11016, 10963,
+    10910, 10856, 10803, 10749, 10695, 10641, 10586, 10531, 10477, 10422, 10366,
+    10311, 10255, 10199, 10143, 10087, 10031, 9974, 9917, 9860, 9803, 9746, 9688,
+    9630, 9572, 9514, 9456, 9397, 9339, 9280, 9221, 9162, 9102, 9043, 8983, 8923,
+    8863, 8803, 8743, 8682, 8621, 8561, 8500, 8438, 8377, 8316, 8254, 8192, 8130,
+    8068, 8006, 7943, 7881, 7818, 7755, 7692, 7629, 7565, 7502, 7438, 7374, 7311,
+    7246, 7182, 7118, 7053, 6989, 6924, 6859, 6794, 6729, 6664, 6599, 6533, 6467,
+    6402, 6336, 6270, 6204, 6138, 6071, 6005, 5938, 5872, 5805, 5738, 5671, 5604,
+    5536, 5469, 5402, 5334, 5266, 5199, 5131, 5063, 4995, 4927, 4859, 4790, 4722,
+    4653, 4585, 4516, 4447, 4378, 4310, 4240, 4171, 4102, 4033, 3964, 3894, 3825,
+    3755, 3686, 3616, 3546, 3476, 3406, 3336, 3266, 3196, 3126, 3056, 2986, 2915,
+    2845, 2775, 2704, 2634, 2563, 2492, 2422, 2351, 2280, 2209, 2139, 2068, 1997,
+    1926, 1855, 1784, 1713, 1641, 1570, 1499, 1428, 1357, 1285, 1214, 1143, 1072,
+    1000, 929, 857, 786, 715, 643, 572, 500, 429, 357, 286, 214, 143, 71, 0, -71,
+    -143, -214, -286, -357, -429, -500, -572, -643, -715, -786, -857, -929,
+    -1000, -1072, -1143, -1214, -1285, -1357, -1428, -1499, -1570, -1641, -1713,
+    -1784, -1855, -1926, -1997, -2068, -2139, -2209, -2280, -2351, -2422, -2492,
+    -2563, -2634, -2704, -2775, -2845, -2915, -2986, -3056, -3126, -3196, -3266,
+    -3336, -3406, -3476, -3546, -3616, -3686, -3755, -3825, -3894, -3964, -4033,
+    -4102, -4171, -4240, -4310, -4378, -4447, -4516, -4585, -4653, -4722, -4790,
+    -4859, -4927, -4995, -5063, -5131, -5199, -5266, -5334, -5402, -5469, -5536,
+    -5604, -5671, -5738, -5805, -5872, -5938, -6005, -6071, -6138, -6204, -6270,
+    -6336, -6402, -6467, -6533, -6599, -6664, -6729, -6794, -6859, -6924, -6989,
+    -7053, -7118, -7182, -7246, -7311, -7374, -7438, -7502, -7565, -7629, -7692,
+    -7755, -7818, -7881, -7943, -8006, -8068, -8130, -8192, -8254, -8316, -8377,
+    -8438, -8500, -8561, -8621, -8682, -8743, -8803, -8863, -8923, -8983, -9043,
+    -9102, -9162, -9221, -9280, -9339, -9397, -9456, -9514, -9572, -9630, -9688,
+    -9746, -9803, -9860, -9917, -9974, -10031, -10087, -10143, -10199, -10255,
+    -10311, -10366, -10422, -10477, -10531, -10586, -10641, -10695, -10749,
+    -10803, -10856, -10910, -10963, -11016, -11069, -11121, -11174, -11226,
+    -11278, -11330, -11381, -11433, -11484, -11535, -11585, -11636, -11686,
+    -11736, -11786, -11835, -11885, -11934, -11982, -12031, -12080, -12128,
+    -12176, -12223, -12271, -12318, -12365, -12412, -12458, -12505, -12551,
+    -12597, -12642, -12688, -12733, -12778, -12822, -12867, -12911, -12955,
+    -12998, -13042, -13085, -13128, -13170, -13213, -13255, -13297, -13338,
+    -13380, -13421, -13462, -13502, -13543, -13583, -13623, -13662, -13702,
+    -13741, -13780, -13818, -13856, -13894, -13932, -13970, -14007, -14044,
+    -14081, -14117, -14153, -14189, -14225, -14260, -14295, -14330, -14364,
+    -14399, -14433, -14466, -14500, -14533, -14566, -14598, -14631, -14663,
+    -14694, -14726, -14757, -14788, -14819, -14849, -14879, -14909, -14938,
+    -14968, -14996, -15025, -15053, -15082, -15109, -15137, -15164, -15191,
+    -15218, -15244, -15270, -15296, -15321, -15346, -15371, -15396, -15420,
+    -15444, -15468, -15491, -15515, -15537, -15560, -15582, -15604, -15626,
+    -15647, -15668, -15689, -15709, -15729, -15749, -15769, -15788, -15807,
+    -15826, -15844, -15862, -15880, -15897, -15914, -15931, -15948, -15964,
+    -15980, -15996, -16011, -16026, -16041, -16055, -16069, -16083, -16096,
+    -16110, -16123, -16135, -16147, -16159, -16171, -16182, -16193, -16204,
+    -16214, -16225, -16234, -16244, -16253, -16262, -16270, -16279, -16287,
+    -16294, -16302, -16309, -16315, -16322, -16328, -16333, -16339, -16344,
+    -16349, -16353, -16358, -16362, -16365, -16368, -16371, -16374, -16376,
+    -16378, -16380, -16382, -16383, -16383, -16384, -16384, -16384, -16383,
+    -16383, -16382, -16380, -16378, -16376, -16374, -16371, -16368, -16365,
+    -16362, -16358, -16353, -16349, -16344, -16339, -16333, -16328, -16322,
+    -16315, -16309, -16302, -16294, -16287, -16279, -16270, -16262, -16253,
+    -16244, -16234, -16225, -16214, -16204, -16193, -16182, -16171, -16159,
+    -16147, -16135, -16123, -16110, -16096, -16083, -16069, -16055, -16041,
+    -16026, -16011, -15996, -15980, -15964, -15948, -15931, -15914, -15897,
+    -15880, -15862, -15844, -15826, -15807, -15788, -15769, -15749, -15729,
+    -15709, -15689, -15668, -15647, -15626, -15604, -15582, -15560, -15537,
+    -15515, -15491, -15468, -15444, -15420, -15396, -15371, -15346, -15321,
+    -15296, -15270, -15244, -15218, -15191, -15164, -15137, -15109, -15082,
+    -15053, -15025, -14996, -14968, -14938, -14909, -14879, -14849, -14819,
+    -14788, -14757, -14726, -14694, -14663, -14631, -14598, -14566, -14533,
+    -14500, -14466, -14433, -14399, -14364, -14330, -14295, -14260, -14225,
+    -14189, -14153, -14117, -14081, -14044, -14007, -13970, -13932, -13894,
+    -13856, -13818, -13780, -13741, -13702, -13662, -13623, -13583, -13543,
+    -13502, -13462, -13421, -13380, -13338, -13297, -13255, -13213, -13170,
+    -13128, -13085, -13042, -12998, -12955, -12911, -12867, -12822, -12778,
+    -12733, -12688, -12642, -12597, -12551, -12505, -12458, -12412, -12365,
+    -12318, -12271, -12223, -12176, -12128, -12080, -12031, -11982, -11934,
+    -11885, -11835, -11786, -11736, -11686, -11636, -11585, -11535, -11484,
+    -11433, -11381, -11330, -11278, -11226, -11174, -11121, -11069, -11016,
+    -10963, -10910, -10856, -10803, -10749, -10695, -10641, -10586, -10531,
+    -10477, -10422, -10366, -10311, -10255, -10199, -10143, -10087, -10031,
+    -9974, -9917, -9860, -9803, -9746, -9688, -9630, -9572, -9514, -9456, -9397,
+    -9339, -9280, -9221, -9162, -9102, -9043, -8983, -8923, -8863, -8803, -8743,
+    -8682, -8621, -8561, -8500, -8438, -8377, -8316, -8254, -8192, -8130, -8068,
+    -8006, -7943, -7881, -7818, -7755, -7692, -7629, -7565, -7502, -7438, -7374,
+    -7311, -7246, -7182, -7118, -7053, -6989, -6924, -6859, -6794, -6729, -6664,
+    -6599, -6533, -6467, -6402, -6336, -6270, -6204, -6138, -6071, -6005, -5938,
+    -5872, -5805, -5738, -5671, -5604, -5536, -5469, -5402, -5334, -5266, -5199,
+    -5131, -5063, -4995, -4927, -4859, -4790, -4722, -4653, -4585, -4516, -4447,
+    -4378, -4310, -4240, -4171, -4102, -4033, -3964, -3894, -3825, -3755, -3686,
+    -3616, -3546, -3476, -3406, -3336, -3266, -3196, -3126, -3056, -2986, -2915,
+    -2845, -2775, -2704, -2634, -2563, -2492, -2422, -2351, -2280, -2209, -2139,
+    -2068, -1997, -1926, -1855, -1784, -1713, -1641, -1570, -1499, -1428, -1357,
+    -1285, -1214, -1143, -1072, -1000, -929, -857, -786, -715, -643, -572, -500,
+    -429, -357, -286, -214, -143, -71, 0 },
+
+  /* Computed Parameter: Vq_max_M1_Table
+   * Referenced by: '<S46>/Vq_max_M1'
+   */
+  { 2918, 2918, 2916, 2912, 2907, 2901, 2893, 2884, 2873, 2861, 2847, 2832, 2816,
+    2797, 2777, 2756, 2733, 2708, 2681, 2653, 2623, 2591, 2556, 2520, 2481, 2441,
+    2398, 2352, 2303, 2252, 2198, 2140, 2079, 2014, 1945, 1871, 1791, 1706, 1613,
+    1512, 1401, 1277, 1137, 971, 766, 472 },
+
+  /* Computed Parameter: iq_maxSca_M1_Table
+   * Referenced by: '<S46>/iq_maxSca_M1'
+   */
+  { 65535U, 65523U, 65484U, 65418U, 65326U, 65207U, 65062U, 64890U, 64691U,
+    64465U, 64211U, 63930U, 63620U, 63281U, 62913U, 62516U, 62088U, 61630U,
+    61140U, 60618U, 60062U, 59473U, 58848U, 58187U, 57489U, 56752U, 55974U,
+    55155U, 54291U, 53381U, 52422U, 51413U, 50349U, 49227U, 48043U, 46792U,
+    45470U, 44069U, 42581U, 40997U, 39307U, 37494U, 35541U, 33422U, 31105U,
+    28540U, 25655U, 22323U, 18304U, 12974U },
+
+  /* Computed Parameter: vec_hallToPos_Value
+   * Referenced by: '<S9>/vec_hallToPos'
+   */
+  { 7, 5, 1, 0, 3, 4, 2, 7 }
+};
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller_private.h

@@ -0,0 +1,105 @@
+/*
+ * File: PMSM_Controller_private.h
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#ifndef RTW_HEADER_PMSM_Controller_private_h_
+#define RTW_HEADER_PMSM_Controller_private_h_
+#include "rtwtypes.h"
+#include "zero_crossing_types.h"
+#include "PMSM_Controller.h"
+#ifndef UCHAR_MAX
+#include <limits.h>
+#endif
+
+#if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
+#error Code was generated for compiler with different sized uchar/char. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+#if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
+#error Code was generated for compiler with different sized ushort/short. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+#if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
+#error Code was generated for compiler with different sized uint/int. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+#if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
+#error Code was generated for compiler with different sized ulong/long. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+/* Skipping ulong_long/long_long check: insufficient preprocessor integer range. */
+extern real_T rt_roundd_snf(real_T u);
+extern uint16_T plook_u16s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace,
+  uint32_T maxIndex);
+extern uint8_T plook_u8u16_evencka(uint16_T u, uint16_T bp0, uint16_T bpSpace,
+  uint32_T maxIndex);
+extern int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator);
+extern void Counter_Init(DW_Counter *localDW, uint16_T rtp_z_cntInit);
+extern uint16_T Counter(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst,
+  DW_Counter *localDW);
+extern boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW);
+extern void Debounce_Filter_Init(DW_Debounce_Filter *localDW);
+extern void Debounce_Filter(boolean_T rtu_u, uint16_T rtu_tAcv, uint16_T
+  rtu_tDeacv, boolean_T *rty_y, DW_Debounce_Filter *localDW);
+extern void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T
+  rty_y[2], DW_Low_Pass_Filter *localDW);
+extern void PI_iq(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T rtu_Kb,
+                  int16_T rtu_satMax, int16_T rtu_satMin, int16_T
+                  rtu_ext_limProt, int16_T *rty_pi_out, DW_PI_iq *localDW);
+extern void PI_id(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T rtu_Kb,
+                  int16_T rtu_satMax, int16_T rtu_satMin, int16_T
+                  rtu_ext_limProt, int16_T *rty_pi_out, DW_PI_id *localDW);
+extern void pi_speed_Init(DW_pi_speed *localDW);
+extern int16_T pi_speed(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T
+  rtu_Kb, int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt,
+  uint8_T rtu_reset, const ConstB_pi_speed *localC, DW_pi_speed *localDW,
+  ZCE_pi_speed *localZCE);
+
+#endif                               /* RTW_HEADER_PMSM_Controller_private_h_ */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

Simulink/PMSM_Controller_ert_rtw/PMSM_Controller_types.h

@@ -0,0 +1,32 @@
+/*
+ * File: PMSM_Controller_types.h
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#ifndef RTW_HEADER_PMSM_Controller_types_h_
+#define RTW_HEADER_PMSM_Controller_types_h_
+
+/* Model Code Variants */
+
+/* Forward declaration for rtModel */
+typedef struct tag_RTM RT_MODEL;
+
+#endif                                 /* RTW_HEADER_PMSM_Controller_types_h_ */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

Simulink/PMSM_Controller_ert_rtw/Shunt_Three_Sample_Point.c

@@ -0,0 +1,171 @@
+/*
+ * File: Shunt_Three_Sample_Point.c
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#include "Shunt_Three_Sample_Point.h"
+
+/* Include model header file for global data */
+#include "PMSM_Controller.h"
+#include "PMSM_Controller_private.h"
+
+/* Output and update for atomic system: '<S68>/Shunt_Three_Sample_Point' */
+void Shunt_Three_Sample_Point(uint16_T rtu_low, uint16_T rtu_midle, uint8_T
+  rtu_i_sector, uint16_T rtu_half_pwm_count, uint16_T rtu_t_adc, uint16_T
+  rtu_t_dead, uint16_T rtu_t_samp_before, uint16_T *rty_o_samp_p1, uint16_T
+  *rty_o_samp_p2, uint8_T *rty_o_sector)
+{
+  uint32_T qY;
+  uint32_T qY_0;
+  uint32_T tmp;
+  uint32_T tmp_0;
+  uint16_T qY_1;
+  *rty_o_sector = rtu_i_sector;
+  tmp_0 = rtu_half_pwm_count + 1U;
+  if (rtu_half_pwm_count + 1U > 65535U) {
+    tmp_0 = 65535U;
+  }
+
+  *rty_o_samp_p1 = (uint16_T)tmp_0;
+  tmp_0 = rtu_half_pwm_count + 1U;
+  if (rtu_half_pwm_count + 1U > 65535U) {
+    tmp_0 = 65535U;
+  }
+
+  *rty_o_samp_p2 = (uint16_T)tmp_0;
+  tmp_0 = (uint32_T)rtu_t_adc + rtu_t_dead;
+  tmp = tmp_0;
+  if (tmp_0 > 65535U) {
+    tmp = 65535U;
+  }
+
+  tmp += rtu_t_samp_before;
+  if (tmp > 65535U) {
+    tmp = 65535U;
+  }
+
+  qY_0 = (uint32_T)rtu_half_pwm_count - /*MW:OvSatOk*/ rtu_low;
+  if (qY_0 > rtu_half_pwm_count) {
+    qY_0 = 0U;
+  }
+
+  qY = (uint32_T)rtu_half_pwm_count - /*MW:OvSatOk*/ rtu_midle;
+  if (qY > rtu_half_pwm_count) {
+    qY = 0U;
+  }
+
+  if ((uint16_T)qY_0 > 32767) {
+    qY_1 = MAX_uint16_T;
+  } else {
+    qY_1 = (uint16_T)((uint16_T)qY_0 << 1);
+  }
+
+  if (qY_1 >= (uint16_T)tmp) {
+    if (tmp_0 > 65535U) {
+      tmp_0 = 65535U;
+    }
+
+    if ((uint16_T)qY_0 >= (int32_T)tmp_0) {
+      qY_0 = rtu_half_pwm_count - /*MW:OvSatOk*/ 1U;
+      if (rtu_half_pwm_count - 1U > rtu_half_pwm_count) {
+        qY_0 = 0U;
+      }
+
+      *rty_o_samp_p1 = (uint16_T)qY_0;
+      *rty_o_sector = 1U;
+    } else {
+      tmp_0 = (uint32_T)rtu_low + rtu_t_samp_before;
+      if (tmp_0 > 65535U) {
+        tmp_0 = 65535U;
+      }
+
+      if ((uint16_T)tmp_0 >= rtu_half_pwm_count) {
+        if (rtu_half_pwm_count > 32767) {
+          qY_1 = MAX_uint16_T;
+        } else {
+          qY_1 = (uint16_T)(rtu_half_pwm_count << 1);
+        }
+
+        qY_0 = (uint32_T)qY_1 - /*MW:OvSatOk*/ (uint16_T)tmp_0;
+        if (qY_0 > qY_1) {
+          qY_0 = 0U;
+        }
+
+        qY = qY_0 - /*MW:OvSatOk*/ 1U;
+        if (qY_0 - 1U > qY_0) {
+          qY = 0U;
+        }
+
+        *rty_o_samp_p2 = (uint16_T)qY;
+      } else {
+        *rty_o_samp_p1 = (uint16_T)tmp_0;
+      }
+    }
+  } else {
+    if ((uint16_T)qY > 32767) {
+      qY_1 = MAX_uint16_T;
+    } else {
+      qY_1 = (uint16_T)((uint16_T)qY << 1);
+    }
+
+    if (qY_1 >= (uint16_T)tmp) {
+      if (tmp_0 > 65535U) {
+        tmp_0 = 65535U;
+      }
+
+      if ((uint16_T)qY >= (int32_T)tmp_0) {
+        qY_0 = rtu_half_pwm_count - /*MW:OvSatOk*/ 1U;
+        if (rtu_half_pwm_count - 1U > rtu_half_pwm_count) {
+          qY_0 = 0U;
+        }
+
+        *rty_o_samp_p1 = (uint16_T)qY_0;
+      } else {
+        tmp_0 = (uint32_T)rtu_midle + rtu_t_samp_before;
+        if (tmp_0 > 65535U) {
+          tmp_0 = 65535U;
+        }
+
+        if ((uint16_T)tmp_0 >= rtu_half_pwm_count) {
+          if (rtu_half_pwm_count > 32767) {
+            qY_1 = MAX_uint16_T;
+          } else {
+            qY_1 = (uint16_T)(rtu_half_pwm_count << 1);
+          }
+
+          qY_0 = (uint32_T)qY_1 - /*MW:OvSatOk*/ (uint16_T)tmp_0;
+          if (qY_0 > qY_1) {
+            qY_0 = 0U;
+          }
+
+          qY = qY_0 - /*MW:OvSatOk*/ 1U;
+          if (qY_0 - 1U > qY_0) {
+            qY = 0U;
+          }
+
+          *rty_o_samp_p2 = (uint16_T)qY;
+        } else {
+          *rty_o_samp_p1 = (uint16_T)tmp_0;
+        }
+      }
+    }
+  }
+}
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

Simulink/PMSM_Controller_ert_rtw/Shunt_Three_Sample_Point.h

@@ -0,0 +1,39 @@
+/*
+ * File: Shunt_Three_Sample_Point.h
+ *
+ * Code generated for Simulink model 'PMSM_Controller'.
+ *
+ * Model version                  : 1.1200
+ * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
+ * C/C++ source code generated on : Mon Apr  4 09:18:28 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#ifndef RTW_HEADER_Shunt_Three_Sample_Point_h_
+#define RTW_HEADER_Shunt_Three_Sample_Point_h_
+#ifndef PMSM_Controller_COMMON_INCLUDES_
+#define PMSM_Controller_COMMON_INCLUDES_
+#include "rtwtypes.h"
+#include "zero_crossing_types.h"
+#endif                                 /* PMSM_Controller_COMMON_INCLUDES_ */
+
+#include "PMSM_Controller_types.h"
+
+extern void Shunt_Three_Sample_Point(uint16_T rtu_low, uint16_T rtu_midle,
+  uint8_T rtu_i_sector, uint16_T rtu_half_pwm_count, uint16_T rtu_t_adc,
+  uint16_T rtu_t_dead, uint16_T rtu_t_samp_before, uint16_T *rty_o_samp_p1,
+  uint16_T *rty_o_samp_p2, uint8_T *rty_o_sector);
+
+#endif                              /* RTW_HEADER_Shunt_Three_Sample_Point_h_ */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */