update

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

Applications/app/app.c

@@ -25,18 +25,19 @@ 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\n", s->Ia, s->Ib, s->Ic);
-	sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
+	//sys_debug("phase current %f %f %f\n", s->Ia, s->Ib, s->Ic);
+	//sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
 }
 
-
+extern void hall_debug_log(void);
 static void _app_low_task(void *args) {
 	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());
+		//sys_debug("vbus voltage: %f\n", foc_get_vbus_voltage());
+		//hall_debug_log();
 		co_task_delay(1000);
 	}
 }

Applications/bsp/adc.c

@@ -67,8 +67,6 @@ static void _adc1_init(void) {
 	u32 adc_dev = ADC1;
     /* enable ADC1 clock */
     rcu_periph_clock_enable(RCU_ADC1);
-    /* ADC mode config,adc0 master, adc1 slave */
-    adc_mode_config(ADC_DAUL_INSERTED_PARALLEL); 
     /* ADC special function config */
     adc_special_function_config(adc_dev, ADC_SCAN_MODE, ENABLE);
     adc_special_function_config(adc_dev, ADC_CONTINUOUS_MODE, DISABLE);  
@@ -92,7 +90,7 @@ static void _adc1_init(void) {
 /* ADC0 insert chan sample phase I(use two chan, selected by foc) */
 static void _adc0_insert_chan_init(void) {
 	u32 adc_dev = ADC0;
-	adc_discontinuous_mode_config(adc_dev, ADC_CHANNEL_DISCON_DISABLE, 0);	
+	//adc_discontinuous_mode_config(adc_dev, ADC_CHANNEL_DISCON_DISABLE, 0);	
     /* ADC channel length config */
     adc_channel_length_config(adc_dev, ADC_INSERTED_CHANNEL, 1);
     /* ADC inserted channel ran config, use ISQ2,ISQ3 */
@@ -104,7 +102,7 @@ static void _adc0_insert_chan_init(void) {
     /* ADC trigger config */
     adc_external_trigger_source_config(adc_dev, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T0_CH3); 
 	/* ADC external trigger enable */
-    adc_external_trigger_config(adc_dev, ADC_INSERTED_CHANNEL, ENABLE);	
+    adc_external_trigger_config(adc_dev, ADC_INSERTED_CHANNEL, DISABLE);	
 }
 
 /* ADC1 insert chan sample vbus I */
@@ -123,18 +121,16 @@ static void _adc1_insert_chan_init(void) {
     /* ADC trigger config, slave must config to software trigger */
     adc_external_trigger_source_config(adc_dev, ADC_INSERTED_CHANNEL, ADC0_1_2_EXTTRIG_REGULAR_NONE); 
 	/* ADC external trigger enable */
-    adc_external_trigger_config(adc_dev, ADC_INSERTED_CHANNEL, ENABLE);
+    adc_external_trigger_config(adc_dev, ADC_INSERTED_CHANNEL, DISABLE);
 }
 
 static void _adc0_regular_chan_init(void) {
-	//adc_discontinuous_mode_config(ADC0, ADC_REGULAR_CHANNEL, 1); //每次转化一个
+	adc_discontinuous_mode_config(ADC0, ADC_REGULAR_CHANNEL, 1); //每次转化一个
 	/* ADC channel length config */
 	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 1);
 	adc_external_trigger_config(ADC0, ADC_REGULAR_CHANNEL, ENABLE);
 	adc_external_trigger_source_config(ADC0, ADC_REGULAR_CHANNEL, ADC0_1_2_EXTTRIG_REGULAR_NONE); 
 	adc_regular_channel_config(ADC0, 0, MOTOR_TEMP_CHAN, ADC_SAMPLETIME_55POINT5);
-	//adc_regular_channel_config(ADC0, 1, HANDLERBAR_CHAN, ADC_SAMPLETIME_55POINT5);
-	//adc_regular_channel_config(ADC0, 2, VBUS_V_CHAN, ADC_SAMPLETIME_55POINT5);
 }
 
 static void _adc1_regular_chan_init(void) {
@@ -144,22 +140,36 @@ static void _adc1_regular_chan_init(void) {
 	adc_external_trigger_config(ADC1, ADC_REGULAR_CHANNEL, ENABLE);
 	adc_external_trigger_source_config(ADC1, ADC_REGULAR_CHANNEL, ADC0_1_2_EXTTRIG_REGULAR_NONE);
 	adc_regular_channel_config(ADC1, 0, W_PHASE_V_CHAN, ADC_SAMPLETIME_55POINT5);
-	//adc_regular_channel_config(ADC1, 1, V_PHASE_V_CHAN, ADC_SAMPLETIME_55POINT5);
-	//adc_regular_channel_config(ADC1, 2, U_PHASE_V_CHAN, ADC_SAMPLETIME_55POINT5);
 }
 
 void adc_start_insert_convert(void) {
     /* clear the ADC flag */
-    adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOC);
-    adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOIC);
-    adc_interrupt_flag_clear(ADC1, ADC_INT_FLAG_EOC);
-    adc_interrupt_flag_clear(ADC1, ADC_INT_FLAG_EOIC);	
+    adc_flag_clear(ADC0, ADC_FLAG_EOIC);
+    adc_flag_clear(ADC1, ADC_FLAG_EOIC);
+
+	while ((adc_flag_get(ADC0, ADC_FLAG_EOIC) == RESET) || (adc_flag_get(ADC1, ADC_FLAG_EOIC) == RESET));
+	adc_flag_clear(ADC0, ADC_FLAG_EOIC);
+	adc_flag_clear(ADC1, ADC_FLAG_EOIC);
+	
     /* enable ADC interrupt */
     adc_interrupt_enable(ADC0, ADC_INT_EOIC);
-	adc_interrupt_enable(ADC1, ADC_INT_EOIC);
+	//adc_interrupt_enable(ADC1, ADC_INT_EOIC);
 }
 
+void adc_stop_insert_convert(void) {
+	adc_disable_ext_trigger();
+    /* enable ADC interrupt */
+    adc_interrupt_disable(ADC0, ADC_INT_EOIC);
+	//adc_interrupt_disable(ADC1, ADC_INT_EOIC);
+
+    /* clear the ADC flag */
+    adc_flag_clear(ADC0, ADC_FLAG_EOIC);
+	adc_flag_clear(ADC1, ADC_FLAG_EOIC);
+}
+
+
 s32 adc_sample_regular_channel(int channel, int times) {
+#if 0
 	u32 adc_device = ADC0;
 	if (channel >= W_PHASE_V_CHAN && channel <= U_PHASE_V_CHAN) {
 		adc_device = ADC1;
@@ -193,6 +203,9 @@ restart:
 	if (times <= 2) {
 		return value/times;
 	}
-	return (value - min - max)/(times-2);	
+	return (value - min - max)/(times-2);
+#else
+	return 0;
+#endif
 }
 

Applications/bsp/adc.h

@@ -27,20 +27,20 @@ inserted ADC 由timer0 ch3触发，
 #define ADC_RANK_CHANNEL(c)  ((c)<<ISO3_OFFSET | (0)<<IL_OFFSET) 
 #define ADC_CALI_RANK_CHANEL(c)  ((c)<<ISO3_OFFSET | (0)<<IL_OFFSET) 
 static u32 adc0_rank_channels[6] = {
-	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),//1, UW, AC
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),//2, VW, BC
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),//3, VU, BA
+	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),//4, WU, CA
+	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),//5, WV, CB
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),//6, UV, AB
 };
 static u32 adc1_rank_channels[6] = {
-	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),
-	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),
 	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),
 	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),
 	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),
 };
 
 static u32 adc0_cali_rank_channels[3] = {
@@ -56,20 +56,20 @@ static u32 adc1_cali_rank_channels[3] = {
 
 #define PHASE_I_ADC ADC0
 static u32 volatile * adc_phase_reg1[6] = {
-	&ADC_IDATA0(ADC0),
-	&ADC_IDATA0(ADC1),
-	&ADC_IDATA0(ADC0),
-	&ADC_IDATA0(ADC1),
-	&ADC_IDATA0(ADC0),
-	&ADC_IDATA0(ADC1),
+	&ADC_IDATA0(ADC0),//1, U
+	&ADC_IDATA0(ADC0),//2, V
+	&ADC_IDATA0(ADC0),//3, V
+	&ADC_IDATA0(ADC1),//4, U
+	&ADC_IDATA0(ADC1),//5, V 
+	&ADC_IDATA0(ADC1),//6, V
 };
 static u32 volatile * adc_phase_reg2[6] = {
-	&ADC_IDATA0(ADC1),
-	&ADC_IDATA0(ADC0),
-	&ADC_IDATA0(ADC1),
-	&ADC_IDATA0(ADC0),
-	&ADC_IDATA0(ADC1),
-	&ADC_IDATA0(ADC0),
+	&ADC_IDATA0(ADC1),//1, W
+	&ADC_IDATA0(ADC1),//2, W
+	&ADC_IDATA0(ADC1),//3, U
+	&ADC_IDATA0(ADC0),//4, W
+	&ADC_IDATA0(ADC0),//5, W
+	&ADC_IDATA0(ADC0),//6, U
 };
 
 static void __inline adc_phase_current_read(u8 sector, s32 *v1, s32 *v2) {
@@ -84,8 +84,12 @@ static void __inline adc_cali_current_read(s32 *v1, s32 *v2) {
 
 
 static void __inline adc_phase_inserted_config(u8 sector) {
+	ADC_CTL0(ADC0) &= ~(ADC_CTL0_SYNCM);
+
 	ADC_ISQ(ADC0) = adc0_rank_channels[sector];
 	ADC_ISQ(ADC1) = adc1_rank_channels[sector];
+
+	ADC_CTL0(ADC0) |= ADC_DAUL_INSERTED_PARALLEL;
 }
 
 
@@ -101,20 +105,27 @@ static void __inline adc_cali_inserted_config(u8 invert) {
 static void __inline adc_config_trigger(u32 trigger) {
 	ADC_CTL1(ADC0) &= ~((uint32_t)ADC_CTL1_ETSIC);
 	ADC_CTL1(ADC0) |= (uint32_t)trigger;
-#if 1
+
 	ADC_CTL1(ADC1) &= ~((uint32_t)ADC_CTL1_ETSIC);
 	ADC_CTL1(ADC1) |= (uint32_t)trigger;
-#endif
 }
 
 static void __inline adc_disable_ext_trigger(void) {
+	ADC_CTL0(ADC0) &= ~(ADC_CTL0_SYNCM);
+   
 	ADC_CTL1(ADC0) &= ~ADC_CTL1_ETEIC;
 	ADC_CTL1(ADC1) &= ~ADC_CTL1_ETEIC;
+
+	ADC_CTL0(ADC0) |= ADC_DAUL_INSERTED_PARALLEL;
 }
 
 static void __inline adc_enable_ext_trigger(void) {
+	ADC_CTL0(ADC0) &= ~(ADC_CTL0_SYNCM);
+	
 	ADC_CTL1(ADC0) |= ADC_CTL1_ETEIC;
 	ADC_CTL1(ADC1) |= ADC_CTL1_ETEIC;
+
+	ADC_CTL0(ADC0) |= ADC_DAUL_INSERTED_PARALLEL;
 }
 
 static bool __inline adc_is_trigged_vbus(void) {
@@ -148,7 +159,7 @@ static __inline__ void adc_update_insert_sample_time(u32 adc, uint8_t adc_channe
 
 static __inline__ bool adc_eoic_interrupt(void)
 {
-#if 1
+#if 0
 	if (ADC_STAT(ADC0) & ADC_STAT_EOIC){
 		return true;
 	}
@@ -156,10 +167,12 @@ static __inline__ bool adc_eoic_interrupt(void)
 		return true;
 	}
 #else
-	if ((ADC_STAT(ADC0) & ADC_STAT_EOIC) && (ADC_STAT(ADC1) & ADC_STAT_EOIC)) {
+	if ((ADC_STAT(ADC0) & ADC_STAT_EOIC) && (ADC_STAT(ADC1) & ADC_STAT_EOIC)){
 		return true;
 	}
 #endif
+
+
 	return false;
 }
 
@@ -175,5 +188,6 @@ static __inline__ void adc_insert_continue_mode(u32 adc_periph) {
 void adc_init(void);
 s32 adc_sample_regular_channel(int chan, int times);
 void adc_start_insert_convert(void);
+void adc_stop_insert_convert(void);
 
 #endif /* _ADC_H__ */

Applications/bsp/bsp.h

@@ -24,7 +24,7 @@
 #define ADC_CLOCK_MHz (30)
 #define NS_PER_TCLK (8) /* (1/120000000 * 1000000000) */
 #define NS_2_TCLK(ns) ((ns/NS_PER_TCLK) + 1) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (30 * 1000)
+#define FOC_PWM_FS (15 * 1000)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 
@@ -44,7 +44,7 @@
 #define SPEED_SAMPLE_INVAL (100) //ת�Ѳɼ��ļ��,ms
 #define SPEED_RAMP_DURATION SPEED_SAMPLE_INVAL //�����ٶȵ�б��ʱ�䣬�ٶ�ƽ���������½�
 
-#define MAX_VBUS_VOLTAGE 48.0f
+#define MAX_VBUS_VOLTAGE 52.0f
 #define MAX_CURRENT      50.0F
 
 #define pwm_timer TIMER0

Applications/bsp/can.c

@@ -142,7 +142,7 @@ static void _can_tx_task(void *args){
 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 = 2000;
+	uint32_t retry_count = 20000;
 	if (CAN_ERR(CAN0) & CAN_ERR_BOERR){
 		shark_can0_reset();
 	}
@@ -199,7 +199,7 @@ void shark_can0_deinit(void){
 }
 
 void shark_can0_init(void){
-	tx_queue = queue_create(32, sizeof(can_trasnmit_message_struct));
+	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, 256);
 	co_task_create(_can_rx_task, NULL, 512);

Applications/bsp/mc_hall_gpio.h

@@ -6,11 +6,11 @@
 //52.2ms 
 //25.2
 #define HALL_GPOI_CLK RCU_GPIOB
-#define HALL_1_PIN GPIO_PIN_6
+#define HALL_1_PIN GPIO_PIN_8
 #define HALL_1_GROUP GPIOB
 #define HALL_2_PIN GPIO_PIN_7
 #define HALL_2_GROUP GPIOB
-#define HALL_3_PIN GPIO_PIN_8
+#define HALL_3_PIN GPIO_PIN_6
 #define HALL_3_GROUP GPIOB
 
 #define DEGREES_120 0u

Applications/bsp/pwm.c

@@ -23,12 +23,12 @@ static void _pwm_gpio_config(void);
 static void _gpio_brakein_irq_enable(void);
 
 void pwm_3phase_init(void){
-	_pwm_gpio_config();
     _init_pwm_timer();
     if (pwm_timer != adc_timer) {
 		_init_adc_timer();
 		_init_aux_timer();
     }
+	_pwm_gpio_config();
 }
 
 static rcu_periph_enum _rcu_clk(u32 timer) {
@@ -149,7 +149,8 @@ static void _init_pwm_timer(void) {
 	}else {
 		timer_master_slave_mode_config(timer,TIMER_MASTER_SLAVE_MODE_DISABLE);
 	}
-    timer_primary_output_config(timer,ENABLE);
+	pwm_enable_channel();
+	//timer_primary_output_config(timer,ENABLE);
     /* auto-reload preload enable */
     timer_auto_reload_shadow_enable(timer);
 
@@ -253,6 +254,7 @@ void pwm_start(void){
 	{}
 	/* Clear Update Flag */
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
+
 	timer_primary_output_config(pwm_timer,ENABLE);
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
 	timer_interrupt_enable(pwm_timer, TIMER_INT_UP);
@@ -276,7 +278,7 @@ void pwm_turn_on_low_side(void)
 	timer_flag_clear(pwm_timer,TIMER_FLAG_UP);
   	while (timer_flag_get(pwm_timer,TIMER_FLAG_UP) == RESET );
   	/* Main PWM Output Enable */
-  	pwm_enable_channel();
+  	timer_primary_output_config(pwm_timer, ENABLE);
 }
 
 

Applications/bsp/uart.c

@@ -4,7 +4,7 @@
 #include "libs/logger.h"
 #include "libs/utils.h"
 
-#define SHARK_UART_BAUDRATE				38400
+#define SHARK_UART_BAUDRATE				500000
 
 #define SHARK_UART0_com					UART3
 #define SHARK_UART0_tx_port				GPIOB

Applications/foc/foc_api.c

@@ -67,6 +67,7 @@ float speed_to_current(u16 rpm) {
 
 void foc_clear(void) {
 	pwm_stop();
+	adc_stop_insert_convert();
 	g_foc.mosfec_gate = false;
 	foc_defulat_value();
 	hall_sensor_clear();
@@ -136,26 +137,29 @@ void foc_current_calibrate(void){
 	g_foc.current_samp.adc_offset_c = 0;
 	g_foc.current_samp.adc_offset_ivbus = 0;
 
-	pwm_disable_channel();
 	foc_pwm_start(false);
 
-	cpu_udelay(10);
+	cpu_udelay(100);
 	
 	phase_current_init(&g_foc.current_samp);
 	g_foc.current_samp.is_calibrating_offset = true;
-	g_foc.current_samp.sector = SECTOR_5;
+	g_foc.current_samp.sector = SECTOR_1;
+
 	foc_pwm_start(true);
+
+	cpu_udelay(50 * 1000);
+
 	adc_start_insert_convert();
 	while(g_foc.current_samp.offset_sample_count != 0){};
 	
 	foc_pwm_start(false);
 	cpu_udelay(100);
 	phase_current_init(&g_foc.current_samp);	
-	g_foc.current_samp.sector = SECTOR_1;
+	g_foc.current_samp.sector = SECTOR_5;
 	foc_pwm_start(true);
 	while(g_foc.current_samp.offset_sample_count != 0){};
+
 	g_foc.current_samp.is_calibrating_offset = false;
-	pwm_enable_channel();
 }
 
 float foc_get_vbus_voltage(void){

Applications/foc/foc_cmd.c

@@ -42,6 +42,22 @@ static void foc_cmd_task(void *args) {
 }
 
 
+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);
+		foc_pwm_start(true);
+		adc_start_insert_convert();
+		int result = hall_sensor_calibrate(vd);
+		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);
+	}
+}
+
 
 static void process_foc_command(foc_cmd_body_t *command) {
 	sys_debug("command %d\n", command->cmd);
@@ -58,6 +74,9 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)f);
 			break;
 		}
+		case Foc_Cali_Hall_Phase:
+			do_hall_calibrate(command->can_src, decode_s32((u8 *)command->data));
+			break;
 		case Foc_Cali_Hall_Offset:
 		{
 			int offset = 0;

Applications/foc/foc_core.c

@@ -98,29 +98,29 @@ static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_
     /* Dead time compensation */
     if ( c_sample->Ia > 0)
     {
-      time->A += TDead;
+      time->A -= TDead;
     }
     else
     {
-      time->A -= TDead;
+      time->A += TDead;
     }
 
     if ( c_sample->Ib > 0 )
     {
-      time->B += TDead;
+      time->B -= TDead;
     }
     else
     {
-      time->B -= TDead;
+      time->B += TDead;
     }
 
     if ( c_sample->Ic > 0 )
     {
-      time->C += TDead;
+      time->C -= TDead;
     }
     else
     {
-      time->C -= TDead;
+      time->C += TDead;
     }
 #endif
 }
@@ -255,7 +255,7 @@ static void foc_measure_task(void *args){
 	while(1) {
 		vbus_sample_voltage();
 		ntc_sensor_sample();
-		g_foc.vbus = vbus_get_filted_voltage();
+		//g_foc.vbus = vbus_get_filted_voltage();
 		wdog_reload();
 		co_task_yield();
 	}

Applications/foc/foc_type.h

@@ -41,6 +41,8 @@ typedef struct _phase_time {
 	u32 A;
 	u32 B;
 	u32 C;
+	u32 Samp_p1; //单电阻方案两个采样时刻可能不是连续的
+	u32 Samp_p2;
 	u32 low;
 	u32 midle;
 	u32 high;

Applications/foc/hall_sensor.c

@@ -19,7 +19,7 @@ static void _hall_init_el_angle(void);
 
 #define PWM_T  (0.000033f)
 
-#define HALL_PLACE_OFFSET (199)
+#define HALL_PLACE_OFFSET (315)//(345) //315
 /* 
 100
 101
@@ -37,9 +37,12 @@ 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 void __inline _hall_put_sample(u32 ticks) {
-	hall_sample_t *s = &_sensor_hander.samples;
+static float hall_speed[8];
+static float  hall_angle[8];
+static void __inline _hall_put_sample(u32 ticks, u8 hall) {
+	hall_sample_t *s = &_sensor_hander.samples[hall];
 	s->ticks_sum -= s->ticks[s->index];
 	s->ticks[s->index] = ticks;
 	s->ticks_sum += s->ticks[s->index];
@@ -51,14 +54,47 @@ static void __inline _hall_put_sample(u32 ticks) {
 	}
 }
 
-static float __inline _hall_avg_speed(void){
-	hall_sample_t *s = &_sensor_hander.samples;
-	if (s->ticks_sum == 0.0f) {
-		return 0.0f;
+static u32 __inline _hall_angle_us_speed(u8 hall){
+	hall_sample_t *s = &_sensor_hander.samples[hall];
+	if (s->ticks_sum == 0) {
+		return 0;
 	}
-	return (((float)PHASE_60_DEGREE * (float)SAMPLE_MAX_COUNT) / (us_2_s(s->ticks_sum)));
+
+	if (!s->full) {
+		return (s->ticks[s->index-1]);
+	}else {
+		return s->ticks_sum/SAMPLE_MAX_COUNT;
+	}
+}
+
+static float __inline _hall_angle_speed(void){
+	u32 sum = 0 ;
+	u32 num = 0;
+	for (int hall = 1; hall < 7; hall++) {
+		hall_sample_t *s = &_sensor_hander.samples[hall];
+		if (s->ticks_sum == 0) {
+			continue;
+		}
+	
+		if (!s->full) {
+			sum +=(s->ticks[s->index-1]);
+			num ++;
+		}else {
+			sum  += s->ticks_sum;
+			num += SAMPLE_MAX_COUNT;
+		}
+	}
+	return (float)PHASE_60_DEGREE * (float)num / us_2_s(sum);
 }
 
+
+static bool __inline _hall_data_empty(u8 hall) {
+	hall_sample_t *s = &_sensor_hander.samples[hall];
+	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;
@@ -75,6 +111,17 @@ void hall_sensor_clear(void) {
 	hall_sensor_default();
 }
 
+
+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("angle dir %d\n", _sensor_hander.direction);
+	
+}
+
 static void _hall_detect_task(void *args) {
 	while(1) {
 		if (_sensor_hander.el_speed != 0) {
@@ -93,24 +140,23 @@ float hall_sensor_get_theta(void){
 		return _sensor_hander.override_el_angle;
 	}
 
-	float angle_step = _sensor_hander.el_speed * PWM_T;
-	float compensated_step = angle_step + _sensor_hander.el_speed_compensate * PWM_T * 0.05f;
-	_sensor_hander.estimate_delta_angle += compensated_step;
-#if 1
+	u32 us_now = timer_count32_delta_us(_sensor_hander.estimate_time_ticks, NULL);
+	float ration = (float)us_now / (float)_sensor_hander.speed_us_for_estimate;
+	
+	float angle_step = (float)PHASE_60_DEGREE * ration;
+	_sensor_hander.estimate_delta_angle = angle_step;
+	if (angle_step >= PHASE_60_DEGREE) {
+		angle_step = PHASE_60_DEGREE;
+	}
+
 	if (_sensor_hander.direction == POSITIVE) {
-		_sensor_hander.estimate_el_angle += compensated_step;
+		_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle + angle_step;
 	}else {
-		_sensor_hander.estimate_el_angle -= compensated_step;
+		_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle - angle_step;
 	}
-#else
-	if (_sensor_hander.estimate_delta_angle >= PHASE_60_DEGREE) {
-		if (_sensor_hander.direction == POSITIVE) {
-			_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle + PHASE_60_DEGREE;
-		}else {
-			_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle - PHASE_60_DEGREE;
-		}
-	}
-#endif
+
+	rand_angle(_sensor_hander.estimate_el_angle);
+
 	return _sensor_hander.estimate_el_angle;
 }
 
@@ -126,7 +172,7 @@ float hall_sensor_get_speed(void) {
 }
 
 float hall_sensor_avg_speed(void) {
-	return _sensor_hander.el_speed_avg / 360 * 60.0f;
+	return _sensor_hander.el_speed / 360 * 60.0f;
 }
 
 int hall_offset_increase(int inc) {
@@ -138,6 +184,69 @@ int hall_offset_increase(int inc) {
 	return _sensor_hander.phase_offset;
 }
 
+
+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);
+	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(hall_iterations, 0, sizeof(hall_iterations));
+	co_task_delay(2 * 1000);
+	// Forwards
+	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]++;
+		}
+	}
+	//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]++;
+		}
+	}
+	foc_pwm_start(false);
+	hall_sensor_set_theta(false, 0.0f);
+	foc_set_dq_command(0.0f, 0.0f);
+	int fails = 0;
+	for(int i = 0;i < 8;i++) {
+		if (hall_iterations[i] > 30) {
+		} else {
+			fails++;
+		}
+	}
+	return fails == 2;	
+}
+
+
 static void _hall_init_el_angle(void) {
 	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
   	switch ( _sensor_hander.hall_stat )
@@ -152,24 +261,26 @@ static void _hall_init_el_angle(void) {
 		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + PHASE_60_DEGREE / 2;
       	break;
     case STATE_2:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_120_DEGREE - PHASE_60_DEGREE / 2;
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_180_DEGREE + PHASE_60_DEGREE / 2;
       	break;
     case STATE_6:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_60_DEGREE - PHASE_60_DEGREE / 2;
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + PHASE_60_DEGREE / 2;
       	break;
     case STATE_4:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_60_DEGREE / 2;
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + PHASE_60_DEGREE / 2;
       	break;
     default:
       	/* Bad hall sensor configutarion so update the speed reliability */
-      	_sensor_hander.sensor_error = true;
+      	_sensor_hander.sensor_error ++;
       	break;
  	}
   	/* 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();
 }
 
+/* 4,5,1,3,2,6,4 */
 static s32 _hall_position(u8 state_now, u8 state_prev) {
 	s32 theta_now =  0xFFFFFFFF;
 	switch (state_now) {
@@ -188,7 +299,7 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
 			}else if (state_prev == STATE_6) {
 				_sensor_hander.direction = NEGATIVE;
-				theta_now = _sensor_hander.phase_offset - PHASE_120_DEGREE;
+				theta_now = _sensor_hander.phase_offset + PHASE_240_DEGREE;
 			}
 			break;
 		case STATE_3:
@@ -203,7 +314,7 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 		case STATE_4:
 			if (state_prev == STATE_6) {
 				_sensor_hander.direction = POSITIVE;
-				theta_now = _sensor_hander.phase_offset - PHASE_60_DEGREE;
+				theta_now = _sensor_hander.phase_offset + PHASE_300_DEGREE;
 			}else if (state_prev == STATE_5) {
 				_sensor_hander.direction = NEGATIVE;
 				theta_now = _sensor_hander.phase_offset;
@@ -221,19 +332,26 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 		case STATE_6:
 			if (state_prev == STATE_2) {
 				_sensor_hander.direction = POSITIVE;
-				theta_now = _sensor_hander.phase_offset - PHASE_120_DEGREE;
+				theta_now = _sensor_hander.phase_offset + PHASE_240_DEGREE;
 			}else if (state_prev == STATE_4) {
 				_sensor_hander.direction = NEGATIVE;
-				theta_now = _sensor_hander.phase_offset - PHASE_60_DEGREE;
+				theta_now = _sensor_hander.phase_offset + PHASE_300_DEGREE;
 			}
 			break;
 		default:
+			_sensor_hander.sensor_error ++;
 			return 0xFFFFFFFF;
 	}
+	rand_angle(theta_now);
 	return theta_now;
 }
 
 void hall_sensor_handler(void) {
+	if (_sensor_hander.is_override_angle) {
+		log_chan_value(1, _sensor_hander.override_el_angle);
+		log_chan_value(2, (int)get_hall_stat(HALL_READ_TIMES));
+		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;
@@ -250,30 +368,28 @@ void hall_sensor_handler(void) {
 	}
 
 	float delta_time = us_2_s(delta_us);
-	_hall_put_sample(delta_us);
-	if (_sensor_hander.samples.full) {
-		_sensor_hander.el_speed_avg = _hall_avg_speed();
-		///_sensor_hander.estimate_angle_aliment = true;
+	_hall_put_sample(delta_us, hall_stat_prev);
+	u32 us_speed_prev = _sensor_hander.speed_us_for_estimate;
+	u32 us_speed_now = 0;
+	if (_hall_data_empty(hall_stat_now)) {
+		us_speed_now = delta_us;
 	}else {
-		_sensor_hander.el_speed_avg = (float)PHASE_60_DEGREE / delta_time;
+		us_speed_now = _hall_angle_us_speed(hall_stat_now);
 	}
-
+	if (us_speed_now != 0) {
+		hall_speed[hall_stat_now] = (float)us_speed_prev / (float)us_speed_now;
+	}
+	hall_angle[hall_stat_now] = (float)_sensor_hander.estimate_delta_angle/(float)PHASE_60_DEGREE;
 	os_disable_irq();
 	
+	_sensor_hander.estimate_delta_angle = 0;
 	_sensor_hander.measured_el_angle = theta_now;	
 	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
 
-	if (_sensor_hander.estimate_angle_aliment) {
-		float delta_angle = (float)PHASE_60_DEGREE - _sensor_hander.estimate_delta_angle;
-		_sensor_hander.el_speed_compensate = delta_angle / delta_time;
-	}else {
-		_sensor_hander.el_speed_compensate = 0.0f;
-	}
-	_sensor_hander.estimate_delta_angle = 0.0f;
-	_sensor_hander.el_speed = _sensor_hander.el_speed_avg;
-
+	_sensor_hander.estimate_time_ticks = hall_ticks_now;
+	_sensor_hander.speed_us_for_estimate = us_speed_now;
 	os_enable_irq();
-	
+	_sensor_hander.el_speed = _hall_angle_speed();
 	_sensor_hander.hall_stat = hall_stat_now;
 	_sensor_hander.hall_ticks = hall_ticks_now;
 	

Applications/foc/hall_sensor.h

@@ -23,7 +23,7 @@
 #define STATE_7 (uint8_t)7
 
 #define THETA_NONE        (float)0xFFFF
-#define SAMPLE_MAX_COUNT 6
+#define SAMPLE_MAX_COUNT 3
 
 typedef struct {
 	u32   ticks[SAMPLE_MAX_COUNT];
@@ -33,14 +33,12 @@ typedef struct {
 }hall_sample_t;
 
 typedef struct {
-	bool  estimate_angle_aliment;
-	s32   measured_el_angle; //hall测量到的电角度
 	float estimate_el_angle; //60度区间内的估计电角度
-	float estimate_delta_angle;
+	u32   estimate_time_ticks;
+	s32   estimate_delta_angle;//for debug
+	s32   measured_el_angle; //hall测量到的电角度
+	u32   speed_us_for_estimate; //hall经过60°的时间，给角度估计使用
 	float el_speed; 		//当前的电角速度, 单位：rad/s
-	float el_speed_avg;	//滤波后的电角速度
-	float el_speed_compensate; //电角速度补偿
-	//float el_angle_mod;
 	float rpm;        		//当前的电速度, 单位：RPM
 	u8    hall_stat;
 	u32   hall_ticks;
@@ -49,8 +47,8 @@ typedef struct {
 	s32   phase_offset;
 	bool  is_override_angle;
 	float override_el_angle;
-	hall_sample_t samples;
-	bool  sensor_error;
+	hall_sample_t samples[8];
+	u32  sensor_error;
 }hall_sensor_t;
 
 
@@ -61,6 +59,7 @@ 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);
 

Applications/foc/phase_current.c

@@ -40,7 +40,7 @@ void phase_current_offset(current_samp_t *cs) {
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
 	if (cs->offset_sample_count > 0) {
 		cs->offset_sample_count--;
-		if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
+		if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
 			cs->adc_offset_a += phase_current1;
 			cs->adc_offset_c += phase_current2;
 			if (cs->offset_sample_count == 0) {
@@ -48,9 +48,8 @@ void phase_current_offset(current_samp_t *cs) {
 				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
 			}
 		}
-		if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
+		if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
 			cs->adc_offset_b += phase_current1;
-			cs->adc_offset_ivbus += phase_current1;
 			if (cs->offset_sample_count == 0) {
 				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
 			}
@@ -62,21 +61,20 @@ void phase_current_sample(current_samp_t *cs){
 	s32 phase_current1, phase_current2;
 
 	adc_disable_ext_trigger();
-	cs->sector = SECTOR_1;
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
-	if (cs->sector == SECTOR_1 || cs->sector == SECTOR_2) {
+	if (cs->sector == SECTOR_3 || cs->sector == SECTOR_6) {
 		/* Current on Phase C is not accessible */
 		/* Ia = PhaseAOffset - ADC converted value) */
 		cs->Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), cs->Rds_b);
 		cs->Ia = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_a), cs->Rds_a);
 		cs->Ic = -(cs->Ia + cs->Ib);
-	}else if (cs->sector == SECTOR_3 || cs->sector == SECTOR_4) {
+	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_5) {
 		/* Current on Phase A is not accessible 	*/
 		/* Ib = PhaseBOffset - ADC converted value) */
-		cs->Ic = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_c), cs->Rds_c);
-		cs->Ib = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_b), cs->Rds_b);
+		cs->Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), cs->Rds_b);
+		cs->Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), cs->Rds_c);
 		cs->Ia = -(cs->Ib + cs->Ic);
-	}else if (cs->sector == SECTOR_5 || cs->sector == SECTOR_6) {
+	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_4) {
 		/* Current on Phase B is not accessible 	*/
 		/* Ia = PhaseAOffset - ADC converted value) */
 		cs->Ia = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_a), cs->Rds_a);
@@ -122,6 +120,7 @@ u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector){
 		}else {
 			u32 sample_point = time->low + (TDead + MAX(TRise, TNoise));
 			if (sample_point >= FOC_PWM_Half_Period) {
+				//这里需要修改触发方式，GD不支持adc设置上升或下降沿触发，考虑切换pwm模式？？？
 				sample_point = ( 2u * FOC_PWM_Half_Period ) - sample_point - (uint16_t) 1;		
 			}
 			return sample_point;
@@ -134,13 +133,7 @@ u32 get_vbus_sample_point(current_samp_t *cs, phase_time_t *time){
 }
 
 void phase_current_adc_triger(current_samp_t *cs){
-	if (cs->vbus_i_invert != INVERT_NO) {
-		adc_config_trigger(ADC_TRIGGER_VBUS);
-		adc_cali_inserted_config(cs->vbus_i_invert-1);
-	}else {
-		adc_config_trigger(ADC_TRIGGER_PHASE);
-		adc_phase_inserted_config(cs->sector);
-	}
+	adc_phase_inserted_config(cs->sector);
 	adc_enable_ext_trigger();
 }
 

Applications/foc/svpwm.c

@@ -1,415 +1,6 @@
 #include "foc/svpwm.h"
 #include "math/fast_math.h"
 
-void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
-	float alpha = alpha_beta->alpha / (3.0f/2.0f * vbus);
-	float beta  = alpha_beta->beta / (3.0f/2.0f * vbus);
-	uint32_t sector = 0xFF;
-
-	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;
-			}
-		}
-	}
-	// PWM timings
-	u32 tA, tB, tC;
-	u32 low, midle, high;
-	switch (sector) {
-		// sector 1-2
-	case SECTOR_1: {
-		// Vector on-times
-		uint32_t t1 = (alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
-		uint32_t t2 = (TWO_BY_SQRT3 * beta) * PWM_half_period;
-	
-		// PWM timings
-		tA = (PWM_half_period - t1 - t2) / 2;
-		tB = tA + t1;
-		tC = tB + t2;
-		low = tA;
-        midle = tB;
-        high = tC;
-		break;
-	}
-	// sector 2-3
-	case SECTOR_2: {
-		// Vector on-times
-		uint32_t t2 = (alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
-		uint32_t t3 = (-alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
-	
-		// PWM timings
-		tB = (PWM_half_period - t2 - t3) / 2;
-		tA = tB + t3;
-		tC = tA + t2;
-		low = tB;
-        midle = tA;
-        high = tC;	
-		break;
-	}
-	
-	// sector 3-4
-	case SECTOR_3: {
-		// Vector on-times
-		uint32_t t3 = (TWO_BY_SQRT3 * beta) * PWM_half_period;
-		uint32_t t4 = (-alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
-	
-		// PWM timings
-		tB = (PWM_half_period - t3 - t4) / 2;
-		tC = tB + t3;
-		tA = tC + t4;
-		low = tB;
-        midle = tC;
-        high = tA;
-		break;
-	}
-	
-	// sector 4-5
-	case SECTOR_4: {
-		// Vector on-times
-		uint32_t t4 = (-alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
-		uint32_t t5 = (-TWO_BY_SQRT3 * beta) * PWM_half_period;
-	
-		// PWM timings
-		tC = (PWM_half_period - t4 - t5) / 2;
-		tB = tC + t5;
-		tA = tB + t4;
-		low = tC;
-        midle = tB;
-        high = tA;	
-		break;
-	}
-	
-	// sector 5-6
-	case SECTOR_5: {
-		// Vector on-times
-		uint32_t t5 = (-alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
-		uint32_t t6 = (alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
-	
-		// PWM timings
-		tC = (PWM_half_period - t5 - t6) / 2;
-		tA = tC + t5;
-		tB = tA + t6;
-		low = tC;
-        midle = tA;
-        high = tB;	
-		break;
-	}
-	
-	// sector 6-1
-	case SECTOR_6: {
-		// Vector on-times
-		uint32_t t6 = (-TWO_BY_SQRT3 * beta) * PWM_half_period;
-		uint32_t t1 = (alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
-
-		// PWM timings
-		tA = (PWM_half_period - t6 - t1) / 2;
-		tC = tA + t1;
-		tB = tC + t6;
-		low = tA;
-        midle = tC;
-        high = tB;	
-		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++ % 5 == 0) {
-		printf("$%d %d %d;", tA, tB, tC);
-	}
-#endif		
-}
-
-#if 0
-static u8 __inline Calc_N(alpha_beta_t *alpha_beta){
-	float sqr_alpha = alpha_beta->alpha * SQRT3_BY_2;
-	float half_beta = 0.5f * alpha_beta->beta;
-	return (((sqr_alpha - half_beta > 0.0f) << 1) + (alpha_beta->beta > 0.0f)) + ((-(sqr_alpha + half_beta) > 0.0f) << 2);
-}
-
-static void __inline Calc_XYZ(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_Period, float *XYZ_Out) {
-	float modu = (1.0f / vbus) * (float)PWM_Period;
-	float sqr_beta = SQRT3_BY_2 * alpha_beta->beta;
-	float alpha = 1.5f * alpha_beta->alpha;
-
-	XYZ_Out[0] = SQRT3 * alpha_beta->beta * modu;
-	XYZ_Out[1] = (sqr_beta + alpha) * modu;
-	XYZ_Out[2] = (sqr_beta - alpha) * modu;
-}
-#endif
-static void __inline ModuleTime(u32 *T4, u32 *T6, u32 PWM_Period) {
-	u32 period = PWM_Period * 95 / 100; //95%�ĵ���
-	if (*T4 + *T6 > period){
-		float ration = ((float)period)/((float)*T4 + (float)*T6);
-		*T4 *= ration;
-		*T6 *= ration;
-	}
-}
-void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
-	u32 PWM_Period = PWM_half_period * 2;
-	float wAlpha = SQRT3 * alpha_beta->alpha * 2.0f;
-	float wBeta = -alpha_beta->beta * 2.0f;
-	float X = wBeta * PWM_Period/vbus;
-	float Y = (wBeta + wAlpha)*PWM_Period/vbus/2.0f;
-	float Z = (wBeta - wAlpha)*PWM_Period/vbus/2.0f;	
-	s32 tA, tB, tC;
-	s32 low, midle, high;
-	if (Y < 0) {
-    	if (Z < 0) {
-        	*sector_out = 5;
-        	tA = PWM_Period/4 + (Y - Z)/4;
-        	tB = tA + Z/2;
-        	tC = tA - Y/2;
-      		low = tC;
-      		midle = tA;
-      		high = tB;			
-    	}else {
-        	if (X <= 0 ) {
-            	*sector_out = 4;
-            	tA = PWM_Period/4 + (X - Z)/4;
-            	tB = tA + Z/2;
-            	tC = tB - X/2;
-				low = tC;
-      			midle = tB;
-      			high = tA;
-        	}else {
-            	*sector_out = 3;
-            	tA = PWM_Period/4 + (Y - X)/4;
-            	tC = tA - Y/2;
-            	tB = tC + X/2;
-      			low = tB;
-      			midle = tC;
-      			high = tA;				
-        	}
-    	}
-	}else {
-    	if (Z >= 0) {
-        	*sector_out = 2;
-        	tA = PWM_Period/4 + (Y - Z)/4;
-        	tB= tA + Z/2;
-        	tC = tA - Y/2;
-      		low = tB;
-      		midle = tC;
-      		high = tA;			
-    	}else {
-        	if (X <= 0 ) {
-            	*sector_out = 6;
-            	tA = PWM_Period/4 + (Y - X)/4;
-            	tC = tA - Y/2;    
-            	tB = tC + X/2;
-	      		low = tA;
-      			midle = tC;
-      			high = tB;			
-        	} else {
-            	*sector_out = 1;
-            	tA = PWM_Period/4 + (X - Z)/4;
-            	tB = tA + Z/2;
-            	tC = tB - X/2;
-	      		low = tA;
-      			midle = tB;
-      			high = tC;
-        	}
-    	}
-	}
-	phase_out->A = ( uint16_t )tA;
-	phase_out->B = ( uint16_t )tB;
-	phase_out->C = ( uint16_t )tC;
-	phase_out->low = low;
-	phase_out->midle = midle;
-	phase_out->high = high;	
-}
-
-void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
-	float alpha = alpha_beta->alpha * 2.0f / 3.0f;
-	float beta  = alpha_beta->beta  * 2.0f / 3.0f;
-	u8 sector = 0xFF;
-	u32 A_duty, B_duty, C_duty;
-	u32 low, midle, high;
-	u32 T1, T2;
-	float X, Y, Z;
-	float modu = (float)(PWM_half_period) / vbus;
-
-	if (beta >= 0.0f) {
-		if (alpha >= 0.0f) {
-			//quadrant I
-			if (ONE_BY_SQRT3 * beta > alpha) {
-				sector = SECTOR_2;
-			} else {
-				sector = SECTOR_1;
-			}
-		} else {
-			//quadrant II
-			if (-ONE_BY_SQRT3 * beta > alpha) {
-				sector = SECTOR_3;
-			} else {
-				sector = SECTOR_2;
-			}
-		}
-	} else {
-		if (alpha >= 0.0f) {
-			//quadrant IV5
-			if (-ONE_BY_SQRT3 * beta > alpha) {
-				sector = SECTOR_5;
-			} else {
-				sector = SECTOR_6;
-			}
-		} else {
-			//quadrant III
-			if (ONE_BY_SQRT3 * beta > alpha) {
-				sector = SECTOR_4;
-			} else {
-				sector = SECTOR_5;
-			}
-		}
-	}
-	//X = SQRT3 * beta * modu;
-	X = TWO_BY_SQRT3 * beta * modu;
-	//Y = (1.5f * alpha + SQRT3_BY_2 * beta) * modu;
-	Y = (alpha + ONE_BY_SQRT3 * beta) * modu;
-	//Z = (-1.5f * alpha + SQRT3_BY_2 * beta) * modu;
-	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
-	switch(sector) {
-		case SECTOR_1: // 3
-			T1 = -Z;
-			T2 = X;
-			break;		
-		case SECTOR_2: // 1
-			T1 = Z;
-			T2 = Y;			
-			break;
-		case SECTOR_3: // 5
-			T1 = X;
-			T2 = -Y;		
-			break;
-		case SECTOR_4: // 4
-			T1 = -X;
-			T2 = Z;		
-			break;
-		case SECTOR_5: // 6
-			T1 = -Y;
-			T2 = -Z;		
-			break;			
-		case SECTOR_6: // 2
-			T1 = Y;
-			T2 = -X;		
-			break;
-		default:
-			break;
-	}
-	ModuleTime(&T1, &T2, PWM_half_period);
-  /*	
-	u32 ta = (PWM_half_period - T1 - T2) / 2;
-	u32 tb = ta + T1 ;
-	u32 tc = tb + T2 ; */
-	switch(sector) {
-		case SECTOR_1: // 3
-			A_duty = (PWM_half_period - T1 - T2) / 2;
-			B_duty = A_duty + T1;
-			C_duty = B_duty + T2;
-
-			low = C_duty;
-			midle = B_duty;
-			high = A_duty;
-			break;		
-		case SECTOR_2: // 1
-			B_duty = (PWM_half_period - T1 - T2) / 2;
-			A_duty = B_duty + T1;
-			C_duty = A_duty + T2;
-
-			low = C_duty;
-			midle = A_duty;
-			high = B_duty;			
-			break;
-		case SECTOR_3: // 5
-			B_duty = (PWM_half_period - T1 - T2) / 2;
-			C_duty = B_duty + T1;
-			A_duty = C_duty + T2;
-
-			low = A_duty;
-			midle = C_duty;
-			high = B_duty;			
-			break;
-		case SECTOR_4: // 4
-			C_duty = (PWM_half_period - T1 - T2) / 2;
-			B_duty = C_duty + T1;
-			A_duty = B_duty + T2;
-
-			low = A_duty;
-			midle = B_duty;
-			high = C_duty;			
-			break;
-		case SECTOR_5: // 6
-			C_duty = (PWM_half_period - T1 - T2) / 2;
-			A_duty = C_duty + T1;
-			B_duty = A_duty + T2;
-
-			low = B_duty;
-			midle = A_duty;
-			high = C_duty;			
-			break;			
-		case SECTOR_6: // 2
-			A_duty = (PWM_half_period - T1 - T2) / 2;
-			C_duty = A_duty + T1;
-			B_duty = C_duty + T2;
-
-			low = B_duty;
-			midle = C_duty;
-			high = A_duty;			
-			break;
-		default:
-			break;
-	}
-	
-	phase_out->A = A_duty;
-	phase_out->B = B_duty;
-	phase_out->C = C_duty;
-	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;", A_duty, B_duty, C_duty);
-	}
-#endif		
-//	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
-}
-
 /* 7段式SVPWM
  * 返回设置3相PWM的3个CCR寄存器的值
  * 这里使用的是stm32的PWM mode1,在向上计数时，一旦TIMx_CNT<TIMx_CCR1时通道1为有效电平，否则为无效电平
@@ -555,160 +146,3 @@ void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_perio
 //	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
 }
 
-
-#if 0
-
-void XYZ_step(void)
-{
-  real_T rtb_Product1;
-  real_T rtb_Product2;
-  real_T rtb_Product3;
-
-  /* Product: '<S1>/Product' incorporates:
-   *  Inport: '<Root>/Ts'
-   *  Inport: '<Root>/Udc'
-   *  Math: '<S1>/Math Function'
-   *
-   * About '<S1>/Math Function':
-   *  Operator: reciprocal
-   */
-  rtb_Product1 = 1.0 / rtU.Udc * rtU.Ts;
-
-  /* Gain: '<S1>/Gain2' incorporates:
-   *  Inport: '<Root>/Ubeta'
-   */
-  rtb_Product2 = 0.8660254037844386 * rtU.Ubeta;
-
-  /* Gain: '<S1>/Gain' incorporates:
-   *  Inport: '<Root>/Ualpha'
-   */
-  rtb_Product3 = 1.5 * rtU.Ualpha;
-
-  /* Outport: '<Root>/XYZ' incorporates:
-   *  Gain: '<S1>/Gain1'
-   *  Inport: '<Root>/Ubeta'
-   *  Product: '<S1>/Product1'
-   *  Product: '<S1>/Product2'
-   *  Product: '<S1>/Product3'
-   *  Sum: '<S1>/Add'
-   *  Sum: '<S1>/Add1'
-   */
-  rtY.XYZ_d[2] = (rtb_Product2 - rtb_Product3) * rtb_Product1;
-  rtY.XYZ_d[1] = (rtb_Product2 + rtb_Product3) * rtb_Product1;
-  rtY.XYZ_d[0] = 1.7320508075688772 * rtU.Ubeta * rtb_Product1;
-}
-
-
-
-
-/* Model step function */
-void T1T2_step(void)
-{
-  real_T rtb_Subtract;
-  real_T rtb_T1;
-  real_T rtb_T2;
-
-  /* MultiPortSwitch: '<S1>/Multiport Switch' incorporates:
-   *  Gain: '<S1>/Gain'
-   *  Gain: '<S1>/Gain1'
-   *  Gain: '<S1>/Gain2'
-   *  Inport: '<Root>/N'
-   *  Inport: '<Root>/XYZ'
-   */
-  switch ((int32_T)rtU.N) {
-   case 1:
-    rtb_T1 = rtU.XYZ[2];
-
-    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
-     *  Inport: '<Root>/XYZ'
-     */
-    rtb_T2 = rtU.XYZ[1];
-    break;
-
-   case 2:
-    rtb_T1 = rtU.XYZ[1];
-
-    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
-     *  Gain: '<S1>/Gain'
-     *  Inport: '<Root>/XYZ'
-     */
-    rtb_T2 = -rtU.XYZ[0];
-    break;
-
-   case 3:
-    rtb_T1 = -rtU.XYZ[2];
-
-    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
-     *  Gain: '<S1>/Gain2'
-     *  Inport: '<Root>/XYZ'
-     */
-    rtb_T2 = rtU.XYZ[0];
-    break;
-
-   case 4:
-    rtb_T1 = -rtU.XYZ[0];
-
-    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
-     *  Gain: '<S1>/Gain'
-     *  Inport: '<Root>/XYZ'
-     */
-    rtb_T2 = rtU.XYZ[2];
-    break;
-
-   case 5:
-    rtb_T1 = rtU.XYZ[0];
-
-    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
-     *  Gain: '<S1>/Gain1'
-     *  Inport: '<Root>/XYZ'
-     */
-    rtb_T2 = -rtU.XYZ[1];
-    break;
-
-   default:
-    rtb_T1 = -rtU.XYZ[1];
-
-    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
-     *  Gain: '<S1>/Gain1'
-     *  Gain: '<S1>/Gain2'
-     *  Inport: '<Root>/XYZ'
-     */
-    rtb_T2 = -rtU.XYZ[2];
-    break;
-  }
-
-  /* End of MultiPortSwitch: '<S1>/Multiport Switch' */
-
-  /* Sum: '<S1>/Subtract' */
-  rtb_Subtract = rtb_T1 + rtb_T2;
-
-  /* Switch: '<S1>/Switch' incorporates:
-   *  Inport: '<Root>/Tpwm'
-   *  Sum: '<S1>/Subtract1'
-   *  Switch: '<S1>/Switch1'
-   */
-  if (rtU.Tpwm - rtb_Subtract > 0.0) {
-    /* Outport: '<Root>/T1 ' */
-    rtY.T1 = rtb_T1;
-
-    /* Outport: '<Root>/T2' */
-    rtY.T2 = rtb_T2;
-  } else {
-    /* Outport: '<Root>/T1 ' incorporates:
-     *  Product: '<S1>/Divide'
-     *  Product: '<S1>/Product'
-     */
-    rtY.T1 = rtb_T1 * rtU.Tpwm / rtb_Subtract;
-
-    /* Outport: '<Root>/T2' incorporates:
-     *  Product: '<S1>/Divide1'
-     *  Product: '<S1>/Product1'
-     */
-    rtY.T2 = 1.0 / rtb_Subtract * (rtb_T2 * rtU.Tpwm);
-  }
-
-  /* End of Switch: '<S1>/Switch' */
-}
-
-#endif
-

Project/MC100_OS.uvoptx

@@ -196,7 +196,7 @@
 
   <Group>
     <GroupName>Application</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -240,7 +240,7 @@
 
   <Group>
     <GroupName>Foc</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -652,7 +652,7 @@
 
   <Group>
     <GroupName>BSP</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1016,7 +1016,7 @@
 
   <Group>
     <GroupName>StartUp</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>