.gitignore

@@ -11,6 +11,7 @@ JLinkSettings.ini
 version.txt
 JLinkLog.txt
 *.uvguix.*
+*.uvoptx
 version.h
 sw_build_info.h
 *.IAB

Applications/app/app.c

@@ -49,28 +49,97 @@ void app_start(void){
 	shark_task_run();
 }
 
+#if 0
+static void test_motor_temp(void) {
+	static s16 temp = 100;
+	static bool add = true;
+	sys_debug("mot: %d->%f\n", temp, motor_temp_high_limit_test(temp));
+	if (temp > 90 && temp != 300) {
+		temp += add?1:-1;
+		if (temp == 133) {
+			add = false;
+		}
+	}else {
+		temp = 300;
+	}
+}
+
+static void test_mos_temp(void) {
+	static s16 temp = 88;
+	static bool add = true;
+	sys_debug("mos: %d->%f\n", temp, mos_temp_high_limit_test(temp));
+	if (temp > 85 && temp != -40) {
+		temp += add?1:-1;
+		if (temp == 123) {
+			add = false;
+		}
+	}else {
+		temp = -40;
+	}
+
+}
+
+static void test_vbus_vol(void) {
+	static s16 vol = 65;
+	static bool add = false;
+	sys_debug("vbus: %d->%d\n", vol, vbus_voltage_low_limit_test(vol));
+	vol += add?1:-1;
+	if (vol < 40) {
+		add = true;
+	}else if (vol >= 65) {
+		add = false;
+	}
+}
+
+#endif
+static bool expf_b = false;
+static float y1[101], x1[101];
+static int exp_idx = 101;
+static measure_time_t exp_m;
+static void expf_benchmark(void) {
+	float thro_curve = 0.0f;
+	float K = -1.5f;
+	u32 mask = cpu_enter_critical();
+	time_measure_start(&exp_m);
+	for (int i = 0; i < 101; i++) {
+		y1[i] = throttle_curve(K, thro_curve);
+		x1[i] = thro_curve;
+		thro_curve += 0.01f;
+	}
+	time_measure_end(&exp_m);
+	cpu_exit_critical(mask);
+	sys_debug("time = %d, %f, %f\n", exp_m.exec_time, y1[0], y1[100]);
+}
+
 static void app_print_log(void) {
 	//sys_debug("Slow: %d - %d, err:%d, %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time, g_meas_MCTask.exec_max_error_time, g_meas_MCTask.exec_time_error);
-	//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
-	//sys_debug("FOC time err %d %d %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_error);
-	//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
+	sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
+	sys_debug("FOC time err %d %d %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_error);
+	sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
 	//sys_debug("throttle %f\n", get_throttle_float());
-	//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
-	//sys_debug("dead time %d\n", get_deadtime());
+	sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
+	//sys_debug("dead time 0x%x\n", get_deadtime());
 	//thro_torque_log();
 	//sys_debug("_>%f, %f, %f\n", ladrc_observer_get()->ld, ladrc_observer_get()->lq, ladrc_observer_get()->poles);
-	encoder_log();
+	//encoder_log();
 	//motor_debug();
 	//sample_log();
 	//throttle_log();
-	sys_debug("Trq: %f-%f-%f\n", motor.controller.input_torque.target, motor.controller.input_torque.interpolation, motor.controller.input_torque.step);
-	sys_debug("acc %d\n", get_acc_vol());
+	//sys_debug("Trq: %f-%f-%f\n", motor.controller.ramp_input_torque.target, motor.controller.ramp_input_torque.interpolation, motor.controller.ramp_input_torque.step);
+	sys_debug("FOC is %s, %d,%d\n", mot_contrl_is_start(mot_contrl())?"start":"stop", motor.foc_start_cnt, motor.foc_stop_cnt);
+	sys_debug("VelLim Ki=%f, %f\n", mot_contrl()->pi_vel_lim.ki, mot_contrl()->pi_vel_lim.kp);
 	//F_debug();
 	//eCtrl_debug_log();
 	//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
 	//mc_err_code_log();
 	//sys_debug("=====\n");
-
+	//test_mos_temp();
+	//test_mos_temp();
+	//test_vbus_vol();
+	if (expf_b) {
+		expf_benchmark();
+		expf_b = false;
+	}
 }
 
 static u32 app_report_task(void *p) {
@@ -106,32 +175,39 @@ static u32 app_report_task(void *p) {
 	if (++loop % 10 == 0) {
 		app_print_log();
 	}
+	if (!expf_b && exp_idx < 101) {
+		can_plot2((s16)(y1[exp_idx] * 100.0f), (s16)(x1[exp_idx] * 100.0f));
+		exp_idx++;
+	}
+
 	return 200;
 }
 int plot_type = 0;
 static void plot_smo_angle(void) {
+	u32 mask = cpu_enter_critical();
 	float smo_angle = foc_observer_sensorless_angle();
-	float delta = smo_angle - foc()->in.mot_angle;
+	float mot_angle = foc()->in.mot_angle;
+	cpu_exit_critical(mask);
+	float delta = smo_angle - mot_angle;
 	float s, c;
 	arm_sin_cos(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
-	can_plot3(foc()->in.mot_angle, smo_angle, delta);
+	can_plot3(mot_angle, smo_angle, delta);
 }
 static u32 app_plot_task(void * args) {
 	if (plot_type == 1) {
-		s16 plot_arg1 = (s16)foc_observer_sensorless_speed();
+		s16 plot_arg1 = mot_contrl()->ramp_vel_lim.interpolation;
 		s16 plot_arg2 = (s16)mot_contrl_get_speed(&motor.controller);
 		if (mot_contrl()->mode_running == CTRL_MODE_SPD) {
 			if (mc_is_cruise_enabled()) {
-				plot_arg1 = mot_contrl()->cruise_vel.target;
+				plot_arg1 = mot_contrl()->ramp_cruise_vel.target;
 			}else {
-				plot_arg1 = mot_contrl()->target_vel.target;
+				plot_arg1 = mot_contrl()->ramp_target_vel.target;
 			}
-		}else {
-			can_plot2(plot_arg1, plot_arg2);
 		}
+		can_plot3(plot_arg1, plot_arg2, mot_contrl()->ramp_vel_lim.target);
 	}else if (plot_type == 2) {
-		can_plot2(mot_contrl_get_final_torque(&motor.controller), mot_contrl()->target_torque);
+		can_plot3(mot_contrl()->ramp_torque_lim.interpolation, mot_contrl()->ramp_input_torque.interpolation, mot_contrl()->target_torque);
 	}else if (plot_type == 3) {
 		plot_smo_angle();
 	}else if (plot_type == 4) {
@@ -149,7 +225,16 @@ static u32 app_plot_task(void * args) {
 		s16 thro2_v = get_throttle2_float() * 100.0f;
 		s16 sig = throttle_get_signal();
 		can_plot3(sig, thro_v, thro2_v);
+	}else if (plot_type == 10) {
+		s16 duty = (s16)(motor.controller.duty_raw * 100);
+		s16 duty_filterd = (s16)(motor.controller.duty_filterd * 100);
+		can_plot2(duty, duty_filterd);
+	}else if (plot_type == 11) {
+		//do it in other place, phase voltage
+	}else if (plot_type == 12) {
+		can_plot2((s16)motor.controller.dc_curr_filted, (s16)motor.controller.dc_curr_calc);
 	}
+
 	return 20;
 }
 static u32 app_low_task(void *args) {

Applications/app/factory.c

@@ -4,6 +4,7 @@
 #include "prot/can_foc_msg.h"
 #include "foc/samples.h"
 #include "foc/motor/current.h"
+#include "foc/motor/motor.h"
 #include "libs/utils.h"
 #include "libs/logger.h"
 #include "os/os_task.h"
@@ -11,27 +12,37 @@
 static u8 factory_mode = 0;
 
 static void stop_pwm_adc(void);
-
+static bool phase_adc_start = false;
 static bool start_pwm_adc(void) {
+	if (phase_adc_start) {
+		return true;
+	}
 	pwm_turn_on_low_side();
 	delay_ms(10);
 	phase_current_offset_calibrate();
 	pwm_start();
-	delay_us(10); //wait for ebrake error
+	delay_us(10);
+	get_motor()->b_start = true;
 	adc_start_convert();
 	phase_current_calibrate_wait();
 	if (phase_curr_offset_check()) {
 		stop_pwm_adc();
 		return false;
 	}
+	phase_adc_start = true;
 	return true;
 }
 
 static void stop_pwm_adc(void) {
+	if (!phase_adc_start) {
+		return;
+	}
+	phase_adc_start = false;
 	u32 mask = cpu_enter_critical();
 	adc_stop_convert();
 	pwm_stop();
 	pwm_up_enable(true);
+	get_motor()->b_start = false;
 	cpu_exit_critical(mask);
 }
 
@@ -47,57 +58,67 @@ void can_process_factory_message(can_message_t *can_message){
 			break;
 		case BUILD_CMD_KEY(0xE1):
 		{
-			if (factory_mode == 0) {
+			if (!factory_is_running()) {
 				response[2] = 1;
 				break;
 			}
 			u8 item = decode_u8(can_message->data);
 			if (item == 1) { //3相驱动测试
-				u8 mask = decode_u8((u8 *)can_message->data + 1);
-				pwm_3phase_sides(false, mask);
+				u8 duty = decode_u8((u8 *)can_message->data + 1);
+				if (duty != 0) {
+					pwm_3phase_test();
+					pwm_start();
+					u16 duty_time = (u16)((float)duty * FOC_PWM_Half_Period / 100.0f);
+					pwm_update_duty(duty_time, duty_time, duty_time);
+				}else {
+					pwm_stop();
+					pwm_3phase_init();
+				}
+				sys_debug("phase test duty %d\n", duty);
 			}else if (item == 2) {//获取所有电压的采集值
 				can_response_vols(can_message->src, can_message->key);
 				return;
 			}else if (item == 3) { //读取gpio状态
-				encode_u16(response + 3, GPIOA_VALUE);
-				encode_u16(response + 5, GPIOB_VALUE);
-				encode_u16(response + 7, GPIOC_VALUE);
-				encode_u16(response + 9, GPIOD_VALUE);
-				encode_u16(response + 11, GPIOE_VALUE);
-				rsplen += 10;
+				encode_u16(response + 3, gpio_get_pin_values());
+				rsplen += 2;
 			}else if (item == 4) { // u phase detect
-				int count = 20;
+				int count = 200;
 				float uvw[3] = {0, 0, 0};
 				s16 uvw_total[3] = {0, 0, 0};
-				if (can_message->len < 2) {
-					response[2] = 2; //长度错误
-					break;
-				}
 				u8 detect = decode_u8((u8 *)can_message->data + 1);
 				gpio_phase_u_detect(detect?true:false);
+				delay_ms(50);
 				while(count-- > 0) {
 					delay_us(100);
 					get_uvw_phases_raw(uvw);
-					uvw_total[0] += (s16)(uvw[0] * 100);
-					uvw_total[1] += (s16)(uvw[1] * 100);
-					uvw_total[2] += (s16)(uvw[2] * 100);
+					uvw_total[0] += S16Q5(uvw[0]);
+					uvw_total[1] += S16Q5(uvw[1]);
+					uvw_total[2] += S16Q5(uvw[2]);
 				}
-				encode_u16(response + 3, uvw_total[0]/20);
-				encode_u16(response + 5, uvw_total[1]/20);
-				encode_u16(response + 7, uvw_total[2]/20);
+				encode_s16(response + 3, uvw_total[0]/200);
+				encode_s16(response + 5, uvw_total[1]/200);
+				encode_s16(response + 7, uvw_total[2]/200);
 				gpio_phase_u_detect(false);
 				rsplen += 6;
 			}else if (item == 5) { //phase current test
 				u8 start = decode_u8((u8 *)can_message->data + 1);
-				if (start) {
-					pwm_3phase_sides(true, 0); //use pwm output, disable timer break in
+				if (start == 1) {
+					pwm_3phase_test(); //use pwm output, disable timer break in
 					if (!start_pwm_adc()) {
 						response[2] = 1;
 						break;
 					}
-				}else {
+				}else if (start == 0){
 					stop_pwm_adc();
 					pwm_3phase_init();
+				}else {
+					s16 ia = S16Q5(foc()->in.curr_abc[0]);
+					s16 ib = S16Q5(foc()->in.curr_abc[1]);
+					s16 ic = S16Q5(foc()->in.curr_abc[2]);
+					encode_s16(response + 3, ia);
+					encode_s16(response + 5, ib);
+					encode_s16(response + 7, ic);
+					rsplen += 6;
 				}
 			}
 			break;
@@ -111,11 +132,7 @@ void can_process_factory_message(can_message_t *can_message){
 	}
 }
 
-u8 factory_get_mode(void) {
-	return factory_mode;
-}
-
 bool factory_is_running(void) {
-	return (factory_mode == 0);
+	return (factory_mode == 0x5A);
 }
 

Applications/bsp/bsp.h

@@ -11,7 +11,7 @@
 #define UART_IRQ_PRIORITY      6 
 #define ENC_TIMER_IRQ_PRIORITY 2
 #define ENC_PWM_IRQ_PRIORITY 2
-#define ENC_I_EXIT_IRQ_PRIORITY 0
+#define ENC_I_EXIT_IRQ_PRIORITY 4
 #define ENC_OTHER_IRQ_PRIORITY 8
 
 #define THREE_SHUNTS_SAMPLE 1

Applications/bsp/gd32/adc.c

@@ -33,7 +33,7 @@
 #define THROTTLE_BUFF_IDX 4
 #define VBUS_I_BUFF_IDX 5
 
-#define THROTTLE2_BUFF_IDX 6
+#define THROTTLE2_BUFF_IDX 14
 #define V_VOL_BUFF_IDX 7
 
 //zero chan            8
@@ -45,7 +45,7 @@
 #define THROTTLE2_5V_BUFF_IDX 12
 #define THROTTLE_5V_BUFF_IDX 13
 
-#define U_VOL_BUFF_IDX 14
+#define U_VOL_BUFF_IDX 6
 //zero chan            15
 
 //zero chan            16
@@ -66,7 +66,6 @@ static void adc01_dma_init(void)
 {
     dma_parameter_struct dma_init_struct;
     rcu_periph_clock_enable(RCU_DMA0);
-
     dma_deinit(DMA0, DMA_CH0);
     dma_init_struct.direction    = DMA_PERIPHERAL_TO_MEMORY;
     dma_init_struct.memory_addr  = (uint32_t)adc_buffer;
@@ -85,14 +84,7 @@ static void adc01_dma_init(void)
 }
 
 
-static void adc01_init(void) {
-    /* config ADC clock */
-    rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV4); //APB2 clk 120M, adc clk 30M
-	rcu_periph_clock_enable(RCU_ADC0);
-	rcu_periph_clock_enable(RCU_ADC1);
-	adc_deinit(ADC0);
-	adc_deinit(ADC1);
-
+static void adc01_init(u32 reg_sampletime) {
 	/* config work mode */
     adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE);
     adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE);
@@ -107,46 +99,46 @@ static void adc01_init(void) {
 	
 #if (CONFIG_MC105_HW_VERSION==2)
 	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, ADC01_NUM);
-	adc_regular_channel_config(ADC0, 0, MOS_TEMP_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 1, MOS_TEMP1_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 2, U_VOL_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 3, V_VOL_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 4, W_VOL_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_17, ADC_REGCHAN_SAMPLE_TIME);//3.3vref
+	adc_regular_channel_config(ADC0, 0, MOS_TEMP_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 1, MOS_TEMP1_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 2, U_VOL_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 3, V_VOL_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 4, W_VOL_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_17, reg_sampletime);//3.3vref
 
 	adc_tempsensor_vrefint_enable();
 
 	adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, ADC01_NUM);
-	adc_regular_channel_config(ADC1, 0, VBUS_V_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 1, ACC_V_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 2, VBUS_I_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 3, THROTTLE_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 4, ADC_CHANNEL_10, ADC_REGCHAN_SAMPLE_TIME); //dumy
-	adc_regular_channel_config(ADC1, 5, MOTOR_TEMP_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
+	adc_regular_channel_config(ADC1, 0, VBUS_V_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 1, ACC_V_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 2, VBUS_I_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 3, THROTTLE_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 4, ADC_CHANNEL_10, reg_sampletime); //dumy
+	adc_regular_channel_config(ADC1, 5, MOTOR_TEMP_ADC_CHAN, reg_sampletime);
 #else
 	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, ADC01_NUM);
-	adc_regular_channel_config(ADC0, 0, MOS_TEMP_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 1, MOTOR_TEMP_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 2, THROTTLE_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 3, THROTTLE2_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 4, ZERO_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
-	adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_17, ADC_REGCHAN_SAMPLE_TIME); //mcu内部vref
-	adc_regular_channel_config(ADC0, 6, THROTTLE2_5V_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 7, U_VOL_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC0, 8, ZERO_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
+	adc_regular_channel_config(ADC0, 0, MOS_TEMP_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 1, MOTOR_TEMP_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 2, THROTTLE_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 3, U_VOL_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 4, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
+	adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_17, reg_sampletime); //mcu内部vref
+	adc_regular_channel_config(ADC0, 6, THROTTLE2_5V_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 7, THROTTLE2_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC0, 8, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
 
 	adc_tempsensor_vrefint_enable();
 
 	adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, ADC01_NUM);
-	adc_regular_channel_config(ADC1, 0, VBUS_V_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 1, ACC_V_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 2, VBUS_I_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 3, V_VOL_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 4, W_VOL_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 5, ZERO_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
-	adc_regular_channel_config(ADC1, 6, THROTTLE_5V_CHAN, ADC_REGCHAN_SAMPLE_TIME);
-	adc_regular_channel_config(ADC1, 7, ZERO_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
-	adc_regular_channel_config(ADC1, 8, DC5V_ADC_CHAN, ADC_REGCHAN_SAMPLE_TIME);
+	adc_regular_channel_config(ADC1, 0, VBUS_V_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 1, ACC_V_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 2, VBUS_I_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 3, V_VOL_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 4, W_VOL_ADC_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 5, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
+	adc_regular_channel_config(ADC1, 6, THROTTLE_5V_CHAN, reg_sampletime);
+	adc_regular_channel_config(ADC1, 7, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
+	adc_regular_channel_config(ADC1, 8, DC5V_ADC_CHAN, reg_sampletime);
 #endif
 	adc_buffer[VREF_BUFF_IDX] = VREF_ADC_DATA; //1.21/3.3*4095
 
@@ -208,16 +200,16 @@ static void adc_gpio_init(void) {
 	rcu_periph_clock_enable(RCU_AF);
 		/* configure ADC pin, current sampling -- ADC_IN1(PA1) ADC_IN12(PC2) ADC_IN13(PC3) */
 #ifdef U_PHASE_ADC_GROUP
-		rcu_periph_clock_enable(U_PHASE_ADC_RCU);
-		gpio_init(U_PHASE_ADC_GROUP, U_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, U_PHASE_ADC_PIN);
+	rcu_periph_clock_enable(U_PHASE_ADC_RCU);
+	gpio_init(U_PHASE_ADC_GROUP, U_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, U_PHASE_ADC_PIN);
 #endif
 #ifdef V_PHASE_ADC_GROUP
-		rcu_periph_clock_enable(V_PHASE_ADC_RCU);
-		gpio_init(V_PHASE_ADC_GROUP, V_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, V_PHASE_ADC_PIN);
+	rcu_periph_clock_enable(V_PHASE_ADC_RCU);
+	gpio_init(V_PHASE_ADC_GROUP, V_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, V_PHASE_ADC_PIN);
 #endif
 #ifdef W_PHASE_ADC_GROUP
-		rcu_periph_clock_enable(W_PHASE_ADC_RCU);
-		gpio_init(W_PHASE_ADC_GROUP, W_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, W_PHASE_ADC_PIN);
+	rcu_periph_clock_enable(W_PHASE_ADC_RCU);
+	gpio_init(W_PHASE_ADC_GROUP, W_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, W_PHASE_ADC_PIN);
 #endif
 
 #ifdef VBUS_V_ADC_GROUP
@@ -297,10 +289,21 @@ static void adc_gpio_init(void) {
 #endif
 }
 
-void adc_init(void) {
+void adc_init(bool mot_ind) {
+	/* config ADC clock */
+    rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV4); //APB2 clk 120M, adc clk 30M
+	rcu_periph_clock_enable(RCU_ADC0);
+	rcu_periph_clock_enable(RCU_ADC1);
+	adc_deinit(ADC0);
+	adc_deinit(ADC1);
+
 	adc_gpio_init();
 	adc01_dma_init();
-	adc01_init();
+	if (!mot_ind) {
+		adc01_init(ADC_REGCHAN_SAMPLE_TIME);
+	}else {
+		adc01_init(ADC_REGCHAN_MOT_IND_SAMPLE_TIME);
+	}
 	adc_current_sample_config(0);
 }
 

Applications/bsp/gd32/adc.h

@@ -173,7 +173,7 @@ static __inline void adc_update_ext_trigger(u32 trigger) {
 	adc_external_trigger_source_config(ADC0, ADC_INSERTED_CHANNEL, trigger);
 }
 
-void adc_init(void);
+void adc_init(bool mot_ind);
 s32 adc_sample_regular_channel(int chan, int times);
 void adc_start_convert(void);
 void adc_stop_convert(void);

Applications/bsp/gd32/board_mc100_v1.h

@@ -6,7 +6,6 @@
 #include "gd32e10x.h"
 #endif
 
-#define CONFIG_HW_MAX_DC_VOLTAGE 115.0F
 #define CONFIG_HW_MAX_DC_CURRENT 250.0f
 #define CONFIG_HW_MAX_CHRG_CURRENT (-100.0f)
 #define CONFIG_HW_MAX_MOTOR_RPM      9000.0f
@@ -337,6 +336,8 @@
 
 #define CONFIG_ENC_FILTER_NR          12 //1100： fSAMP=fDTS/16, N=8,采样率 120M/16 = 7.5M
 #define CONFIG_PWM_FILTER_NR          12
+/* Z 信号宽度设计为4LSB，200hz机械转速，200K，Z脉冲宽度5us*4=20us, 按照timer滤波系数15算，1/3.75*8=2.1us采集到信号没有变才认为是有效值*/
+#define CONFIG_Z_IDX_FILTER_NR        15 //1111:  fSAMP=fDTS/32, N=8,采样率 120M/32 = 3.75M
 
 #ifdef CONFIG_PWM_UV_SWAP
 #define ENCODER_CC_INVERT 1

Applications/bsp/gd32/board_mc105_v3.h

@@ -6,7 +6,6 @@
 #include "gd32e10x.h"
 #endif
 
-#define CONFIG_HW_MAX_DC_VOLTAGE CONFIG_BOARD_MAX_DC
 #define CONFIG_HW_MAX_DC_CURRENT 250.0f
 #define CONFIG_HW_MAX_CHRG_CURRENT (-100.0f)
 #define CONFIG_HW_MAX_MOTOR_RPM      9000.0f
@@ -39,19 +38,20 @@
 #define SCHED_TIMER_IRQ TIMER5_IRQn
 #define SCHED_TIMER_IRQHandler TIMER5_IRQHandler
 
-#define PWM_DEAD_TIME_NS 400u
-#define PWM_TOFF_DELAY_MAX 240u
-#define PWM_TON_DELAY_MIN 200u
-#define HW_DEAD_TIME_NS  210u
+#define PWM_DEAD_TIME_NS 600u
+#define MOS_TOFF_DELAY_MAX 100u //from mos datasheet
+#define MOS_TON_DELAY_MIN 30u   //from mos datasheet
+#define MOSDRV_DEAD_TIME_NS  200u //from H bridge driver's datasheet max is 190
+#define MOS_MILE_VOL_DROP_NS 100u //from mos' mile
 #define HW_RISE_TIME_NS  150u
 #define HW_NOISE_TIME_NS 300u
 
-#define CONFIG_HW_DeadTime NS_2_TCLK(HW_DEAD_TIME_NS + PWM_DEAD_TIME_NS + (PWM_TOFF_DELAY_MAX - PWM_TON_DELAY_MIN))/* ����ʱ�� */
+#define CONFIG_HW_DeadTime NS_2_TCLK(PWM_DEAD_TIME_NS + MOSDRV_DEAD_TIME_NS + (MOS_TOFF_DELAY_MAX - MOS_TON_DELAY_MIN) + MOS_MILE_VOL_DROP_NS)/* ����ʱ�� */
 #define TRise NS_2_TCLK(HW_RISE_TIME_NS)/* MOS ����ʱ��*/
 #define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS��������Ŀ�������ʱ�� */
 #define TADC  ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) *2 * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */
-#define TSampleMIN (TDead + TRise + TADC) //采样需要的总时间
-#define TSampleBefore (TDead + TRise) //采样开始前需要等待的时间
+#define TSampleMIN (CONFIG_HW_DeadTime + TRise + TADC) //采样需要的总时间
+#define TSampleBefore (CONFIG_HW_DeadTime + TRise) //采样开始前需要等待的时间
 
 //#define CONFIG_START_LINE_DTC_CURRENT 5.0F /* 死区补偿开始电流，取决于电流噪声 */
 //#define CONFIG_END_LINE_DTC_CURRENT   15.0F
@@ -223,7 +223,14 @@
 #define W_VOL_ADC_PIN 	GPIO_PIN_2
 #define W_VOL_ADC_RCU 	RCU_GPIOA
 #define W_VOL_ADC_MODE 	GPIO_MODE_AIN
-#define UVW_VOL_CEOF (ADC_REFERENCE_VOLTAGE*(41.0f)/ADC_FULL_MAX)
+
+#define PHASE_VOL_R (40*1000.0f)
+#define PHASE_VOL_R1 (1*1000.0f)
+#define PHASE_VOL_C1 (470e-9f) //470nF
+#define PHASE_VOL_Gain  ((PHASE_VOL_R + PHASE_VOL_R1)/PHASE_VOL_R1)
+#define Phase_Vol_LPF_R  ((PHASE_VOL_R * PHASE_VOL_R1)/(PHASE_VOL_R + PHASE_VOL_R1))
+#define PHASE_VOL_LPF_BAND  (1/(2*3.14F*Phase_Vol_LPF_R*PHASE_VOL_C1))
+#define UVW_VOL_CEOF (ADC_REFERENCE_VOLTAGE*PHASE_VOL_Gain/ADC_FULL_MAX)
 
 /* 模拟5v电压采集 */
 #define DC5V_ADC_CHAN     ADC_CHANNEL_3 //adc012
@@ -378,9 +385,11 @@
 
 #define ENC_MAX_interpolation 1.0F
 
+/* 200hz机械转速，200K(12000RPM)，AB信号高低电平宽度5us/2=2.5us, 按照timer滤波系数12算，1/7.5*8=1.06us采集到信号没有变才认为是有效值*/
 #define CONFIG_ENC_FILTER_NR          12 //1100： fSAMP=fDTS/16, N=8,采样率 120M/16 = 7.5M
 #define CONFIG_PWM_FILTER_NR          12
-
+/* Z 信号宽度设计为4LSB，200hz机械转速，200K(12000RPM)，Z脉冲宽度5us*4=20us, 按照timer滤波系数15算，1/3.75*8=2.1us采集到信号没有变才认为是有效值*/
+#define CONFIG_Z_IDX_FILTER_NR        15///1111:  fSAMP=fDTS/32, N=8,采样率 120M/32 = 3.75M
 #ifdef CONFIG_PWM_UV_SWAP
 #define ENCODER_CC_INVERT 1
 #endif

Applications/bsp/gd32/bsp.h

@@ -20,6 +20,8 @@
 #define FOC_CTRL_US (1.0f/(float)FOC_PWM_FS)
 
 #define ADC_REGCHAN_SAMPLE_TIME ADC_SAMPLETIME_71POINT5
+#define ADC_REGCHAN_MOT_IND_SAMPLE_TIME ADC_SAMPLETIME_13POINT5
+
 #define ADC_TRIG_CONV_LATENCY_CYCLES 12.5f
 #define ADC_SAMPLING_CYCLES 13.5f
 
@@ -29,23 +31,27 @@
 #include "bsp/board_yuanqu.h"
 #elif defined (MC100_HW_V1)
 #define CONFIG_CURRENT_SENSOR_CEOF 0.32F
+#define CONFIG_BOARD_MAX_VOLTAGE 120.0f
+#define CONFIG_BOARD_MIN_VOLTAGE 50.0f
 #include "bsp/gd32/board_mc100_v1.h"
 #define CONFIG_BOARD_MCXXX
 #define CONFIG_BOARD_NAME "MC100"
 #define CONFIG_MC105_HW_VERSION 2
 
 #elif defined (CONFIG_MC105_HW_V3)
-#define CONFIG_BOARD_MAX_DC 115.0F
 #define CONFIG_VBUS_I_POSITIVE 1
 #define CONFIG_CURRENT_SENSOR_CEOF 0.303F
+#define CONFIG_BOARD_MAX_VOLTAGE 120.0f
+#define CONFIG_BOARD_MIN_VOLTAGE 50.0f
 #include "bsp/gd32/board_mc105_v3.h"
 #define CONFIG_BOARD_MCXXX
 #define CONFIG_BOARD_NAME "MC105"
 #define CONFIG_MC105_HW_VERSION 3
 
 #elif defined (CONFIG_BOARD_MC124)
-#define CONFIG_BOARD_MAX_DC 84.0F
-#define CONFIG_CURRENT_SENSOR_CEOF 0.36F
+#define CONFIG_CURRENT_SENSOR_CEOF 0.313F
+#define CONFIG_BOARD_MAX_VOLTAGE 90.0f
+#define CONFIG_BOARD_MIN_VOLTAGE 40.0f
 #include "bsp/gd32/board_mc105_v3.h"
 #define CONFIG_BOARD_MCXXX
 #define CONFIG_BOARD_NAME "MCXXX"

Applications/bsp/gd32/enc_intf.c

@@ -69,15 +69,25 @@ void enc_intf_quadrature_init(u32 rate) {
 	timer_input_capture_config(timer,TIMER_CH_1,&timer_icinitpara);
 
 	timer_quadrature_decoder_mode_config(timer ,TIMER_ENCODER_MODE2,TIMER_IC_POLARITY_FALLING, TIMER_IC_POLARITY_RISING);
-	
+
+	/* TIMER CH2 PWM input capture configuration */
+	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
+	timer_icinitpara.icselection = TIMER_IC_SELECTION_ITS;
+	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
+	timer_icinitpara.icfilter	 = 0;
+	timer_input_capture_config(timer, TIMER_CH_2, &timer_icinitpara);
+	timer_input_trigger_source_select(timer, TIMER_SMCFG_TRGSEL_ITI3);//come from trigger out of time3 which captrue the Z singnal and output to me
+
 	/* auto-reload preload enable */
 	timer_auto_reload_shadow_enable(timer);	
 
-	
 	timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_UP);
 
-	//timer_interrupt_enable(timer, TIMER_INT_UP);
-	//nvic_irq_enable(ENC_TIMER_IRQ, ENC_TIMER_IRQ_PRIORITY, 0);
+    /* clear channel 0 interrupt bit */
+    timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_CH2);
+    /* channel 0 interrupt enable */
+    timer_interrupt_enable(timer, TIMER_INT_CH2);
+	nvic_irq_enable(ENC_TIMER_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0);
 
 	/* TIMER2 counter enable */
 	timer_enable(timer);
@@ -141,7 +151,7 @@ void enc_intf_z_counter(void) {
 
 	/* TIMER configuration */
 	timer_struct_para_init(&timer_initpara);
-	timer_initpara.prescaler		= TIM_CLOCK/PWM_TIME_CLK - 1;
+	timer_initpara.prescaler		= 0;
 	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
 	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
 	timer_initpara.period		   = 65535;
@@ -154,23 +164,23 @@ void enc_intf_z_counter(void) {
 	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
 	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
 	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
-	timer_icinitpara.icfilter	 = CONFIG_PWM_FILTER_NR;
+	timer_icinitpara.icfilter	 = CONFIG_Z_IDX_FILTER_NR;
 	timer_input_capture_config(timer, ENC_I_TIMER_CHAN, &timer_icinitpara);
 
-	/* slave mode selection: TIMER */
-	timer_input_trigger_source_select(timer, TIMER_SMCFG_TRGSEL_CI0FE0);
-	timer_slave_mode_select(timer, TIMER_SLAVE_MODE_RESTART);
-
 	/* select the master slave mode */
-	timer_master_slave_mode_config(timer, TIMER_MASTER_SLAVE_MODE_ENABLE);
+	timer_master_slave_mode_config(timer,TIMER_MASTER_SLAVE_MODE_ENABLE);
+	/* TIMER1 update event is used as trigger output */
+	timer_master_output_trigger_source_select(timer,TIMER_TRI_OUT_SRC_CH0); //ch0 as tirgger out to encoder timer
 
     /* auto-reload preload enable */
     timer_auto_reload_shadow_enable(timer);
-    /* clear channel 0 interrupt bit */
+#if 0
+	/* clear channel 0 interrupt bit */
     timer_interrupt_flag_clear(timer, ENC_I_TIMER_INT_FLG);
     /* channel 0 interrupt enable */
     timer_interrupt_enable(timer, ENC_I_TIMER_IRQ_CH);
 	nvic_irq_enable(ENC_I_TIMER_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0);
+#endif
     /* TIMER2 counter enable */
     timer_enable(timer);
 #endif
@@ -180,21 +190,26 @@ __weak void ENC_TIMER_Overflow(void) {
 
 }
 
+__weak void ENC_ABI_IRQHandler(u32 count) {
+
+}
+
+
 void ENC_TIMER_IRQHandler(void) {
 	if (SET == timer_interrupt_flag_get(ENC_TIMER, TIMER_INT_FLAG_UP)) {
 		timer_interrupt_flag_clear(ENC_TIMER, TIMER_INT_FLAG_UP);
 		ENC_TIMER_Overflow();
 	}
+	if (SET == timer_interrupt_flag_get(ENC_TIMER, TIMER_INT_CH2)) {
+		timer_interrupt_flag_clear(ENC_TIMER, TIMER_INT_FLAG_CH2);
+		ENC_ABI_IRQHandler(TIMER_CH2CV(ENC_TIMER));
+	}
 }
 
 
-__weak void ENC_ABI_IRQHandler(void) {
-
-}
-
 void ABI_I_IRQHandler(void) {
 #ifdef ENC_I_IRQ
-	ENC_ABI_IRQHandler();
+	ENC_ABI_IRQHandler(TIMER_CNT(ENC_TIMER));
 #endif
 }
 
@@ -203,7 +218,9 @@ void ENC_I_IRQHandler(void) {
     if(SET == timer_interrupt_flag_get(ENC_I_TIMER, ENC_I_TIMER_INT_FLG)){
         /* clear channel 0 interrupt bit */
         timer_interrupt_flag_clear(ENC_I_TIMER, ENC_I_TIMER_INT_FLG);
-		ENC_ABI_IRQHandler();
+#if 0
+		ENC_ABI_IRQHandler(TIMER_CNT(ENC_TIMER));
+#endif
     }
 #endif
 }

Applications/bsp/gd32/gpio.c

@@ -229,7 +229,25 @@ bool gpio_motor_locked(void) {
 #endif
 }
 
+u16 gpio_get_pin_values(void) {
+	u32 value = gpio_input_bit_get(GPIO_MLOCK_IN_GROUP, GPIO_MLOCK_IN_PIN) == SET?1:0;
+	value |= (gpio_input_bit_get(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_PIN) == SET?1:0)<<1;
+#ifdef REPEAR_IN_GROUP
+	value |= (gpio_input_bit_get(REPEAR_IN_GROUP, REPEAR_IN_PIN) == SET?1:0) << 2;
+#endif
+#ifdef BOOT_PIN_0_GROUP
+	value |= (gpio_input_bit_get(BOOT_PIN_0_GROUP, BOOT_PIN_0_PIN) == SET?:0) << 3;
+#endif
+#ifdef BOOT_PIN_1_GROUP
+	value |= (gpio_input_bit_get(BOOT_PIN_1_GROUP, BOOT_PIN_1_PIN) == SET?:0) << 4;
+#endif
+	value |= (gpio_input_bit_get(ENC_A_GROUP, ENC_A_PIN) == SET?:0) << 5;
+	value |= (gpio_input_bit_get(ENC_B_GROUP, ENC_B_PIN) == SET?:0) << 6;
+	value |= (gpio_input_bit_get(ENC_I_GROUP, ENC_I_PIN) == SET?:0) << 7;
+	value |= (gpio_input_bit_get(ENC_PWM_GROUP, ENC_PWM_PIN) == SET?:0) << 8;
 
+	return value;
+}
 
 void gpio_ir2136_enable(bool enable) {
 #ifdef GD32_FOC_DEMO

Applications/bsp/gd32/gpio.h

@@ -39,5 +39,6 @@ void gpio_led_enable(bool enable);
 void gpio_brk_light_enable(bool enable);
 u8  gpio_board_id(void);
 bool gpio_is_repear_mode(void);
+u16 gpio_get_pin_values(void);
 
 #endif /* _GPIO_PIN_H__ */

Applications/bsp/gd32/pwm.c

@@ -37,56 +37,19 @@ static rcu_periph_enum _rcu_clk(u32 timer) {
     return RCU_TIMER2;      
 }
 
+static bool phase_start = false;
 void pwm_3phase_init(void){
 	_pwm_gpio_config();
     _init_pwm_timer(true);
+	phase_start = false;
 }
 
-void pwm_3phase_sides(bool pwm, u8 mask) {
-	timer_deinit(MOS_PWM_TIMER);
-	if (pwm) {
+void pwm_3phase_test(void) {
+	if (!phase_start) {
+		timer_deinit(MOS_PWM_TIMER);
 		_pwm_gpio_config();
 		_init_pwm_timer(false);
-		return;
-	}
-	rcu_periph_clock_enable(_rcu_clk(MOS_PWM_TIMER));
-    gpio_init(PWM_U_P_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_U_P_PIN);
-    gpio_init(PWM_V_P_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_V_P_PIN);
-    gpio_init(PWM_W_P_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_W_P_PIN);
-
-    gpio_init(PWM_U_N_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_U_N_PIN);
-    gpio_init(PWM_V_N_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_V_N_PIN);
-    gpio_init(PWM_W_N_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_W_N_PIN);
-
-	sys_debug("pwm_3phase_sides\n");
-	gpio_bit_write(PWM_U_P_GROUP, PWM_U_P_PIN, RESET);
-	gpio_bit_write(PWM_V_P_GROUP, PWM_V_P_PIN, RESET);
-	gpio_bit_write(PWM_W_P_GROUP, PWM_W_P_PIN, RESET);
-	gpio_bit_write(PWM_U_N_GROUP, PWM_U_N_PIN, RESET);
-	gpio_bit_write(PWM_V_N_GROUP, PWM_V_N_PIN, RESET);
-	gpio_bit_write(PWM_W_N_GROUP, PWM_W_N_PIN, RESET);
-	delay_us(1);
-	
-	if (mask & 0x01) {
-		gpio_bit_write(PWM_U_P_GROUP, PWM_U_P_PIN, SET);
-		gpio_bit_write(PWM_U_N_GROUP, PWM_U_N_PIN, RESET);
-	}else {
-		gpio_bit_write(PWM_U_P_GROUP, PWM_U_P_PIN, RESET);
-		gpio_bit_write(PWM_U_N_GROUP, PWM_U_N_PIN, SET);
-	}
-	if (mask & 0x02) {
-		gpio_bit_write(PWM_V_P_GROUP, PWM_V_P_PIN, SET);
-		gpio_bit_write(PWM_V_N_GROUP, PWM_V_N_PIN, RESET);
-	}else {
-		gpio_bit_write(PWM_V_P_GROUP, PWM_V_P_PIN, RESET);
-		gpio_bit_write(PWM_V_N_GROUP, PWM_V_N_PIN, SET);
-	}
-	if (mask & 0x04) {
-		gpio_bit_write(PWM_W_P_GROUP, PWM_W_P_PIN, SET);
-		gpio_bit_write(PWM_W_N_GROUP, PWM_W_N_PIN, RESET);
-	}else {
-		gpio_bit_write(PWM_W_P_GROUP, PWM_W_P_PIN, RESET);
-		gpio_bit_write(PWM_W_N_GROUP, PWM_W_N_PIN, SET);
+		phase_start = true;
 	}
 }
 

Applications/bsp/gd32/pwm.h

@@ -90,7 +90,7 @@
 #define PWM_Direction_Down() true
 #endif
 void pwm_3phase_init(void);
-void pwm_3phase_sides(bool pwm, u8 mask);
+void pwm_3phase_test(void);
 void pwm_start(void);
 void pwm_stop(void);
 void pwm_turn_on_low_side(void);

Applications/foc/commands.c

@@ -136,7 +136,7 @@ static void process_ext_command(foc_cmd_body_t *command) {
 	
 }
 
-static u8 ignore_with_speed[] = {Foc_Set_Adrc_Params, Foc_Set_Gear_Limit, Foc_Conf_Pid, Foc_Set_Throttle_throld, Foc_Set_Config, Foc_Set_eBrake_Throld, Foc_Set_Limiter_Config, Foc_Write_Config, Foc_SN_Write};
+static u8 ignore_with_speed[] = {Foc_Set_Adrc_Params, Foc_Set_Gear_Limit, Foc_Set_Throttle_throld, Foc_Set_Config, Foc_Set_eBrake_Throld, Foc_Set_Limiter_Config, Foc_Write_Config, Foc_SN_Write};
 
 static bool _can_process_with_speed(u8 cmd) {
 	int size = ARRAY_SIZE(ignore_with_speed);
@@ -252,7 +252,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			u8 mode = decode_u8(command->data);
 			sys_debug("ctl mode = %d, len = %d\n", mode, command->len);
-			if (!mc_set_foc_mode(mode)) {
+			if (!mc_set_ctrl_mode(mode)) {
 				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}
 			response[len++] = motor.controller.mode_req;
@@ -346,15 +346,17 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			u8 id = decode_u8((u8 *)command->data);
 			mc_conf_decode_pid(&pid, (u8 *)command->data + 1);
 			sys_debug("set id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
-			mot_contrl_set_pid(&motor.controller, id, pid.kp, pid.ki, pid.kd);
 			mc_conf_set_pid(id, &pid);
+			if (id < PID_Max_ID) {
+				mot_contrl_set_pid(&motor.controller, id, pid.kp, pid.ki, pid.kd);
+			}
 			break;
 		}
 		case Foc_Get_Pid:
 		{
 			pid_t pid;
 			u8 id =   decode_u8((u8 *)command->data);
-			if (id < PID_Max_ID) {
+			if ((id < PID_Max_ID) || (id == PID_EPM_ExtID)) {
 				mc_conf_get_pid(id, &pid);
 				erroCode = id;
 				len += mc_conf_encode_pid(&pid, response + 3);
@@ -445,12 +447,12 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			u8 start = decode_u8((u8 *)command->data);
 			if (start == 1) {
 				sys_debug("start mpta cali\n");
-				if (mc_set_foc_mode(CTRL_MODE_CURRENT)) {
+				if (mc_set_ctrl_mode(CTRL_MODE_CURRENT)) {
 					mot_contrl_mtpa_calibrate(&motor.controller, true);
 				}
 			}else {
 				mot_contrl_mtpa_calibrate(&motor.controller, false);
-				mc_set_foc_mode(CTRL_MODE_TRQ);
+				mc_set_ctrl_mode(CTRL_MODE_TRQ);
 			}
 			break;
 		}
@@ -472,9 +474,11 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			plot_type = (int)decode_u8((u8 *)command->data);
 		#if (CONFIG_ENABLE_IAB_REC==1)
 			if (plot_type == 6) {
-				mc_start_current_rec(true);
+				mc_start_current_rec(true, true);
+			}else if (plot_type == 11) {
+				mc_start_current_rec(false, true);
 			}else if (plot_type == 0) {
-				mc_start_current_rec(false);
+				mc_start_current_rec(false, false);
 			}
 		#endif
 			sys_debug("plot type %d\n", plot_type);

Applications/foc/commands.h

@@ -18,7 +18,7 @@ typedef enum {
 	Foc_Set_Open_Dq_Vol,		//u32, u32, 114d000000000a000000, 114d0000000000000000
 	Foc_Set_EPM_Mode,
 	Foc_Start_EPM_Move,
-	Foc_Conf_Pid,
+	Foc_Conf_Pid,  //设置pid参数，但是不保存,需要保存的话需要用Foc_Write_Config命令
 	Foc_Start_DQ_Calibrate,
 	Foc_Set_IS_Curr_Angle,  //设置DQ电流矢量和超前角
 	Foc_Set_Plot_Type,

Applications/foc/core/PI_Controller.h

@@ -10,9 +10,7 @@ typedef struct {
 	float  max;
 	float  min;
 	float  ui;
-	float  sat;
 	float  ts;
-	bool   is_sat;
 }PI_Controller;
 
 static __INLINE void PI_Controller_Change_Kpi(PI_Controller *pi, float kp, float ki) {
@@ -27,87 +25,40 @@ static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min)
 }
 static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->ui = (init);
-	pi->sat = 0.0f;
-	pi->is_sat = false;
 }
 
-static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp;
-	float ki_err = (err) * pi->ki;
-	float integral = ki_err * pi->ts;
-	pi->ui = fclamp(pi->ui + integral, pi->min, pi->max);
-	float out = pi->ui + kp_err;
-	float sat_out = fclamp(out, pi->min, pi->max);
-	if (out != sat_out) {
-		pi->is_sat = true;
-	}else {
-		pi->is_sat = false;
-	}
-	return sat_out;
-}
-
-static __INLINE float PI_Controller_RunVel(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp;
-	float ki_err = (kp_err) * pi->ki;
-	float integral = ki_err * pi->ts;
-
-	pi->ui = pi->ui + integral;
-	float out = pi->ui + kp_err;
+static __INLINE float PI_Controller_Parallel(PI_Controller *pi, float err, float ff) {
+	pi->ui += err * pi->ki * pi->ts;
+	float out = pi->ui + err * pi->kp + ff;
+	float out_presat = out;
 	if (out > pi->max) {
 		out = pi->max;
-		pi->ui = out - kp_err;
+		pi->ui += (out - out_presat);
 	}else if (out < pi->min) {
 		out = pi->min;
-		pi->ui = out - kp_err;
+		pi->ui += (out - out_presat);
 	}
 	return out;
 }
 
 
 /* 电流环，PI 串联结构 */
-static __INLINE float PI_Controller_Current(PI_Controller *pi, float err, float ff) {
-	float kp_err = (err) * pi->kp;
-	float ki_err = (kp_err) * pi->ki;
-	float integral = ki_err * pi->ts;
-
-	pi->ui = pi->ui + integral;
+static __INLINE float PI_Controller_Serial(PI_Controller *pi, float err, float ff) {
+	float kp_err = err * pi->kp;
+	pi->ui += kp_err * pi->ki * pi->ts;
 	float out = pi->ui + kp_err + ff;
+	float out_presat = out;
 	if (out > pi->max) {
 		out = pi->max;
-		pi->is_sat = true;
-		pi->ui = out - (kp_err + ff);
+		pi->ui += (out - out_presat);
 	}else if (out < pi->min) {
 		out = pi->min;
-		pi->is_sat = true;
-		pi->ui = out - (kp_err + ff);
-	}else {
-		pi->is_sat = false;
+		pi->ui += (out - out_presat);
 	}
 	return out;
 }
 
 
-static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp;
-	float ki_err = (kp_err) * pi->ki;
-	float integral = ki_err * pi->ts;
-
-	pi->ui = pi->ui + integral;
-	float out = pi->ui + kp_err;
-	if (out > pi->max) {
-		out = pi->max;
-		pi->is_sat = true;
-		pi->ui = out - kp_err;
-	}else if (out < pi->min) {
-		out = pi->min;
-		pi->is_sat = true;
-		pi->ui = out - kp_err;
-	}else {
-		pi->is_sat = false;
-	}
-	return out;
-}
-
 static __INLINE float _fmod(float v, s32 m) {
 	int v_i = (int)v;
 	int m_i = v_i % m;

Applications/foc/core/PMSM_FOC_Core.c

@@ -1,1270 +0,0 @@
-#include "arm_math.h"
-#include "PMSM_FOC_Core.h"
-#include "foc/foc_config.h"
-#include "foc/mc_config.h"
-#include "foc/motor/motor_param.h"
-#include "foc/core/e_ctrl.h"
-#include "foc/core/etcs.h"
-#include "math/fix_math.h"
-#include "math/fast_math.h"
-#include "foc/motor/current.h"
-#include "foc/motor/motor.h"
-#include "foc/core/svpwm.h"
-#include "foc/core/thro_torque.h"
-#include "foc/core/foc_observer.h"
-#include "foc/core/F_Calc.h"
-#include "foc/samples.h"
-#include "foc/limit.h"
-#include "foc/mc_error.h"
-#include "app/nv_storage.h"
-#include "bsp/bsp_driver.h"
-#include "libs/logger.h"
-#include "math/fir.h"
-
-#define _DEBUG(fmt, args...) sys_debug(fmt, ##args)
-
-PMSM_FOC_Ctrl gFoc_Ctrl;
-static bool g_focinit = false;
-
-static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
-	float c,s;
-#if 0
-	arm_sin_cos(angle, &s, &c);
-#else
-	s = gFoc_Ctrl.out.sin;
-	c = gFoc_Ctrl.out.cos;
-#endif
-
-	alpha_beta->a = dq->d * c - dq->q * s;
-	alpha_beta->b = dq->d * s + dq->q * c;
-}
-
-static __INLINE void RevClark(AB_t *alpha_beta, float *ABC){
-	ABC[0] = alpha_beta->a;
-	ABC[1] = -alpha_beta->a * 0.5f + alpha_beta->b * SQRT3_BY_2;
-	ABC[2] = -alpha_beta->a * 0.5f - alpha_beta->b * SQRT3_BY_2;
-}
-
-static __INLINE void Clark(float A, float B, float C, AB_t *alpha_beta){
-	alpha_beta->a = A;
-	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
-}
-
-static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
-	float c,s;
-#if 0
-	arm_sin_cos(angle, &s, &c);
-#else
-	s = gFoc_Ctrl.out.sin;
-	c = gFoc_Ctrl.out.cos;
-#endif
-	dq->d = alpha_beta->a * c + alpha_beta->b * s;
-	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
-}
-
-void PMSM_FOC_ABC2Dq(float a, float b, float c, float *d, float *q) {
-	AB_t ab;
-	DQ_t dq;
-	Clark(a, b, c, &ab);
-	Park(&ab, 0, &dq);
-	*d = dq.d;
-	*q = dq.q;
-}
-
-
-static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {	
-	float cp = c->s_Cp;
-	c->s_FinalTgt = target; 
-	c->s_Step = (c->s_FinalTgt - cp) / (float)time;
-}
-
-static __INLINE float FOC_Get_DqRamp(dq_Rctrl *c) {
-	if (++c->n_StepCount == c->n_CtrlCount) {
-		c->s_Cp += c->s_Step;
-		if (c->s_Step < 0) {
-			if (c->s_Cp < c->s_FinalTgt) {
-				c->s_Cp = c->s_FinalTgt;
-			}
-		}else {
-			if (c->s_Cp > c->s_FinalTgt) {
-				c->s_Cp = c->s_FinalTgt;
-			}
-		}
-		c->n_StepCount = 0;
-	}
-	return c->s_Cp;
-}
-
-static __INLINE void FOC_DqRamp_init(dq_Rctrl *c, int count) {
-	c->n_CtrlCount = count;
-	c->n_StepCount = 0;
-	c->s_Cp = 0;
-	c->s_FinalTgt = 0;
-	c->s_Step = 0;
-}
-
-static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, float target) {
-	FOC_Set_DqRamp(c, target, (/*CONFIG_IDQ_CTRL_TS/CONFIG_SPD_CTRL_TS - 1*/CURRENT_LOOP_RAMP_COUNT));
-}
-
-static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
-	FOC_Set_DqRamp(c, target, (CONFIG_FOC_VDQ_RAMP_FINAL_TIME/1000*((CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS))));
-}
-
-
-static void PMSM_FOC_Reset_PID(void) {
-	PI_Controller_Reset(&gFoc_Ctrl.pi_id, 0);
-	PI_Controller_Reset(&gFoc_Ctrl.pi_iq, 0);
-	PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
-	PI_Controller_Reset(&gFoc_Ctrl.pi_power, 0);
-#ifdef CONFIG_SPEED_LADRC
-	ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, 0);
-	ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, 0);
-#else
-	PI_Controller_Reset(&gFoc_Ctrl.pi_vel, 0);
-	PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, 0);
-#endif
-}
-
-static void PMSM_FOC_Conf_PID(void) {
-	float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-	gFoc_Ctrl.pi_id.kp = mc_conf()->c.pid[PID_ID_ID].kp;
-	gFoc_Ctrl.pi_id.ki = mc_conf()->c.pid[PID_ID_ID].ki;
-	gFoc_Ctrl.pi_id.kd = mc_conf()->c.pid[PID_ID_ID].kd;
-	gFoc_Ctrl.pi_id.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
-
-	gFoc_Ctrl.pi_iq.kp = mc_conf()->c.pid[PID_IQ_ID].kp;
-	gFoc_Ctrl.pi_iq.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
-	gFoc_Ctrl.pi_iq.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
-	gFoc_Ctrl.pi_iq.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
-
-	gFoc_Ctrl.pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
-	gFoc_Ctrl.pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
-	gFoc_Ctrl.pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
-	gFoc_Ctrl.pi_power.ts = slow_ctrl_ts;
-
-	gFoc_Ctrl.pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
-	gFoc_Ctrl.pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
-	gFoc_Ctrl.pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
-	gFoc_Ctrl.pi_lock.ts = slow_ctrl_ts;
-
-#ifdef CONFIG_SPEED_LADRC
-	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
-	ladrc_init(&gFoc_Ctrl.vel_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_Wo, nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
-#else
-	gFoc_Ctrl.pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
-	gFoc_Ctrl.pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
-	gFoc_Ctrl.pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
-	gFoc_Ctrl.pi_vel_lim.ts = slow_ctrl_ts;
-	gFoc_Ctrl.pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
-	gFoc_Ctrl.pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
-	gFoc_Ctrl.pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
-	gFoc_Ctrl.pi_vel.ts = slow_ctrl_ts;
-#endif
-}
-
-static void PMSM_FOC_UserInit(void) {
-	memset(&gFoc_Ctrl.userLim, 0, sizeof(gFoc_Ctrl.userLim));
-	gFoc_Ctrl.userLim.s_iDCLim = min(mc_conf()->c.max_idc, gFoc_Ctrl.hwLim.s_iDCMax);
-	gFoc_Ctrl.userLim.s_motRPMLim = min(mc_conf()->c.max_rpm, gFoc_Ctrl.hwLim.s_motRPMMax);
-	gFoc_Ctrl.userLim.s_torqueLim = mc_conf()->c.max_torque;//MAX_TORQUE;
-	gFoc_Ctrl.userLim.s_PhaseCurrLim = min(mc_conf()->c.max_phase_curr, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
-	gFoc_Ctrl.userLim.s_vDCMaxLim = mc_conf()->c.max_dc_vol;
-	gFoc_Ctrl.userLim.s_vDCMinLim = mc_conf()->c.min_dc_vol;
-	gFoc_Ctrl.userLim.s_iDCeBrkLim = 0xFF;
-	gFoc_Ctrl.userLim.s_PhaseVoleBrkLim = gFoc_Ctrl.hwLim.s_PhaseVolMax;
-}
-
-void PMSM_FOC_RT_LimInit(void) {
-	gFoc_Ctrl.protLim.s_iDCLim = HW_LIMIT_NONE;
-	gFoc_Ctrl.protLim.s_TorqueLim = HW_LIMIT_NONE;
-
-	eRamp_init_target2(&gFoc_Ctrl.rtLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim, CONFIG_LIMIT_RAMP_TIME);
-	eRamp_init_target2(&gFoc_Ctrl.rtLim.torqueLimRamp, gFoc_Ctrl.userLim.s_torqueLim, CONFIG_LIMIT_RAMP_TIME);
-	eRamp_init_target2(&gFoc_Ctrl.rtLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim, CONFIG_LIMIT_RAMP_TIME);
-}
-
-void PMSM_FOC_CoreInit(void) {
-
-	PMSM_FOC_Conf_PID();
-	
-	memset(&gFoc_Ctrl.in, 0, sizeof(FOC_InP));
-	memset(&gFoc_Ctrl.out, 0, sizeof(FOC_OutP));
-	
-	gFoc_Ctrl.hwLim.s_iDCMax = CONFIG_HW_MAX_DC_CURRENT;
-	gFoc_Ctrl.hwLim.s_motRPMMax = CONFIG_HW_MAX_MOTOR_RPM;
-	gFoc_Ctrl.hwLim.s_PhaseCurrMax = CONFIG_HW_MAX_PHASE_CURR;
-	gFoc_Ctrl.hwLim.s_PhaseVolMax = CONFIG_HW_MAX_PHASE_VOL;
-	gFoc_Ctrl.hwLim.s_vDCMax      = CONFIG_HW_MAX_DC_VOLTAGE;
-	gFoc_Ctrl.hwLim.s_torqueMax  = mc_conf()->m.max_torque; //电机的最大扭矩
-	gFoc_Ctrl.hwLim.s_FWDCurrMax = mc_conf()->m.max_fw_id;  //电池能支持的最大弱磁电流
-	if (!g_focinit) {
-		PMSM_FOC_UserInit();
-		PMSM_FOC_RT_LimInit();
-		g_focinit = true;
-		//_DEBUG("User Limit:\n");
-		//_DEBUG("dc %f, rpm %f, torque %f, phase %f, vDCmax %f, vDCmin %f, ebrk %f\n", gFoc_Ctrl.userLim.s_iDCLim, gFoc_Ctrl.userLim.s_motRPMLim, gFoc_Ctrl.userLim.s_torqueLim,
-		//	gFoc_Ctrl.userLim.s_PhaseCurrLim, gFoc_Ctrl.userLim.s_vDCMaxLim, gFoc_Ctrl.userLim.s_vDCMinLim, gFoc_Ctrl.userLim.s_TorqueBrkLim);
-		//_DEBUG("Hw Limit:\n");
-		//_DEBUG("dc %f, rpm %f, torque %f, phase %f\n", gFoc_Ctrl.hwLim.s_iDCMax, gFoc_Ctrl.hwLim.s_motRPMMax, gFoc_Ctrl.hwLim.s_torqueMax, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
-	}
-	gFoc_Ctrl.userLim.s_TorqueBrkLim = mc_get_ebrk_torque();
-	gFoc_Ctrl.params.n_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
-	gFoc_Ctrl.params.n_poles = mc_conf()->m.poles;//MOTOR_POLES;
-	gFoc_Ctrl.params.lq = mc_conf()->m.lq;
-	gFoc_Ctrl.params.ld = mc_conf()->m.lq;
-	gFoc_Ctrl.params.flux = mc_conf()->m.flux;
-	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
-	gFoc_Ctrl.in.s_dqAngle     = INVALID_ANGLE;
-	gFoc_Ctrl.in.s_vDC = sample_vbus_raw();
-	gFoc_Ctrl.in.s_angleLast = INVALID_ANGLE;
-	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
-	gFoc_Ctrl.out.f_vdqRation = 0;
-
-	eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_CRUISE_RAMP_TIME);
-
-	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[0], 1);
-	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[1], 1);
-
-	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[0], (CONFIG_FOC_VDQ_RAMP_TS));
-	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_FOC_VDQ_RAMP_TS));	
-	PMSM_FOC_Reset_PID();
-
-	foc_observer_init();
-
-	gFoc_Ctrl.plot_type = Plot_None;
-}
-
-
-#define CONFIG_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
-#define CONFIG_PHASE_UNBALANCE_THROLD 4.0F
-#define CONFIG_PHASE_UNBALANCE_R      0.1F
-static float phase_unbalance_r = 0.0f;
-static float phase_a_max, phase_b_max, phase_c_max;
-static u32 phase_unbalance_cnt;
-static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
-	static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
-	static float a_max = 0, b_max = 0, c_max = 0;
-	static u32 _unbalance_cnt = 0;
-	static u32 _unbalance_time = 0;
-	float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US / 2.0f;
-	if (lowpass > 1.0f) {
-		lowpass = 1.0f;
-	}
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], lowpass);
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], lowpass);
-	gFoc_Ctrl.in.s_iABCFilter[2] = -(gFoc_Ctrl.in.s_iABCFilter[0] + gFoc_Ctrl.in.s_iABCFilter[1]);
-	if ((gFoc_Ctrl.in.s_angleLast == INVALID_ANGLE) || (gFoc_Ctrl.in.s_motVelRadusPers < 100)) {
-		gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
-		a_max = b_max = c_max = 0;
-		_unbalance_cnt = 0;
-		_unbalance_time = get_tick_ms();
-		_cycle_cnt = 0;
-		_last_mod_cnt = 0;
-		phase_unbalance_r = 0;
-		return;
-	}
-	float delta_angle = gFoc_Ctrl.in.s_motAngle - gFoc_Ctrl.in.s_angleLast;
-	if (delta_angle > 200 || delta_angle < -200) { //one cycle
-		_cycle_cnt ++;
-	}
-	gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
-	u32 mod_cnt = _cycle_cnt % CONFIG_PEAK_CNT;
-	bool trigger = false;
-	if ((mod_cnt == 0) && (_last_mod_cnt != mod_cnt)) {
-		trigger = true;
-	}
-	_last_mod_cnt = mod_cnt;
-
-	a_max = MAX(a_max, gFoc_Ctrl.in.s_iABCFilter[0] * (2.2f));
-	b_max = MAX(b_max, gFoc_Ctrl.in.s_iABCFilter[1] * (2.2f));
-	c_max = MAX(c_max, gFoc_Ctrl.in.s_iABCFilter[2] * (2.2f));
-	if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
-		float i_min = 1000.0f, i_max = 0;
-		if (a_max > i_max) {
-			i_max = a_max;
-		}
-		if (a_max < i_min) {
-			i_min = a_max;
-		}
-		if (b_max > i_max) {
-			i_max = b_max;
-		}
-		if (b_max < i_min) {
-			i_min = b_max;
-		}
-		if (c_max > i_max) {
-			i_max = c_max;
-		}
-		if (c_max < i_min) {
-			i_min = c_max;
-		}
-		float unbalance_r = (i_max - i_min - CONFIG_PHASE_UNBALANCE_THROLD)/(i_max + 1e-8f);
-		if (unbalance_r >= CONFIG_PHASE_UNBALANCE_R) {
-			if ((_unbalance_cnt++ >= 500) || (get_delta_ms(_unbalance_time) >= 1000*10)) {
-				if (mc_set_critical_error(FOC_CRIT_PHASE_UNBalance_Err)) {
-					mc_crit_err_add(FOC_CRIT_PHASE_UNBalance_Err, (s16)i_max, (s16)i_min);
-				}
-			}
-		}else {
-			_unbalance_cnt = 0;
-			_unbalance_time = get_tick_ms();
-		}
-		phase_unbalance_r = unbalance_r;
-		phase_a_max = a_max;
-		phase_b_max = b_max;
-		phase_c_max = c_max;
-		phase_unbalance_cnt = _unbalance_cnt;
-		a_max = b_max = c_max = 0;
-	}
-}
-
-/* 死区补偿 */
-static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
-#ifdef CONFIG_START_LINE_DTC_CURRENT
-	float deadTime = (float)CONFIG_HW_DeadTime/2.0f;
-	s32 dutyDTCA = 0;
-	s32 dutyDTCB = 0;
-	s32 dutyDTCC = 0;
-	float r, delta;
-	float iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[0]);
-	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
-		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
-		r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
-		if (r > 1.0f) {
-			r = 1.0f;
-		}
-		if (gFoc_Ctrl.in.s_iABC_DT[0] < 0) {
-			r = -r;
-		}
-		dutyDTCA = (s32)(r * deadTime);
-	}
-	iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[1]);
-	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
-		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
-		r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
-		if (r > 1.0f) {
-			r = 1.0f;
-		}
-		if (gFoc_Ctrl.in.s_iABC_DT[1] < 0) {
-			r = -r;
-		}
-		dutyDTCB = (s32)(r * deadTime);
-	}
-	iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[2]);
-	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
-		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
-		r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
-		if (r > 1.0f) {
-			r = 1.0f;
-		}
-		if (gFoc_Ctrl.in.s_iABC_DT[2] < 0) {
-			r = -r;
-		}
-		dutyDTCC = (s32)(r * deadTime);
-	}
-	s32 dutyA = (s32)gFoc_Ctrl.out.n_Duty[0] + dutyDTCA;
-	s32 dutyB = (s32)gFoc_Ctrl.out.n_Duty[1] + dutyDTCB;
-	s32 dutyC = (s32)gFoc_Ctrl.out.n_Duty[2] + dutyDTCC;
-	gFoc_Ctrl.out.n_Duty[0] = sclamp(dutyA, 0, PWM_Half_Period);
-	gFoc_Ctrl.out.n_Duty[1] = sclamp(dutyB, 0, PWM_Half_Period);
-	gFoc_Ctrl.out.n_Duty[2] = sclamp(dutyC, 0, PWM_Half_Period);
-#endif
-}
-
-static __INLINE void Phase_Voltage_update(float lowpass) {
-	float v_ABC[3];
-	get_uvw_phases_raw(v_ABC);
-	LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[0], v_ABC[0], lowpass);
-	LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[1], v_ABC[1], lowpass);
-	LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[2], v_ABC[2], lowpass);
-	/* phase voltage = phase-phase voltage / sqrt(3), 1.4是滤波器幅值补偿系数 */
-	float phase_vAN = (gFoc_Ctrl.in.s_SamplePhaseV[0] - gFoc_Ctrl.in.s_SamplePhaseV[1]) * ONE_BY_SQRT3 * 1.4f;
-	float phase_vBN = (gFoc_Ctrl.in.s_SamplePhaseV[1] - gFoc_Ctrl.in.s_SamplePhaseV[2]) * ONE_BY_SQRT3 * 1.4f;
-	float phase_vCN = (gFoc_Ctrl.in.s_SamplePhaseV[2] - gFoc_Ctrl.in.s_SamplePhaseV[0]) * ONE_BY_SQRT3 * 1.4f;
-	Clark(phase_vAN, phase_vBN, phase_vCN, &gFoc_Ctrl.out.s_SampleAB);
-	Park(&gFoc_Ctrl.out.s_SampleAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_SamplevDQ);
-}
-
-//#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
-#define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
-#define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
-static float encoder_angle,obser_angle, obser_vel = 111111;
-static __INLINE bool PMSM_FOC_Update_Input(void) {
-	AB_t iAB;
-	float *iabc = gFoc_Ctrl.in.s_iABC;
-
-	phase_current_get(iabc);
-
-	Clark(iabc[0], iabc[1], iabc[2], &iAB);
-
-	foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * TWO_BY_THREE, gFoc_Ctrl.out.s_OutVAB.b * TWO_BY_THREE, iAB.a, iAB.b);
-
-	float enc_angle = motor_encoder_get_angle();
-	float enc_vel = motor_encoder_get_speed();
-	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
-		/* detect encoder angle error, do something here */
-		if (!foc_observer_sensorless_stable()) {
-			gFoc_Ctrl.in.s_motVelocity = 0;
-			return false;
-		}
-		if (obser_vel == 111111) {
-			obser_vel = foc_observer_sensorless_speed();
-			obser_angle = foc_observer_sensorless_angle();
-			encoder_angle = enc_angle;
-		}
-		enc_angle = foc_observer_sensorless_angle();
-		enc_vel = foc_observer_sensorless_speed();
-
-	}
-	
-	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
-		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
-	}else {
-		gFoc_Ctrl.in.s_motAngle = enc_angle;
-	}
-	gFoc_Ctrl.in.s_motVelocity = enc_vel;
-	LowPass_Filter(gFoc_Ctrl.in.s_motVelocityFiltered, gFoc_Ctrl.in.s_motVelocity, 0.01f);
-	gFoc_Ctrl.in.s_motVelRadusPers = gFoc_Ctrl.in.s_motVelocityFiltered / 30.0f * PI * gFoc_Ctrl.params.n_poles;
-
-	PMSM_FOC_Phase_Unbalance();
-
-#ifdef CONFIG_DQ_STEP_RESPONSE
-	gFoc_Ctrl.in.s_motAngle = 0;
-#endif
-
-	gFoc_Ctrl.in.s_vDC = get_vbus_float();
-	
-	arm_sin_cos(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
-	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
-
-	float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US;
-	float iqLowPass = lowpass * 2.0f;
-	if (iqLowPass > 1.0f) {
-		iqLowPass = 1.0f;
-	}else if (iqLowPass <= 0.0001f) {
-		iqLowPass = 0.001f;
-	}
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, iqLowPass);
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, iqLowPass);
-	/* 使用低通后的dq电流重新变换得到abc电流,给死区补偿使用 */
-	RevPark(&gFoc_Ctrl.out.s_FilterIdq, gFoc_Ctrl.in.s_motAngle, &iAB);
-	RevClark(&iAB, gFoc_Ctrl.in.s_iABC_DT);
-	
-	Phase_Voltage_update(lowpass);
-
-#ifdef CONFIG_START_LINE_DTC_CURRENT
-	gFoc_Ctrl.out.s_OutVdqDTC.d = 0;
-	gFoc_Ctrl.out.s_OutVdqDTC.q = 0;
-#else
-	AB_t vAB;
-	vAB.a = (1.0f / 3.0f) * (2.0f * SIGN(gFoc_Ctrl.in.s_iABC_DT[0]) - SIGN(gFoc_Ctrl.in.s_iABC_DT[1]) - SIGN(gFoc_Ctrl.in.s_iABC_DT[2]));
-	vAB.b = ONE_BY_SQRT3 * (SIGN(gFoc_Ctrl.in.s_iABC_DT[1]) - SIGN(gFoc_Ctrl.in.s_iABC_DT[2]));
-	float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * gFoc_Ctrl.in.s_vDC;
-	vAB.a = vAB.a * dtc;
-	vAB.b = vAB.b * dtc;
-	Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVdqDTC); //used for vbus current calc
-#endif
-#ifdef CONFIG_Volvec_Delay_Comp
-	if (gFoc_Ctrl.in.s_motVelocityFiltered >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
-		float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelRadusPers / PI * 180.0f * (FOC_CTRL_US * 0.8f);
-		rand_angle(next_angle);
-		arm_sin_cos(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
-	}
-#endif
-	return true;
-}
-
-static __INLINE float id_feedforward(float eW) {
-#ifdef CONFIG_CURRENT_LOOP_DECOUPE
-	return -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
-#else
-	return 0;
-#endif
-}
-
-static __INLINE float iq_feedforward(float eW) {
-#ifdef CONFIG_CURRENT_LOOP_DECOUPE
-	return (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
-#else
-	return 0;
-#endif
-}
-
-bool PMSM_FOC_Schedule(void) {
-
-	gFoc_Ctrl.ctrl_count++;
-
-	if (!PMSM_FOC_Update_Input()){
-		return false;
-	}
-	
-	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
-
-		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
-		float max_vd = max_Vdc * SQRT3_BY_2;
-
-		/* limiter Vd output for PI controller */
-		gFoc_Ctrl.pi_id.max = max_vd;
-		gFoc_Ctrl.pi_id.min = -max_vd;
-	#ifndef CONFIG_DQ_STEP_RESPONSE
-		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
-	#endif
-		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
-		float id_ff = id_feedforward(gFoc_Ctrl.in.s_motVelRadusPers);
-		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_Current(&gFoc_Ctrl.pi_id, err, id_ff);
-#ifdef UPDATE_Lq_By_iq
-		/* update kp&ki from lq for iq PI controller */
-		float lq = motor_get_lq_from_iq((s16)gFoc_Ctrl.out.s_FilterIdq.q);
-		LowPass_Filter(gFoc_Ctrl.params.lq, lq, 0.01f);
-		gFoc_Ctrl.pi_iq.kp = ((float)nv_get_foc_params()->n_currentBand * gFoc_Ctrl.params.lq);
-		gFoc_Ctrl.pi_iq.ki = (nv_get_motor_params()->r/gFoc_Ctrl.params.lq);
-#endif
-		/* limiter Vq output for PI controller */
-		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
-		gFoc_Ctrl.pi_iq.max = max_vq;
-		gFoc_Ctrl.pi_iq.min = -max_vq;
-	#ifndef CONFIG_DQ_STEP_RESPONSE
-		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
-	#endif
-		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
-		float iq_ff = iq_feedforward(gFoc_Ctrl.in.s_motVelRadusPers);
-		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_Current(&gFoc_Ctrl.pi_iq, err, iq_ff);
-	}else {
-		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
-		float max_vd = max_Vdc * SQRT3_BY_2;
-		float vd_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[0]);
-		gFoc_Ctrl.in.s_targetVdq.d = fclamp(vd_ref, -max_vd, max_vd);
-	
-		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
-		float vq_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[1]);
-		gFoc_Ctrl.in.s_targetVdq.q = fclamp(vq_ref, -max_vq, max_vq);
-	}
-
-	gFoc_Ctrl.out.s_OutVdq.d = gFoc_Ctrl.in.s_targetVdq.d;
-	gFoc_Ctrl.out.s_OutVdq.q = gFoc_Ctrl.in.s_targetVdq.q;
-
-	RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVAB);
-	
-	SVM_Duty_Fix(&gFoc_Ctrl.out.s_OutVAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);
-
-	PMSM_FOC_DeadTime_Compensate((s32)FOC_PWM_Half_Period);
-
-	phase_current_point(&gFoc_Ctrl.out);
-	
-	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
-	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
-
-	return true;
-}
-
-void PMSM_FOC_LogDebug(void) {
-	sys_debug("DC curr %f --- %f,  %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_FilteriDC, gFoc_Ctrl.out.s_CalciDC2);
-	sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
-	sys_debug("unbalance: %d, %f, %f, %f, %f\n", phase_unbalance_cnt, phase_unbalance_r, phase_a_max, phase_b_max, phase_c_max);
-	if (obser_vel != 111111) {
-		sys_debug("AB error: %f,%f,%f\n", obser_angle, encoder_angle, obser_vel);
-	}
-}
-
-/*called in media task */
-u8 PMSM_FOC_CtrlMode(void) {
-	u8 preMode = gFoc_Ctrl.out.n_RunMode;
-
-	if (!gFoc_Ctrl.in.b_motEnable) {
-		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
-	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
-		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
-	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_SPD || gFoc_Ctrl.in.b_cruiseEna){
-		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
-	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
-		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
-	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_EBRAKE) {
-		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_EBRAKE;
-	}else {
-		if (!gFoc_Ctrl.in.b_cruiseEna) {
-			gFoc_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
-		}
-	}
-	if (preMode != gFoc_Ctrl.out.n_RunMode) {
-		if ((preMode == CTRL_MODE_SPD) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
-#ifdef CONFIG_SPEED_LADRC
-			//ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
-			ladrc_copy(&gFoc_Ctrl.vel_lim_adrc, &gFoc_Ctrl.vel_adrc);
-#else
-			PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, gFoc_Ctrl.in.s_targetTorque);
-#endif
-		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
-#ifdef CONFIG_SPEED_LADRC
-			ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
-#else
-			PI_Controller_Reset(&gFoc_Ctrl.pi_vel, gFoc_Ctrl.in.s_targetTorque);
-#endif
-		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
-#ifdef CONFIG_SPEED_LADRC
-			ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
-#else
-			PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, gFoc_Ctrl.in.s_targetTorque);
-#endif
-		}else if ((preMode != gFoc_Ctrl.out.n_RunMode) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
-			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
-			eCtrl_set_TgtTorque(motor_get_ebreak_toruqe(gFoc_Ctrl.in.s_motVelocity));
-		}else if ((preMode == CTRL_MODE_EBRAKE) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
-#ifdef CONFIG_SPEED_LADRC
-			ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, F_get_air());
-#else
-			PI_Controller_Reset(&gFoc_Ctrl.pi_vel, F_get_air());
-#endif
-		}
-	}
-	
-	return gFoc_Ctrl.out.n_RunMode;
-}
-
-static void crosszero_step_towards(float *value, float target) {
-	static float no_cro_step = CONFIG_CrossZero_NorStep;
-	float v_now = *value;
-	bool cross_zero = false;
-	float nor_step = mc_conf()->cz.normal_step;
-	float min_step = mc_conf()->cz.min_step;
-	float min_ramp_torque = mc_conf()->cz.low;
-	float high_ramp_torque = mc_conf()->cz.high;
-	if (target > 0) {
-		if (v_now < -min_ramp_torque) {
-			step_towards(value, -min_ramp_torque + 0.001f, nor_step);
-			cross_zero = true;
-		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
-			step_towards(value, target, min_step);
-			cross_zero = true;
-		}
-	}else if (target == 0) {
-		if (v_now > high_ramp_torque) {
-			step_towards(value, high_ramp_torque - 0.001f, nor_step);
-			cross_zero = true;
-		}else if (v_now >= min_ramp_torque && v_now <= high_ramp_torque) {
-			step_towards(value, target, min_step);
-			cross_zero = true;
-		}
-	}else {
-		if (v_now > high_ramp_torque) {
-			step_towards(value, high_ramp_torque - 0.001f, nor_step);
-			cross_zero = true;
-		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
-			step_towards(value, target, min_step);
-			cross_zero = true;
-		}
-	}
-	if (!cross_zero) {
-		step_towards(&no_cro_step, nor_step, 0.1f);
-		step_towards(value, target, no_cro_step);
-	}else {
-		no_cro_step = 0.5f;
-	}
-}
-
-
-/* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
-static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
-#if 1
-	gFoc_Ctrl.pi_power.max = maxTrq;
-	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.DCCurrLimRamp) - (gFoc_Ctrl.out.s_FilteriDC);
-	return PI_Controller_Run(&gFoc_Ctrl.pi_power, errRef);
-#else
-	return maxTrq;
-#endif
-}
-
-static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
-#ifdef CONFIG_SPEED_LADRC
-	float lim = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp);
-	ladrc_set_range(&gFoc_Ctrl.vel_lim_adrc, 0, maxTrq);
-	return ladrc_run(&gFoc_Ctrl.vel_lim_adrc, lim, gFoc_Ctrl.in.s_motVelocity);
-#else
-	gFoc_Ctrl.pi_vel_lim.max = maxTrq;
-	gFoc_Ctrl.pi_vel_lim.min = 0;
-
-	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motVelocity;
-	return PI_Controller_RunVel(&gFoc_Ctrl.pi_vel_lim, err);
-#endif
-}
-
-static __INLINE void PMSM_FOC_idq_Assign(void) {
-	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
-		if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_dqAngle != INVALID_ANGLE)) {
-			float s, c;
-			normal_sincosf(degree_2_pi(gFoc_Ctrl.in.s_dqAngle + 90.0f), &s, &c);
-			gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.in.s_targetCurrent * c;
-			
-			if (gFoc_Ctrl.in.s_targetIdq.d > gFoc_Ctrl.hwLim.s_FWDCurrMax) {
-				gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.hwLim.s_FWDCurrMax;
-			}else if (gFoc_Ctrl.in.s_targetIdq.d < -gFoc_Ctrl.hwLim.s_FWDCurrMax) {
-				gFoc_Ctrl.in.s_targetIdq.d = -gFoc_Ctrl.hwLim.s_FWDCurrMax;
-			}
-			gFoc_Ctrl.in.s_targetIdq.q = sqrtf(SQ(gFoc_Ctrl.in.s_targetCurrent) - SQ(gFoc_Ctrl.in.s_targetIdq.d));
-			if (s < 0) {
-				gFoc_Ctrl.in.s_targetIdq.q = -gFoc_Ctrl.in.s_targetIdq.q;
-			}
-		}else {
-			gFoc_Ctrl.in.s_targetIdq.d = 0;
-			gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent;
-		}
-	}else if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) ||
-				(gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
-		motor_mpta_fw_lookup(gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
-	}
-	u32 mask = cpu_enter_critical();
-	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);
-	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[1], gFoc_Ctrl.in.s_targetIdq.q);
-	cpu_exit_critical(mask);
-}
-
-/*called in media task */
-void PMSM_FOC_idqCalc(void) {
-	if (gFoc_Ctrl.in.b_AutoHold) {
-		float hold_torque = min(gFoc_Ctrl.protLim.s_TorqueLim, mc_conf()->c.max_autohold_torque);
-		gFoc_Ctrl.pi_lock.max = hold_torque;
-		gFoc_Ctrl.pi_lock.min = -hold_torque;
-		float vel_count = motor_encoder_get_vel_count();
-		float errRef = 0 - vel_count;
-		gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(&gFoc_Ctrl.pi_lock ,errRef);
-		PMSM_FOC_idq_Assign();
-		return;
-	}
-	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
-		gFoc_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
-	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE) {
-		float maxTrq = eCtrl_get_RefTorque();
-		if (eCtrl_get_FinalTorque() < 0.0001f && gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
-			maxTrq = 0;
-		}
-		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
-	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
-		float refTorque = min(eCtrl_get_RefTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp));
-		float maxTrq = PMSM_FOC_Limit_Speed(refTorque);
-		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
-		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
-	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
-		float maxSpeed = eCtrl_get_FinalSpeed();
-		float refSpeed = eCtrl_get_RefSpeed();
-		if (gFoc_Ctrl.in.b_cruiseEna) {
-			maxSpeed = eRamp_get_target(&gFoc_Ctrl.in.cruiseRpmRamp);
-			refSpeed = eRamp_get_intepolation(&gFoc_Ctrl.in.cruiseRpmRamp);//gFoc_Ctrl.in.s_cruiseRPM;
-		}
-#ifdef CONFIG_SPEED_LADRC
-		if (maxSpeed >= 0) {
-			ladrc_set_range(&gFoc_Ctrl.vel_adrc, -CONFIG_MAX_NEG_TORQUE, eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp));
-		}else if (maxSpeed < 0) {
-			ladrc_set_range(&gFoc_Ctrl.vel_adrc, -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp), CONFIG_MAX_NEG_TORQUE);
-		}
-
-		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
-			ladrc_set_range(&gFoc_Ctrl.vel_adrc, 0, 0);
-		}
-		gFoc_Ctrl.in.s_targetRPM = refSpeed;
-		float maxTrq = ladrc_run(&gFoc_Ctrl.vel_adrc, refSpeed, gFoc_Ctrl.in.s_motVelocity);
-#else
-		if (maxSpeed >= 0) {
-			gFoc_Ctrl.pi_vel.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
-#ifdef CONFIG_SERVO_MOTOR
-			gFoc_Ctrl.pi_vel.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
-#else
-			gFoc_Ctrl.pi_vel.min = -CONFIG_MAX_NEG_TORQUE;
-#endif
-		}else if (maxSpeed < 0) {
-			gFoc_Ctrl.pi_vel.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
-#ifdef CONFIG_SERVO_MOTOR
-			gFoc_Ctrl.pi_vel.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
-#else
-			gFoc_Ctrl.pi_vel.max = CONFIG_MAX_NEG_TORQUE;
-#endif
-		}
-
-		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
-			gFoc_Ctrl.pi_vel.max = 0;
-			gFoc_Ctrl.pi_vel.min = 0; //防止倒转
-		}
-		gFoc_Ctrl.in.s_targetRPM = refSpeed;
-		float errRef = refSpeed - gFoc_Ctrl.in.s_motVelocity;
-		float maxTrq = PI_Controller_RunVel(&gFoc_Ctrl.pi_vel, errRef);
-#endif
-		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
-		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
-	}
-
-	PMSM_FOC_idq_Assign();
-}
-
-u8 PMSM_FOC_RunTime_Limit(void) {
-	u8 changed = FOC_LIM_NO_CHANGE;
-	float dc_lim = (float)vbus_under_vol_limit();
-	float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
-
-	if (gFoc_Ctrl.protLim.s_iDCLim != dc_lim || gFoc_Ctrl.protLim.s_TorqueLim != torque_lim) {
-		if ((dc_lim > gFoc_Ctrl.protLim.s_iDCLim) || (torque_lim > gFoc_Ctrl.protLim.s_TorqueLim)) {
-			changed = FOC_LIM_CHANGE_H;
-		}else {
-			changed = FOC_LIM_CHANGE_L;
-		}
-		gFoc_Ctrl.protLim.s_iDCLim = dc_lim;
-		gFoc_Ctrl.protLim.s_TorqueLim = torque_lim;
-	}
-	return changed;
-}
-
-
-bool PMSM_FOC_iDC_is_Limited(void) {
-	return (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE);
-}
-
-bool PMSM_FOC_Torque_is_Limited(void) {
-	return (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE);
-}
-
-void PMSM_FOC_Slow_Task(void) {
-	eRamp_running(&gFoc_Ctrl.rtLim.torqueLimRamp);
-	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
-	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
-	eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
-	PMSM_FOC_idqCalc();
-}
-
-void PMSM_FOC_Change_VelLoop_Params(float wcv, float b0) {
-#ifdef CONFIG_SPEED_LADRC
-	ladrc_change_b0(&gFoc_Ctrl.vel_adrc, b0);
-	ladrc_change_K(&gFoc_Ctrl.vel_adrc, wcv);
-#else
-	PI_Controller_Change_Kpi(&gFoc_Ctrl.pi_vel, wcv, b0);
-#endif
-}
-
-void PMSM_FOC_Change_TrqLoop_Params(float wcv, float b0) {
-#ifdef CONFIG_SPEED_LADRC
-	ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, b0);
-	ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, wcv);
-#else
-	PI_Controller_Change_Kpi(&gFoc_Ctrl.pi_vel_lim, wcv, b0);
-#endif
-}
-
-
-float PMSM_FOC_Get_Real_dqVector(void) {
-	if (gFoc_Ctrl.out.s_RealCurrentFiltered == 0) {
-		gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
-	}
-	return gFoc_Ctrl.out.s_RealCurrentFiltered;
-}
-
-PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
-	return &gFoc_Ctrl;
-}
-
-void PMSM_FOC_Start(u8 nCtrlMode) {
-	if (gFoc_Ctrl.in.b_motEnable) {
-		return;
-	}
-	PMSM_FOC_CoreInit();
-	eCtrl_Reset();
-	gFoc_Ctrl.in.n_ctlMode = nCtrlMode;
-	gFoc_Ctrl.in.b_motEnable = true;
-}
-
-void PMSM_FOC_Stop(void) {
-	if (!gFoc_Ctrl.in.b_motEnable) {
-		return;
-	}
-	PMSM_FOC_CoreInit();
-	gFoc_Ctrl.in.b_motEnable = false;
-}
-
-bool PMSM_FOC_Is_Start(void) {
-	return gFoc_Ctrl.in.b_motEnable;
-}
-
-void PMSM_FOC_DCCurrLimit(float ibusLimit) {
-	if (ibusLimit > gFoc_Ctrl.hwLim.s_iDCMax) {
-		ibusLimit = gFoc_Ctrl.hwLim.s_iDCMax;
-	}
-	if (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE) {
-		ibusLimit = min(ibusLimit, gFoc_Ctrl.protLim.s_iDCLim);
-	}
-	gFoc_Ctrl.userLim.s_iDCLim = ibusLimit;
-
-	if (ABS(gFoc_Ctrl.in.s_motVelocity) <= CONFIG_ZERO_SPEED_RPM){
-		eRamp_reset_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, ibusLimit);
-	}else {
-		eRamp_set_step_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, ibusLimit, CONFIG_eCTRL_STEP_TS);
-	}
-}
-
-float PMSM_FOC_GetDCCurrLimit(void) {
-	return gFoc_Ctrl.userLim.s_iDCLim;
-}
-
-void PMSM_FOC_SpeedRampLimit(float speedLimit, float speed) {
-	if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
-		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
-	}
-	gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
-	if (ABS(speed) <= CONFIG_ZERO_SPEED_RPM) {
-		eRamp_reset_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit);
-	}else {
-		eRamp_set_step_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
-	}
-}
-
-void PMSM_FOC_SpeedLimit(float speedLimit) {
-	PMSM_FOC_SpeedRampLimit(speedLimit, gFoc_Ctrl.in.s_motVelocity);
-}
-
-void PMSM_FOC_SpeedDirectLimit(float limit) {
-	PMSM_FOC_SpeedRampLimit(limit, 0);
-}
-
-
-float PMSM_FOC_GetSpeedLimit(void) {
-	return gFoc_Ctrl.userLim.s_motRPMLim;
-}
-
-void PMSM_FOC_TorqueLimit(float torqueLimit) {
-	if (torqueLimit > gFoc_Ctrl.hwLim.s_torqueMax) {
-		torqueLimit = gFoc_Ctrl.hwLim.s_torqueMax;
-	}
-
-	if (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE) {
-		torqueLimit = min(torqueLimit, gFoc_Ctrl.protLim.s_TorqueLim);
-	}
-
-	gFoc_Ctrl.userLim.s_torqueLim = torqueLimit;
-
-	if (ABS(gFoc_Ctrl.in.s_motVelocity) <= CONFIG_ZERO_SPEED_RPM){
-		eRamp_reset_target(&gFoc_Ctrl.rtLim.torqueLimRamp, torqueLimit);
-	}else {
-		eRamp_set_step_target(&gFoc_Ctrl.rtLim.torqueLimRamp, torqueLimit, CONFIG_eCTRL_STEP_TS);
-	}
-}
-float PMSM_FOC_GetTorqueLimit(void) {
-	return gFoc_Ctrl.userLim.s_torqueLim;
-}
-
-void PMSM_FOC_SetEbrkTorque(s16 torque) {
-	gFoc_Ctrl.userLim.s_TorqueBrkLim = (float)torque;
-	//gFoc_Ctrl.userLim.s_iDCeBrkLim = fclamp(dc_curr, 0, nv_get_foc_params()->s_iDCeBrkLim);
-}
-
-float PMSM_FOC_GetEbrkTorque(void) {
-	if (!foc_observer_is_encoder()) {
-		return 0; //无感运行关闭能量回收
-	}
-	return gFoc_Ctrl.userLim.s_TorqueBrkLim;
-}
-
-float PMSM_FOC_GetVbusVoltage(void) {
-	return gFoc_Ctrl.in.s_vDC;
-}
-
-float PMSM_FOC_GetVbusCurrent(void) {
-	return gFoc_Ctrl.out.s_FilteriDC;
-}
-
-DQ_t* PMSM_FOC_GetDQCurrent(void) {
-	return &gFoc_Ctrl.out.s_RealIdq;
-}
-
-bool PMSM_FOC_SetCtrlMode(u8 mode) {
-	if (mode > CTRL_MODE_EBRAKE) {
-		PMSM_FOC_SetErrCode(FOC_Param_Err);
-		return false;
-	}
-	gFoc_Ctrl.in.n_ctlMode = mode;
-	return true;
-}
-
-
-u8 PMSM_FOC_GetCtrlMode(void) {
-	return gFoc_Ctrl.in.n_ctlMode;
-}
-
-void PMSM_FOC_PhaseCurrLim(float lim) {
-	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
-		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
-	}
-	gFoc_Ctrl.userLim.s_PhaseCurrLim = lim;
-}
-
-void PMSM_FOC_RT_PhaseCurrLim(float lim) {
-	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
-		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
-	}
-	eRamp_init_target2(&gFoc_Ctrl.rtLim.torqueLimRamp, lim, CONFIG_LIMIT_RAMP_TIME);
-}
-
-float PMSM_FOC_GetPhaseCurrLim(void) {
-	return gFoc_Ctrl.userLim.s_PhaseCurrLim;
-}
-
-void PMSM_FOC_SetOpenVdq(float vd, float vq) {
-	FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[0], vd);
-	FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[1], vq);
-}
-
-void PMSM_FOC_SetOpenVdq_Immediate(float vd, float vq) {
-	gFoc_Ctrl.vdq_ctl[0].s_Step = 0;
-	gFoc_Ctrl.vdq_ctl[0].s_FinalTgt = vd;
-	gFoc_Ctrl.vdq_ctl[0].s_Cp = vd;
-
-	gFoc_Ctrl.vdq_ctl[1].s_Step = 0;
-	gFoc_Ctrl.vdq_ctl[1].s_FinalTgt = vq;
-	gFoc_Ctrl.vdq_ctl[1].s_Cp = vq;
-}
-
-bool PMSM_FOC_EnableCruise(bool enable) {
-	if (enable != gFoc_Ctrl.in.b_cruiseEna) {
-		float motSpd = PMSM_FOC_GetSpeed();
-		if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
-			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
-			return false;
-		}
-		eRamp_reset_target(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd);
-		gFoc_Ctrl.in.s_cruiseRPM = motSpd;
-		gFoc_Ctrl.in.b_cruiseEna = enable;
-	}
-	
-	return true;
-}
-
-bool PMSM_FOC_PauseCruise(void) {
-	gFoc_Ctrl.in.b_cruiseEna = false;
-	return true;
-}
-
-bool PMSM_FOC_ResumeCruise(void) {
-	gFoc_Ctrl.in.b_cruiseEna = true;
-	eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, PMSM_FOC_GetSpeed(), CONFIG_CRUISE_RAMP_TIME);
-	eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
-	return true;
-}
-
-bool PMSM_FOC_Is_CruiseEnabled(void) {
-	return (gFoc_Ctrl.in.b_cruiseEna && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD));
-}
-
-bool PMSM_FOC_Set_TgtSpeed(float rpm) {
-	if (gFoc_Ctrl.in.b_cruiseEna) {
-		return false;
-	}
-	eCtrl_set_TgtSpeed(min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm));
-	return true;
-}
-
-
-bool PMSM_FOC_Set_Current(float is) {
-	if (is > gFoc_Ctrl.userLim.s_PhaseCurrLim) {
-		is = gFoc_Ctrl.userLim.s_PhaseCurrLim;
-	}else if (is < -gFoc_Ctrl.userLim.s_PhaseCurrLim) {
-		is = -gFoc_Ctrl.userLim.s_PhaseCurrLim;
-	}
-	eCtrl_set_TgtCurrent(is);
-	return true;
-}
-
-bool PMSM_FOC_Set_Torque(float trq) {
-	if (trq > gFoc_Ctrl.userLim.s_torqueLim) {
-		trq = gFoc_Ctrl.userLim.s_torqueLim;
-	}else if (trq < -gFoc_Ctrl.userLim.s_torqueLim) {
-		trq = -gFoc_Ctrl.userLim.s_torqueLim;
-	}
-	eCtrl_set_TgtTorque(trq);
-	return true;
-}
-
-void PMSM_FOC_Reset_Torque(void) {
-	float real_trq = PMSM_FOC_Get_Real_dqVector();
-	eCtrl_reset_Torque(real_trq);
-}
-
-bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
-	if (PMSM_FOC_Is_CruiseEnabled()) {
-		if (rpm < CONFIG_MIN_CRUISE_RPM) {
-			rpm = CONFIG_MIN_CRUISE_RPM;
-		}
-		gFoc_Ctrl.in.s_cruiseRPM = min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm);
-		eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
-		return true;
-	}
-	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
-	return false;
-}
-
-void PMSM_FOC_MTPA_Calibrate(bool enable) {
-	if (enable) {
-		gFoc_Ctrl.in.b_MTPA_calibrate = true;
-		gFoc_Ctrl.in.s_dqAngle = 0;
-	}else {
-		gFoc_Ctrl.in.s_dqAngle = INVALID_ANGLE;
-		gFoc_Ctrl.in.b_MTPA_calibrate = false;
-	}
-}
-
-void PMSM_FOC_Set_MotAngle(float angle) {
-	gFoc_Ctrl.in.s_manualAngle = (angle);
-}
-
-void PMSM_FOC_Set_Dq_Angle(float angle) {
-	gFoc_Ctrl.in.s_dqAngle = (angle);
-}
-
-void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
-	dq->d = gFoc_Ctrl.in.s_targetIdq.d;
-	dq->q = gFoc_Ctrl.in.s_targetIdq.q;
-}
-
-float PMSM_FOC_GetSpeed(void) {
-	float speed = gFoc_Ctrl.in.s_motVelocity;
-	if (!gFoc_Ctrl.in.b_motEnable || foc_observer_is_encoder()) {
-		speed = motor_encoder_get_speed();
-	}else {
-		if (foc_observer_sensorless_stable()) {
-			speed = foc_observer_sensorless_speed();
-		}else {
-			speed = 0;
-		}
-	}
-	return speed;
-}
-
-
-void PMSM_FOC_AutoHold(bool lock) {
-	if (gFoc_Ctrl.in.b_AutoHold != lock) {
-		motor_encoder_lock_pos(lock);
-		PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
-		if (!lock) {
-			float hold_torque = gFoc_Ctrl.in.s_targetTorque * 1.1f;
-			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
-#ifdef CONFIG_SPEED_LADRC
-				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, hold_torque);
-#else
-				PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, hold_torque);
-#endif
-			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
-#ifdef CONFIG_SPEED_LADRC
-				ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, hold_torque);
-#else
-				PI_Controller_Reset(&gFoc_Ctrl.pi_vel, hold_torque);
-#endif
-			}
-			etcs_reset_torque(hold_torque);
-			gFoc_Ctrl.out.f_autohold_trq = hold_torque;
-		}else {
-			gFoc_Ctrl.out.f_autohold_trq = 0;
-		}
-		gFoc_Ctrl.in.b_AutoHold = lock;
-	}
-}
-
-
-bool PMSM_FOC_AutoHoldding(void) {
-	return gFoc_Ctrl.in.b_AutoHold;
-}
-
-static PI_Controller *_pid(u8 id) {
-	PI_Controller *pi = NULL;
-	if (id == PID_ID_ID) {
-		pi = &gFoc_Ctrl.pi_id;
-	}else if (id == PID_IQ_ID) {
-		pi = &gFoc_Ctrl.pi_iq;
-	}else if (id == PID_VelLim_ID) {
-#ifndef CONFIG_SPEED_LADRC
-		pi = &gFoc_Ctrl.pi_vel_lim;
-#endif
-	}else if (id == PID_Vel_ID) {
-#ifndef CONFIG_SPEED_LADRC
-		pi = &gFoc_Ctrl.pi_vel;
-#endif
-	}else if (id == PID_AutoHold_ID) {
-		pi = &gFoc_Ctrl.pi_lock;
-	}
-	return pi;
-}
-
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd) {
-	if (id > PID_Max_ID) {
-		return;
-	}
-	PI_Controller *pi = _pid(id);
-	if (pi != NULL) {
-		pi->kp = kp;
-		pi->ki = ki;
-		pi->kd = kd;
-	}
-}
-
-void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kd) {
-	if (id > PID_Max_ID) {
-		return;
-	}
-	PI_Controller *pi = _pid(id);
-	if (pi != NULL) {
-		*kp = pi->kp;
-		*ki = pi->ki;
-		*kd = pi->kd;
-	}
-}
-
-void PMSM_FOC_SetErrCode(u8 error) {
-	if (gFoc_Ctrl.out.n_Error != error) {
-		gFoc_Ctrl.out.n_Error = error;
-	}
-}
-
-u8 PMSM_FOC_GetErrCode(void) {
-	return gFoc_Ctrl.out.n_Error;
-}
-
-
-void PMSM_FOC_Set_PlotType(Plot_t t) {
-	gFoc_Ctrl.plot_type = t;
-}
-//获取母线电流和实际输出电流矢量大小
-void PMSM_FOC_Calc_Current(void) {
-	float vd = gFoc_Ctrl.out.s_OutVdq.d - gFoc_Ctrl.out.s_OutVdqDTC.d * TWO_BY_THREE;
-	float vq = gFoc_Ctrl.out.s_OutVdq.q - gFoc_Ctrl.out.s_OutVdqDTC.q * TWO_BY_THREE;
-
-	float id = gFoc_Ctrl.out.s_FilterIdq.d;
-	float iq = gFoc_Ctrl.out.s_FilterIdq.q;
-    /*
-		根据公式（等幅值变换，功率不等）:
-		iDC x vDC = 3/2(iq x vq + id x vd);
-	*/
-	float m_pow = (vd * id + vq * iq);
-	float raw_idc = 0.0f;
-	float v_dc = get_vbus_float();
-	if (v_dc != 0.0f) {
-		raw_idc = m_pow / v_dc;
-	}
-	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC, raw_idc, 0.02f);
-
-	m_pow = (gFoc_Ctrl.out.s_SamplevDQ.d * id + gFoc_Ctrl.out.s_SamplevDQ.q * iq) * 1.5f;
-	if (v_dc != 0.0f) {
-		raw_idc = m_pow / v_dc;
-	}
-	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, raw_idc, 0.02f);
-
-	raw_idc = get_vbus_current();
-	if (raw_idc != NO_VALID_CURRENT) {
-		LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.05f);
-	}else {
-		gFoc_Ctrl.out.s_FilteriDC = gFoc_Ctrl.out.s_CalciDC;
-	}
-
-	gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
-
-}
-
-void PMSM_FOC_Brake(bool brake) {
-	gFoc_Ctrl.in.b_eBrake = brake;
-	if (gFoc_Ctrl.in.b_eBrake & gFoc_Ctrl.in.b_cruiseEna) {
-		gFoc_Ctrl.in.b_cruiseEna = false;
-	}
-	eCtrl_brake_signal(brake);
-}
-

Applications/foc/core/PMSM_FOC_Core_unused.h

@@ -1,320 +0,0 @@
-#ifndef _PMSM_FOC_Core_H__
-#define _PMSM_FOC_Core_H__
-#include "math/fix_math.h"
-#include "foc/core/PI_Controller.h"
-#include "foc/core/e_ctrl.h"
-#include "foc/core/adrc.h"
-
-typedef struct {
-	float a;
-	float b;
-}AB_t;
-
-typedef struct {
-	float d;
-	float q;
-}DQ_t;
-
-typedef enum {
-	EPM_Dir_None,
-	EPM_Dir_Back,
-	EPM_Dir_Forward,
-}EPM_Dir_t;
-
-typedef enum {
-	Plot_None,
-	Plot_Phase_curr,
-	Plot_DQ_Curr,
-	Plot_Phase_vol,
-	Plot_Spd_flow,
-	Plot_D_flow,
-	Plot_Q_flow,
-	Plot_D_Step,
-	Plot_Q_Step,
-	Plot_SMO_OBS,
-	Plot_t_Max,
-}Plot_t;
-
-typedef enum {
-	FOC_Success = 0,
-	FOC_NotAllowed = 1,
-	FOC_Have_CritiCal_Err,
-	FOC_Throttle_Err, //ready的时候检测到转把信号
-	FOC_NowAllowed_With_Speed,
-	FOC_Speed_TooLow,
-	FOC_NotCruiseMode,
-	FOC_Param_Err,
-	FOC_MEM_Err,
-	FOC_CRC_Err,
-	FOC_Unknow_Cmd,
-}FOC_ErrCode_t;
-
-typedef enum {
-	FOC_CRIT_OV_Vol_Err,
-	FOC_CRIT_UN_Vol_Err,
-	FOC_CRIT_ACC_OV_Err,
-	FOC_CRIT_ACC_Un_Err,
-	FOC_CRIT_Phase_Err,
-	FOC_CRIT_Encoder_Err, /* 编码器错误，可能还是可以骑行，取决无感是否稳定 */
-	FOC_CRIT_Angle_Err, /* FOC 角度错误，一般发生在编码器错误，同时无感没有稳定的情况下，必须要停机 */
-	FOC_CRIT_CURR_OFF_Err,
-	FOC_CRIT_H_MOS_Err,
-	FOC_CRIT_L_MOS_Err,
-	FOC_CRIT_Phase_Conn_Err,
-	FOC_CRIT_MOTOR_TEMP_Lim,
-	FOC_CRIT_MOS_TEMP_Lim,
-	FOC_CRIT_Fan_Err,
-	FOC_CRIT_IDC_OV,
-	FOC_CRIT_THRO_Err,
-	FOC_CRIT_ENC_AB_Err,
-	FOC_CRIT_Vol_HW_Err, //17
-	FOC_CRIT_PHASE_UNBalance_Err, /* 三相不平衡错误，比如相线螺丝松了 */
-	FOC_CRIT_THRO2_Err,
-	FOC_CRIT_MOT_TEMP_Sensor,
-	FOC_CRIT_MOS_TEMP_Sensor,  //21
-	FOC_CRIT_BRK_Err,
-	FOC_CRIT_Err_Max = 32,
-}FOC_CritiCal_Ebit_t;
-
-typedef enum {
-	FOC_EV_MOT_Limit_L=FOC_CRIT_Err_Max + 1,
-	FOC_EV_MOS_Limit_L,
-	FOC_EV_THRO_START_V,
-	FOC_START_Err_Code,
-}FOC_EVENT_R_t;
-
-#define FOC_Cri_Err_Mask(err) (1<<(err))
-
-typedef struct {
-	u8 	  n_poles;
-	float n_modulation;
-	float lq;
-	float ld;
-	float flux;
-}FOC_Params;
-
-typedef struct {
-	float 	s_iABC[3];
-	float 	s_iABCFilter[3];
-	float   s_iABC_DT[3]; //abc phase current for deadtime compesition
-	float   s_SamplePhaseV[3]; //相对地电压
-	float   s_motVelocity;   //from hall or encoder
-	float 	s_motAngle; //from hall or encoder
-	float   s_angleLast;
-	float   s_targetRPM;
-	float   s_cruiseRPM;
-	e_Ramp  cruiseRpmRamp;
-	float 	s_targetCurrent;
-	DQ_t    s_targetIdq;
-	DQ_t    s_targetVdq;
-	float   s_targetTorque; //限速后的实际扭矩
-	float 	s_vDC;
-	u8      n_ctlMode;
-	bool    b_motEnable;
-	bool    b_cruiseEna;
-	bool    b_AutoHold;
-	bool    b_eBrake;
-	bool    b_epmMode;
-	bool    b_fwEnable;
-	float   s_motVelocityFiltered; //电机滤波后的转速
-	float   s_motVelRadusPers; //电机的电角速度
-	volatile bool    b_MTPA_calibrate;
-	float   s_manualAngle; //mainly used when calibrate hall/mtpa.
-	float   s_dqAngle; //D轴电流超前角
-}FOC_InP;
-
-typedef struct {
-	float s_motRPMLim;
-	float s_torqueLim;
-	float s_iDCLim;
-	float s_PhaseCurrLim; //最大相电流
-	float s_iDCeBrkLim; //最大母线回收电流
-	float s_TorqueBrkLim;
-	float s_PhaseVoleBrkLim;
-	float s_vDCMinLim;
-	float s_vDCMaxLim;
-}FOC_UserLimit;
-
-typedef struct {
-	e_Ramp rpmLimRamp;
-	e_Ramp torqueLimRamp;
-	e_Ramp DCCurrLimRamp;
-}FOC_RTLimit;
-
-typedef struct {
-	float s_motRPMMax;
-	float s_PhaseCurrMax;
-	float s_PhaseVolMax;
-	float s_FWDCurrMax; //D轴最大退磁电流
-	float s_iDCMax;
-	float s_vDCMax;
-	float s_torqueMax;
-}FOC_HwLimit;
-
-typedef struct {
-	float s_iDCLim;
-	float s_TorqueLim;
-}FOC_ProtLimit;
-
-typedef struct {
-	u16   n_Duty[3];
-	u16   n_lowDuty;
-	u16   n_midDuty;
-	u8    n_Sector;
-	u8    n_CPhases;
-	u16   n_Sample1;
-	u16   n_Sample2;
-	u8    n_RunMode;
-	DQ_t  s_OutVdq;
-	DQ_t  s_OutVdqDTC;
-	AB_t  s_OutVAB;
-	DQ_t  s_RealIdq;
-	DQ_t  s_RealVdq;
-	DQ_t  s_FilterIdq;
-	AB_t  s_SampleAB;
-	DQ_t  s_SamplevDQ;
-	float s_FilteriDC;
-	float s_CalciDC;
-	float s_CalciDC2;
-	float s_RealCurrentFiltered;
-	float f_vdqRation;
-	float f_autohold_trq;
-	s16   test_sample;
-	float sin;
-	float cos;
-	u8    n_Error;
-	bool  empty_load; //空载运行
-}FOC_OutP;
-
-typedef struct {
-	float s_FinalTgt;
-	float s_Cp;
-	float s_Step;
-	int      n_CtrlCount;
-	int      n_StepCount;
-}dq_Rctrl; //dq ramp ctrl
-
-typedef struct {
-	PI_Controller pi_id;
-	PI_Controller pi_iq;
-	PI_Controller pi_lock;
-	PI_Controller pi_power;
-#ifdef CONFIG_SPEED_LADRC
-	ladrc_t        vel_lim_adrc;
-	ladrc_t        vel_adrc;
-#else
-	PI_Controller pi_vel_lim;
-	PI_Controller pi_vel;
-#endif
-	dq_Rctrl      idq_ctl[2];
-	dq_Rctrl      vdq_ctl[2];	
-	FOC_InP       in;
-	FOC_OutP      out;
-	FOC_Params    params;
-	FOC_UserLimit userLim;
-	FOC_HwLimit   hwLim;
-	FOC_ProtLimit protLim;
-	FOC_RTLimit   rtLim;
-	Plot_t        plot_type;
-	int           ctrl_count;
-}PMSM_FOC_Ctrl;
-
-#define CTRL_MODE_OPEN                      ((u8)0U)
-#define CTRL_MODE_SPD                       ((u8)1U)
-#define CTRL_MODE_TRQ                       ((u8)2U)
-#define CTRL_MODE_CURRENT                   ((u8)3U)
-#define CTRL_MODE_EBRAKE               ((u8)4U)
-
-#define FOC_CALIMOD_HALL               ((u8) 1U)
-#define FOC_CALIMOD_MTPA               ((u8) 2U)
-
-#define FOC_LIM_NO_CHANGE  0
-#define FOC_LIM_CHANGE_H   1
-#define FOC_LIM_CHANGE_L   2
-
-#if 1
-#define SECTOR_1  0u
-#define SECTOR_2  1u
-#define SECTOR_3  2u
-#define SECTOR_4  3u
-#define SECTOR_5  4u
-#define SECTOR_6  5u
-#define SECTOR_UKNOW 0xFF
-#else
-#define SECTOR_1  3u
-#define SECTOR_2  4u
-#define SECTOR_3  5u
-#define SECTOR_4  0u
-#define SECTOR_5  1u
-#define SECTOR_6  2u
-#endif
-
-PMSM_FOC_Ctrl *PMSM_FOC_Get(void);
-void PMSM_FOC_CoreInit(void);
-bool PMSM_FOC_Schedule(void);
-u8 PMSM_FOC_CtrlMode(void);
-void PMSM_FOC_idqCalc(void);
-void PMSM_FOC_Start(u8 nCtrlMode);
-void PMSM_FOC_Stop(void);
-void PMSM_FOC_DCCurrLimit(float ibusLimit);
-void PMSM_FOC_SpeedLimit(float speedLimit);
-float PMSM_FOC_GetSpeedLimit(void);
-float PMSM_FOC_GetVbusVoltage(void);
-float PMSM_FOC_GetVbusCurrent(void);
-DQ_t  *PMSM_FOC_GetDQCurrent(void);
-bool PMSM_FOC_SetCtrlMode(u8 mode);
-u8 PMSM_FOC_GetCtrlMode(void);
-void PMSM_FOC_SetOpenVdq(float vd, float vq);
-void PMSM_FOC_SetOpenVdq_Immediate(float vd, float vq);
-bool PMSM_FOC_EnableCruise(bool enable);
-bool PMSM_FOC_Set_TgtSpeed(float rpm);
-bool PMSM_FOC_Set_Torque(float trque);
-bool PMSM_FOC_Set_Current(float current);
-bool PMSM_FOC_Set_CruiseSpeed(float rpm);
-float PMSM_FOC_GetSpeed(void);
-bool PMSM_FOC_Lock_Motor(bool lock);
-void PMSM_FOC_Brake(bool brake);
-void PMSM_FOC_Calc_Current(void);
-void PMSM_FOC_AutoHold(bool lock);
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
-void PMSM_FOC_Set_MotAngle(float angle);
-void PMSM_FOC_Set_Dq_Angle(float angle);
-bool PMSM_FOC_Is_Start(void);
-void PMSM_FOC_SetErrCode(u8 error);
-u8 PMSM_FOC_GetErrCode(void);
-void PMSM_FOC_MTPA_Calibrate(bool enable);
-void PMSM_FOC_Get_TgtIDQ(DQ_t * dq);
-void PMSM_FOC_TorqueLimit(float torqueLimit);
-float PMSM_FOC_GetTorqueLimit(void);
-bool PMSM_FOC_Set_epmMode(bool epm);
-bool PMSM_FOC_is_epmMode(void);
-bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir);
-EPM_Dir_t PMSM_FOC_Get_epmDir(void);
-void PMSM_FOC_SetEbrkTorque(s16 torque);
-float PMSM_FOC_GetEbrkTorque(void);
-void PMSM_FOC_PhaseCurrLim(float lim);
-float PMSM_FOC_GetPhaseCurrLim(void);
-float PMSM_FOC_GetDCCurrLimit(void);
-void PMSM_FOC_GetRunningStatus(u8 *data);
-bool PMSM_FOC_Is_CruiseEnabled(void);
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
-void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kd);
-bool PMSM_FOC_AutoHoldding(void);
-void PMSM_FOC_Slow_Task(void);
-void PMSM_FOC_Set_PlotType(Plot_t t);
-u8   PMSM_FOC_RunTime_Limit(void);
-void PMSM_FOC_RT_PhaseCurrLim(float lim);
-void PMSM_FOC_RT_LimInit(void);
-float PMSM_FOC_Get_Real_dqVector(void);
-void PMSM_FOC_Reset_Torque(void);
-void PMSM_FOC_SpeedDirectLimit(float limit);
-bool PMSM_FOC_iDC_is_Limited(void);
-bool PMSM_FOC_Torque_is_Limited(void);
-bool PMSM_FOC_PauseCruise(void);
-bool PMSM_FOC_ResumeCruise(void);
-void PMSM_FOC_Change_VelLoop_Params(float wcv, float b0);
-void PMSM_FOC_Change_TrqLoop_Params(float wcv, float b0);
-void PMSM_FOC_ABC2Dq(float a, float b, float c, float *d, float *q);
-
-#endif /* _PMSM_FOC_Core_H__ */
-

Applications/foc/core/controller.c

@@ -23,15 +23,16 @@ void mot_contrl_init(mot_contrl_t *ctrl) {
 	ctrl->hwlim.dc_curr = CONFIG_HW_MAX_DC_CURRENT;
 	ctrl->hwlim.mot_vel = CONFIG_HW_MAX_MOTOR_RPM;
 	ctrl->hwlim.phase_curr = CONFIG_HW_MAX_PHASE_CURR;
-	ctrl->hwlim.phase_vol = CONFIG_HW_MAX_PHASE_VOL;
-	ctrl->hwlim.dc_vol      = CONFIG_HW_MAX_DC_VOLTAGE;
+	//ctrl->hwlim.phase_vol = CONFIG_HW_MAX_PHASE_VOL;
+	//ctrl->hwlim.dc_vol      = CONFIG_HW_MAX_DC_VOLTAGE;
 	ctrl->hwlim.torque  = mc_conf()->m.max_torque; //电机的最大扭矩
-	ctrl->hwlim.fw_id = mc_conf()->m.max_fw_id;  //电池能支持的最大弱磁电流
+	ctrl->hwlim.fw_id = mc_conf()->m.max_fw_id;  //电机能支持的最大弱磁电流
 	ctrl->protlim.dc_curr = HW_LIMIT_NONE;
 	ctrl->protlim.torque = HW_LIMIT_NONE;
 	ctrl->torque_acc_time = 500; //will be set after start
 	ctrl->torque_dec_time = 500; //will be set after start
 	ctrl->ebrk_ramp_time = 500;  //will be set after start
+	etcs_init(&ctrl->etcs);
 	foc_init(&ctrl->foc);
 }
 bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
@@ -39,21 +40,20 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 		return true;
 	}
 	if (start) {
-		line_ramp_init(&ctrl->torque_lim, CONFIG_LIMIT_RAMP_TIME);
-		line_ramp_init(&ctrl->dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
-		line_ramp_init(&ctrl->vel_lim, CONFIG_LIMIT_RAMP_TIME);
-		line_ramp_init(&ctrl->cruise_vel, CONFIG_CRUISE_RAMP_TIME);
-		line_ramp_init(&ctrl->target_vd, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
-		line_ramp_init(&ctrl->target_vq, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
-		line_ramp_init(&ctrl->target_vel, CONFIG_CRUISE_RAMP_TIME);
-		line_ramp_init(&ctrl->target_current, CONFIG_CURRENT_RAMP_TIME);
-		line_ramp_init(&ctrl->input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_torque_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_vel_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_cruise_vel, CONFIG_CRUISE_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_target_vd, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
+		line_ramp_init(&ctrl->ramp_target_vq, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
+		line_ramp_init(&ctrl->ramp_target_vel, CONFIG_CRUISE_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_target_current, CONFIG_CURRENT_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);
 		line_ramp_init(&ctrl->ramp_adv_angle, CONFIG_CURRENT_RAMP_TIME);
-		mot_contrl_pid(ctrl);
 		mot_contrl_ulimit(ctrl);
 		mot_contrl_rtlimit(ctrl);
-		etcs_init(&ctrl->etcs);
 	}
+	mot_contrl_pid(ctrl);
 	ctrl->b_ebrk_running = false;
 	ctrl->b_AutoHold = false;
 	ctrl->b_cruiseEna = false;
@@ -71,13 +71,13 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	ctrl->dc_curr_calc = 0;
 	ctrl->phase_curr_filted[0] = ctrl->phase_curr_filted[1] = 0;
 	ctrl->out_idq_filterd.d = ctrl->out_idq_filterd.q = 0;
+	ctrl->out_vdq_filterd.d = ctrl->out_vdq_filterd.q = 0;
 	ctrl->autohold_torque = 0;
 	ctrl->out_current_vec = 0;
 	ctrl->target_idq.d = 0;
 	ctrl->target_idq.q = 0;
 	ctrl->target_torque = 0;
 	ctrl->target_torque_raw = 0;
-
 	foc_init(&ctrl->foc);
 	foc_observer_init();
 	ctrl->b_start = start;
@@ -113,29 +113,31 @@ u8 mot_contrl_mode(mot_contrl_t *ctrl) {
 	}
 	if (preMode != ctrl->mode_running) {
 		if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_TRQ)) {
-			line_ramp_set_acctime(&ctrl->input_torque, ctrl->torque_acc_time);
-			line_ramp_set_dectime(&ctrl->input_torque, ctrl->torque_dec_time);
-			line_ramp_update(&ctrl->input_torque);
+			line_ramp_set_acctime(&ctrl->ramp_input_torque, ctrl->torque_acc_time);
+			line_ramp_set_dectime(&ctrl->ramp_input_torque, ctrl->torque_dec_time);
+			line_ramp_update(&ctrl->ramp_input_torque);
 			if (preMode == CTRL_MODE_SPD) {
 				ctrl->target_torque_raw = ctrl->target_torque;
 				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
 			}else if (preMode == CTRL_MODE_CURRENT) {
-				ctrl->target_torque_raw = line_ramp_get_interp(&ctrl->target_current);
+				ctrl->target_torque_raw = line_ramp_get_interp(&ctrl->ramp_target_current);
 				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
+			}else if (preMode == CTRL_MODE_EBRAKE) {
+				line_ramp_set_target(&ctrl->ramp_input_torque, 0);
 			}
 		}else if ((preMode == CTRL_MODE_TRQ) && (ctrl->mode_running == CTRL_MODE_SPD)) {
 			PI_Controller_Reset(&ctrl->pi_vel, ctrl->target_torque);
 		}else if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_EBRAKE)) {
-			line_ramp_reset(&ctrl->input_torque, ctrl->target_torque);
-			line_ramp_set_time(&ctrl->input_torque, ctrl->ebrk_ramp_time);
-			line_ramp_set_target(&ctrl->input_torque, motor_get_ebreak_toruqe(ctrl->foc.in.mot_velocity));
+			line_ramp_reset(&ctrl->ramp_input_torque, ctrl->target_torque);
+			line_ramp_set_time(&ctrl->ramp_input_torque, ctrl->torque_dec_time);
+			line_ramp_set_target(&ctrl->ramp_input_torque, 0);//先把扭矩快速减小到0
 		}else if ((preMode == CTRL_MODE_EBRAKE) && (ctrl->mode_running == CTRL_MODE_SPD)) {
 			PI_Controller_Reset(&ctrl->pi_vel, F_get_air());
 		}
 	}
 	if (ctrl->mode_running == CTRL_MODE_OPEN) {
-		line_ramp_step(&ctrl->target_vd);
-		line_ramp_step(&ctrl->target_vq);
+		line_ramp_step(&ctrl->ramp_target_vd);
+		line_ramp_step(&ctrl->ramp_target_vq);
 	}
 	return ctrl->mode_running;
 }
@@ -212,14 +214,47 @@ static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
 	}
 }
 
+static __INLINE void mot_contrl_update_phase_vol(mot_contrl_t *ctrl) {
+	float phase_vol[3];
+	get_uvw_phases_raw(phase_vol);
+	/* 三相端电压转到alpha-beta轴的相电压 */
+	ctrl->phase_v_ab.a = (2 * phase_vol[0] - phase_vol[1] - phase_vol[2])/3.0f;
+	ctrl->phase_v_ab.b = (phase_vol[1] - phase_vol[2]) * ONE_BY_SQRT3;
+	/* 当前电气频率   除于相电压低通滤波器截止频率 */
+	float We_hz = (ctrl->foc.in.mot_velocity / 60.0f * mc_conf()->m.poles);
+	float w_r_wc = We_hz / PHASE_VOL_LPF_BAND;
+	if (ctrl->mot_param_ind_freq > 10.0f) {//dq轴电感识别
+		w_r_wc = (ctrl->mot_param_ind_freq / (2.0f * M_PI)) / PHASE_VOL_LPF_BAND;
+	}
+	/* 计算低通滤波器的幅度补偿系数*/
+	float mag_mul = sqrtf(1 + SQ(w_r_wc));
+	ctrl->phase_v_ab.a *= mag_mul;
+	ctrl->phase_v_ab.b *= mag_mul;
+	/* 计算低通相位延时 */
+	float angle_rad = fast_atan2(We_hz, PHASE_VOL_LPF_BAND);
+	/* 延时半周期*/
+	angle_rad += (We_hz * 2 * M_PI * ctrl->foc.ts / 2);
+	norm_angle_rad(angle_rad);
+	/* 补偿 alpha-beta的相位 */
+	float sin,cos;
+	arm_sin_cos(-pi_2_degree(angle_rad), &sin, &cos);
+	float alpha = ctrl->phase_v_ab.a * cos + ctrl->phase_v_ab.b * sin;
+	float beta  = ctrl->phase_v_ab.b * cos - ctrl->phase_v_ab.a * sin;
+
+	ctrl->phase_v_ab.a = alpha;
+	ctrl->phase_v_ab.b = beta;
+}
 
 bool mot_contrl_update(mot_contrl_t *ctrl) {
 	foc_t *foc = &ctrl->foc;
 
+	mot_contrl_update_phase_vol(ctrl);
+
 	phase_current_get(foc->in.curr_abc);
 	clark(foc->in.curr_abc[0], foc->in.curr_abc[1], foc->in.curr_abc[2], &foc->in.curr_ab);
 
 	foc_observer_update(foc->out.vol_albeta.a * TWO_BY_THREE, foc->out.vol_albeta.b * TWO_BY_THREE, foc->in.curr_ab.a, foc->in.curr_ab.b);
+
 	float enc_angle = motor_encoder_get_angle();
 	float enc_vel = motor_encoder_get_speed();
 	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
@@ -244,37 +279,105 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 	foc->in.b_openloop = ctrl->mode_running == CTRL_MODE_OPEN;
 	phase_curr_unbal_check(ctrl);
 	if (foc->in.b_openloop) {
-		foc->in.target_vol_dq.d = line_ramp_get_interp(&ctrl->target_vd);
-		foc->in.target_vol_dq.q = line_ramp_get_interp(&ctrl->target_vq);
+		foc->in.target_vol_dq.d = line_ramp_get_interp(&ctrl->ramp_target_vd);
+		foc->in.target_vol_dq.q = line_ramp_get_interp(&ctrl->ramp_target_vq);
 	}
 
 	foc_update(foc);
 
-	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;
-	lowpass = fclamp(lowpass, 0.001f, 1.0f);
+	park(foc, &ctrl->phase_v_ab, &ctrl->phase_v_dq);
+
+	ctrl->duty_raw = NORM2_f(foc->out.vol_dq.d, foc->out.vol_dq.q)/(foc->in.dc_vol * CONFIG_SVM_MODULATION * SQRT3_BY_2);
+	LowPass_Filter(ctrl->duty_filterd, ctrl->duty_raw, 0.01f);
+	float lowpass = 0.001f;
 	LowPass_Filter(ctrl->out_idq_filterd.d, foc->out.curr_dq.d ,lowpass);
 	LowPass_Filter(ctrl->out_idq_filterd.q, foc->out.curr_dq.q ,lowpass);
+	LowPass_Filter(ctrl->out_vdq_filterd.d, foc->out.vol_dq.d ,lowpass/1.5f);
+	LowPass_Filter(ctrl->out_vdq_filterd.q, foc->out.vol_dq.q ,lowpass/1.5f);
+
+	/* 计算母线电流 */
+	float vd = ctrl->out_vdq_filterd.d;
+	float vq = ctrl->out_vdq_filterd.q;
+	float id = ctrl->out_idq_filterd.d;
+	float iq = ctrl->out_idq_filterd.q;
+    /*
+		根据公式（等幅值变换，功率不等）:
+		iDC x vDC = 3/2(iq x vq + id x vd);
+	*/
+	float m_pow = (vd * id + vq * iq);
+	float raw_idc = 0.0f;
+	float v_dc = get_vbus_float();
+	if (v_dc != 0.0f) {
+		raw_idc = m_pow / v_dc;
+	}
+	ctrl->dc_curr_calc = raw_idc;
 	return true;
 }
 
+#ifndef CONFIG_TORQUE_LUT
 static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
-	ctrl->pi_power.max = maxTrq;
-	float errRef = line_ramp_get_interp(&ctrl->dc_curr_lim) - ctrl->dc_curr_filted;
-	return PI_Controller_Run(&ctrl->pi_power, errRef);
-}
+	float dc_lim = line_ramp_get_interp(&ctrl->ramp_dc_curr_lim);
+	/* 计算iq的前馈,(母线功率 - d轴功率)/vq
+	 * 加上0.0001f 避免除0
+	*/
 
+	float r = ctrl->dc_curr_filted / (ctrl->dc_curr_calc + 0.0001f) + 0.0001f;
+	r = fclamp(ABS(r), 0.5f, 1.5f);
+	float q_axis_power = dc_lim * get_vbus_float() / r - ctrl->out_idq_filterd.d * ctrl->out_vdq_filterd.d;
+	float vq = ctrl->out_vdq_filterd.q;
+	if (vq < 1.0f) {
+		vq = 1.0f;
+	}
+	float iq_ff = q_axis_power / vq;
+	iq_ff = fabs(iq_ff);
+	if (iq_ff > maxTrq) {
+		ctrl->dc_lim_t_ff = maxTrq;
+	}else {
+		ctrl->dc_lim_t_ff = iq_ff;
+	}
+	/** 使用min(maxTrq, ctrl->dc_lim_t_ff * 1.1f)带入max的原因是为了防止积分器饱和后有过冲现象
+	 *  1.1f的系数是为了保证ctrl->dc_lim_t_ff 计算偏小的情况下功率能达到上限，这个系数太大不能
+	 *  防止过冲
+	*/
+	ctrl->pi_power.max = min(maxTrq, ctrl->dc_lim_t_ff * 1.1f);
+	float errRef = dc_lim - ctrl->dc_curr_filted;
+	return PI_Controller_Parallel(&ctrl->pi_power, errRef, ctrl->dc_lim_t_ff);
+}
+#else
+static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
+	float dc_lim = line_ramp_get_interp(&ctrl->ramp_dc_curr_lim);
+	/* 计算扭矩的前馈,(母线功率 * 9.55 * 电机效率)/(电机速度)
+	 * 加上0.0001f 避免除0
+	*/
+	float torque_ff = dc_lim * get_vbus_float() * 9.55f * 0.8f/(ctrl->foc.in.mot_velocity + 0.0001f);
+	torque_ff = ABS(torque_ff);
+	if (torque_ff > maxTrq) {
+		ctrl->dc_lim_t_ff = maxTrq;
+	}else if (torque_ff < 0) {
+		ctrl->dc_lim_t_ff = 0;
+	}else {
+		ctrl->dc_lim_t_ff = torque_ff;
+	}
+	/** 使用min(maxTrq, ctrl->dc_lim_t_ff * 1.1f)带入max的原因是为了防止积分器饱和后有过冲现象
+	 *  1.1f的系数是为了保证ctrl->dc_lim_t_ff 计算偏小的情况下功率能达到上限，这个系数太大不能
+	 *  防止过冲
+	*/
+	ctrl->pi_power.max = min(maxTrq, ctrl->dc_lim_t_ff * 1.1f);
+	float errRef = dc_lim - ctrl->dc_curr_filted;
+	return PI_Controller_Parallel(&ctrl->pi_power, errRef, ctrl->dc_lim_t_ff);
+}
+#endif
 static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
 	ctrl->pi_vel_lim.max = maxTrq;
 	ctrl->pi_vel_lim.min = 0;
-
-	float err = line_ramp_get_interp(&ctrl->vel_lim) - ctrl->foc.in.mot_velocity;
-	return PI_Controller_RunVel(&ctrl->pi_vel_lim, err);
+	float err = line_ramp_get_interp(&ctrl->ramp_vel_lim) - ctrl->foc.in.mot_velocity;
+	return PI_Controller_Parallel(&ctrl->pi_vel_lim, err, 0);
 }
 
 /* current vector or torque to dq axis current */
 static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
-		float target_current = line_ramp_get_interp(&ctrl->target_current);
+		float target_current = line_ramp_get_interp(&ctrl->ramp_target_current);
 		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
 			float s, c;
 			float angle_step = line_ramp_step(&ctrl->ramp_adv_angle);
@@ -285,7 +388,7 @@ static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 			}else if (ctrl->target_idq.d < -ctrl->hwlim.fw_id) {
 				ctrl->target_idq.d = -ctrl->hwlim.fw_id;
 			}
-			ctrl->target_idq.q = sqrtf(SQ(target_current) - SQ(ctrl->target_idq.d));
+			ctrl->target_idq.q = sqrtsub2_f(target_current, ctrl->target_idq.d);
 			if (s < 0) {
 				ctrl->target_idq.q = -ctrl->target_idq.q;
 			}
@@ -352,21 +455,21 @@ void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
 		ctrl->pi_lock.min = -hold_torque;
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
-		ctrl->target_torque = PI_Controller_Run(&ctrl->pi_lock ,errRef);
+		ctrl->target_torque = PI_Controller_Parallel(&ctrl->pi_lock ,errRef, 0);
 		mot_contrl_dq_assign(ctrl);
 		return;
 	}
 	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
-		line_ramp_step(&ctrl->target_current);
+		line_ramp_step(&ctrl->ramp_target_current);
 	}else if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
-		float maxTrq = line_ramp_step(&ctrl->input_torque);
-		if (line_ramp_get_target(&ctrl->input_torque) < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+		float maxTrq = line_ramp_step(&ctrl->ramp_input_torque);
+		if (line_ramp_get_target(&ctrl->ramp_input_torque) < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
 			maxTrq = 0;
 		}
 		crosszero_step_towards(&ctrl->target_torque, maxTrq);
 	}else if (ctrl->mode_running == CTRL_MODE_TRQ) {
-		float refTorque = line_ramp_step(&ctrl->input_torque);
-		refTorque = min(refTorque, line_ramp_get_interp(&ctrl->torque_lim)) * etcs_out;
+		float refTorque = line_ramp_step(&ctrl->ramp_input_torque);
+		refTorque = min(refTorque, line_ramp_get_interp(&ctrl->ramp_torque_lim)) * etcs_out;
 		float maxTrq = mot_contrl_vel_limiter(ctrl, refTorque);
 		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
 		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
@@ -374,13 +477,13 @@ void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
 		float refSpeed;
 		float maxSpeed;
 		if (ctrl->b_cruiseEna) {
-			refSpeed = line_ramp_step(&ctrl->cruise_vel);
-			maxSpeed = line_ramp_get_target(&ctrl->cruise_vel);
+			refSpeed = line_ramp_step(&ctrl->ramp_cruise_vel);
+			maxSpeed = line_ramp_get_target(&ctrl->ramp_cruise_vel);
 		}else {
-			refSpeed = line_ramp_step(&ctrl->target_vel);
-			maxSpeed = line_ramp_get_target(&ctrl->target_vel);
+			refSpeed = line_ramp_step(&ctrl->ramp_target_vel);
+			maxSpeed = line_ramp_get_target(&ctrl->ramp_target_vel);
 		}
-		float max_input = line_ramp_get_interp(&ctrl->torque_lim) * etcs_out;
+		float max_input = line_ramp_get_interp(&ctrl->ramp_torque_lim) * etcs_out;
 		if (maxSpeed >= 0) {
 			ctrl->pi_vel.max = max_input;
 #ifdef CONFIG_SERVO_MOTOR
@@ -402,7 +505,7 @@ void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
 			ctrl->pi_vel.min = 0; //防止倒转
 		}
 		float errRef = refSpeed - ctrl->foc.in.mot_velocity;
-		float maxTrq = PI_Controller_RunVel(&ctrl->pi_vel, errRef);
+		float maxTrq = PI_Controller_Parallel(&ctrl->pi_vel, errRef, 0);
 		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
 		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
 	}
@@ -449,24 +552,39 @@ static void mot_contrl_ulimit(mot_contrl_t *ctrl) {
 }
 
 static void mot_contrl_rtlimit(mot_contrl_t *ctrl) {
-	line_ramp_reset(&ctrl->torque_lim, ctrl->userlim.torque);
-	line_ramp_reset(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
-	line_ramp_reset(&ctrl->vel_lim, ctrl->userlim.mot_vel);
+	line_ramp_reset(&ctrl->ramp_torque_lim, ctrl->userlim.torque);
+	line_ramp_reset(&ctrl->ramp_dc_curr_lim, ctrl->userlim.dc_curr);
+	line_ramp_reset(&ctrl->ramp_vel_lim, ctrl->userlim.mot_vel);
 }
 
 void mot_contrl_slow_task(mot_contrl_t *ctrl) {
-	line_ramp_step(&ctrl->torque_lim);
-	line_ramp_step(&ctrl->dc_curr_lim);
-	line_ramp_step(&ctrl->vel_lim);
+	line_ramp_step(&ctrl->ramp_torque_lim);
+	line_ramp_step(&ctrl->ramp_dc_curr_lim);
+	line_ramp_step(&ctrl->ramp_vel_lim);
 	mot_contrl_dq_calc(ctrl);
 }
 
 
 u8 mot_contrl_protect(mot_contrl_t *ctrl) {
+	float torque_lim;
 	u8 changed = FOC_LIM_NO_CHANGE;
-	float dc_lim = (float)vbus_under_vol_limit();
-	float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
-	
+	float dc_lim = (float)vbus_voltage_low_limit();
+	float mot_lim_r = motor_temp_high_limit();
+	float mos_lim_r = mos_temp_high_limit();
+	float mos_torque = HW_LIMIT_NONE;
+	float mot_torque = HW_LIMIT_NONE;
+	float mot_idc = HW_LIMIT_NONE;
+	if (mot_lim_r < 1.0f) {
+		mot_torque = mot_lim_r * mc_gear_conf()->max_torque;
+		mot_idc    = mot_lim_r * mc_gear_conf()->max_idc;
+	}
+	if (mos_lim_r < 1.0f) {
+		mos_torque = mos_lim_r * mc_gear_conf()->max_torque;
+	}
+
+	dc_lim = min(dc_lim, mot_idc);
+	torque_lim = min(mos_torque, mot_torque);
+
 	if (ctrl->protlim.dc_curr != dc_lim || ctrl->protlim.torque != torque_lim) {
 		if ((dc_lim > ctrl->protlim.dc_curr) || (torque_lim > ctrl->protlim.torque)) {
 			changed = FOC_LIM_CHANGE_H;
@@ -519,11 +637,11 @@ void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit) {
 		ibusLimit = min(ibusLimit, ctrl->protlim.dc_curr);
 	}
 	ctrl->userlim.dc_curr = ibusLimit;
-	if (ABS(ctrl->dc_curr_filted) <= ibusLimit){
-		line_ramp_reset(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
-	}else {
-		line_ramp_set_target(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
-	}
+	/*
+	 * 这个地方不能像限制扭矩和速度那样从当前实际母线电流开始做ramp
+     * 因为用了功率环前馈，会导致扭矩突然下降
+	*/
+	line_ramp_set_target(&ctrl->ramp_dc_curr_lim, ctrl->userlim.dc_curr);
 }
 
 void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel) {
@@ -531,16 +649,13 @@ void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel) {
 		vel = ctrl->hwlim.mot_vel;
 	}
 	ctrl->userlim.mot_vel = vel;
-	if (ABS(ctrl->foc.in.mot_velocity) <= vel) {
-		line_ramp_reset(&ctrl->vel_lim, ctrl->userlim.mot_vel);
-	}else {
-		line_ramp_set_target(&ctrl->vel_lim, ctrl->userlim.mot_vel);
-	}
+	line_ramp_reset(&ctrl->ramp_vel_lim, ctrl->foc.in.mot_velocity);//从当前实际转速做ramp
+	line_ramp_set_target(&ctrl->ramp_vel_lim, ctrl->userlim.mot_vel);
 }
 
 void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time) {
-	line_ramp_set_time(&ctrl->vel_lim, (float)time);
-	line_ramp_update(&ctrl->vel_lim);
+	line_ramp_set_time(&ctrl->ramp_vel_lim, (float)time);
+	line_ramp_update(&ctrl->ramp_vel_lim);
 }
 
 
@@ -552,16 +667,13 @@ void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque) {
 		torque = min(torque, ctrl->protlim.torque);
 	}
 	ctrl->userlim.torque = torque;
-	if (ABS(ctrl->target_torque) <= torque){
-		line_ramp_reset(&ctrl->torque_lim, ctrl->userlim.torque);
-	}else {
-		line_ramp_set_target(&ctrl->torque_lim, ctrl->userlim.torque);
-	}
+	line_ramp_reset(&ctrl->ramp_torque_lim, ctrl->target_torque_raw);//从当前给定的torque做ramp
+	line_ramp_set_target(&ctrl->ramp_torque_lim, ctrl->userlim.torque);
 }
 
 void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time) {
-	line_ramp_set_time(&ctrl->torque_lim, (float)time);
-	line_ramp_update(&ctrl->torque_lim);
+	line_ramp_set_time(&ctrl->ramp_torque_lim, (float)time);
+	line_ramp_update(&ctrl->ramp_torque_lim);
 }
 
 float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl) {
@@ -573,20 +685,20 @@ float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl) {
 
 void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time) {
 	ctrl->ebrk_ramp_time = time;
-	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
-		line_ramp_set_time(&ctrl->input_torque, time);
-		line_ramp_update(&ctrl->input_torque);
+	if ((ctrl->mode_running == CTRL_MODE_EBRAKE) && (time != ctrl->ramp_input_torque.time_dec)) {
+		line_ramp_set_time(&ctrl->ramp_input_torque, time);
+		line_ramp_update(&ctrl->ramp_input_torque);
 	}
 }
 
 void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq) {
-	line_ramp_set_target(&ctrl->target_vd, vd);
-	line_ramp_set_target(&ctrl->target_vq, vq);
+	line_ramp_set_target(&ctrl->ramp_target_vd, vd);
+	line_ramp_set_target(&ctrl->ramp_target_vq, vq);
 }
 
 void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq) {
-	line_ramp_reset(&ctrl->target_vd, vd);
-	line_ramp_reset(&ctrl->target_vq, vq);
+	line_ramp_reset(&ctrl->ramp_target_vd, vd);
+	line_ramp_reset(&ctrl->ramp_target_vq, vq);
 }
 
 bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable) {
@@ -596,7 +708,7 @@ bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable) {
 			mot_contrl_set_error(ctrl, FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		line_ramp_reset(&ctrl->cruise_vel, motSpd);
+		line_ramp_reset(&ctrl->ramp_cruise_vel, motSpd);
 		ctrl->b_cruiseEna = enable;
 	}
 	return true;
@@ -605,7 +717,7 @@ bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable) {
 
 bool mot_contrl_resume_cruise(mot_contrl_t *ctrl) {
 	ctrl->b_cruiseEna = true;
-	line_ramp_set_time(&ctrl->cruise_vel, CONFIG_CRUISE_RAMP_TIME);
+	line_ramp_set_time(&ctrl->ramp_cruise_vel, CONFIG_CRUISE_RAMP_TIME);
 	return true;
 }
 
@@ -615,7 +727,7 @@ bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm) {
 			rpm = CONFIG_MIN_CRUISE_RPM;
 		}
 		float vel = min(ABS(rpm),ctrl->userlim.mot_vel)*SIGN(rpm);
-		line_ramp_set_target(&ctrl->cruise_vel, vel);
+		line_ramp_set_target(&ctrl->ramp_cruise_vel, vel);
 		return true;
 	}
 	mot_contrl_set_error(ctrl, FOC_NotCruiseMode);
@@ -624,7 +736,7 @@ bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm) {
 
 bool mot_contrl_set_current(mot_contrl_t *ctrl, float is) {
 	is = fclamp(is, -ctrl->userlim.phase_curr, ctrl->userlim.phase_curr);
-	line_ramp_set_target(&ctrl->target_current, is);
+	line_ramp_set_target(&ctrl->ramp_target_current, is);
 	return true;
 }
 
@@ -632,17 +744,17 @@ void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec) {
 	ctrl->torque_acc_time = acc;
 	ctrl->torque_dec_time = dec;
 	if (ctrl->mode_running == CTRL_MODE_TRQ) {
-		line_ramp_set_acctime(&ctrl->input_torque, acc);
-		line_ramp_set_dectime(&ctrl->input_torque, dec);
-		line_ramp_update(&ctrl->input_torque);
+		line_ramp_set_acctime(&ctrl->ramp_input_torque, acc);
+		line_ramp_set_dectime(&ctrl->ramp_input_torque, dec);
+		line_ramp_update(&ctrl->ramp_input_torque);
 	}
 }
 
 void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc) {
 	ctrl->torque_acc_time = acc;
 	if (ctrl->mode_running == CTRL_MODE_TRQ) {
-		line_ramp_set_acctime(&ctrl->input_torque, acc);
-		line_ramp_update(&ctrl->input_torque);
+		line_ramp_set_acctime(&ctrl->ramp_input_torque, acc);
+		line_ramp_update(&ctrl->ramp_input_torque);
 	}
 }
 
@@ -653,11 +765,11 @@ bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque) {
 	float torque_min = 0;
 	float torque_max = ctrl->userlim.torque;
 	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
-		torque_min = -ctrl->userlim.torque;
+		torque_min = -ctrl->userlim.ebrk_torque;
 		torque_max = 0;
 	}
 	torque = fclamp(torque, torque_min, torque_max);
-	line_ramp_set_target(&ctrl->input_torque, torque);
+	line_ramp_set_target(&ctrl->ramp_input_torque, torque);
 	return true;
 }
 
@@ -673,7 +785,7 @@ bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque) {
 		torque_max = 0;
 	}
 	torque = fclamp(torque, torque_min, torque_max);
-	line_ramp_set_target(&ctrl->input_torque, torque);
+	line_ramp_set_target(&ctrl->ramp_input_torque, torque);
 	return true;
 }
 
@@ -700,7 +812,7 @@ void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock) {
 			}else if (ctrl->mode_running == CTRL_MODE_SPD) {
 				PI_Controller_Reset(&ctrl->pi_vel, hold_torque);
 			}
-			line_ramp_reset(&ctrl->input_torque, hold_torque);
+			line_ramp_reset(&ctrl->ramp_input_torque, hold_torque);
 			ctrl->autohold_torque = hold_torque;
 		}else {
 			ctrl->autohold_torque = 0;
@@ -713,7 +825,7 @@ static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl) {
 	return (ctrl->b_hw_braker && mc_hwbrk_can_shutpower());
 }
 
-bool mot_contrl_energy_recovery(mot_contrl_t *ctrl, bool start) {
+bool mot_contrl_set_ebreak(mot_contrl_t *ctrl, bool start) {
 	bool enable = ctrl->b_ebrk_running;
 	if (mot_contrl_get_ebrk_torque(ctrl) == 0) {
 		enable = false;
@@ -739,8 +851,8 @@ void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake) {
 		ctrl->b_hw_braker = hw_brake;
 	}
 	if (is_hw_brake_shutting_power(ctrl)) {
-		if (!ctrl->b_ebrk_running && !mot_contrl_energy_recovery(ctrl, true)) {
-			line_ramp_reset(&ctrl->input_torque, 0);
+		if (!ctrl->b_ebrk_running && !mot_contrl_set_ebreak(ctrl, true)) {
+			line_ramp_reset(&ctrl->ramp_input_torque, 0);
 		}
 	}
 	cpu_exit_critical(mask);
@@ -769,9 +881,11 @@ void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd)
 	}
 	PI_Controller *pi = _pid(ctrl, id);
 	if (pi != NULL) {
+		u32 mask = cpu_enter_critical();
 		pi->kp = kp;
 		pi->ki = ki;
 		pi->kd = kd;
+		cpu_exit_critical(mask);
 	}
 }
 
@@ -788,30 +902,16 @@ void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *
 }
 
 void mot_contrl_calc_current(mot_contrl_t *ctrl) {
-	float vd = ctrl->foc.out.vol_dq.d;
-	float vq = ctrl->foc.out.vol_dq.q;
-
 	float id = ctrl->out_idq_filterd.d;
 	float iq = ctrl->out_idq_filterd.q;
-    /*
-		根据公式（等幅值变换，功率不等）:
-		iDC x vDC = 3/2(iq x vq + id x vd);
-	*/
-	float m_pow = (vd * id + vq * iq);
-	float raw_idc = 0.0f;
-	float v_dc = get_vbus_float();
-	if (v_dc != 0.0f) {
-		raw_idc = m_pow / v_dc;
-	}
-	LowPass_Filter(ctrl->dc_curr_calc, raw_idc, 0.02f);
 
-	raw_idc = get_vbus_current();
+	float raw_idc = get_vbus_current();
 	if (raw_idc != NO_VALID_CURRENT) {
-		LowPass_Filter(ctrl->dc_curr_filted, raw_idc, 0.05f);
+		LowPass_Filter(ctrl->dc_curr_filted, raw_idc, 0.04f);
 	}else {
 		ctrl->dc_curr_filted = ctrl->dc_curr_calc;
 	}
-	ctrl->out_current_vec = sqrtf(SQ(id) + SQ(iq));
+	ctrl->out_current_vec = NORM2_f(id, iq);
 }
 
 

Applications/foc/core/controller.h

@@ -79,6 +79,7 @@ typedef enum {
 	FOC_EV_THRO_START_V,
 	FOC_START_Err_Code,
 	FOC_Reset_Reson,
+	FOC_NV_Err,
 }ctrl_event_r_t;
 
 
@@ -102,10 +103,10 @@ typedef struct {
 typedef struct {
 	float mot_vel;
 	float phase_curr;
-	float phase_vol;
+	//float phase_vol;
 	float fw_id; //D轴最大退磁电流
 	float dc_curr;
-	float dc_vol;
+	//float dc_vol;
 	float torque;
 }hw_limit;
 
@@ -128,16 +129,23 @@ typedef struct{
 	float           target_torque_raw; //扭矩过零点处理前的扭矩
 	dq_t			target_idq;
 	dq_t            out_idq_filterd;
+	dq_t            out_vdq_filterd;
+	albt_t			phase_v_ab; //sampled
+	dq_t			phase_v_dq;
 	float 			out_current_vec;
 	u32				ebrk_ramp_time;
 	u32				torque_acc_time;
 	u32				torque_dec_time;
 	float   		dc_curr_filted;
 	float			dc_curr_calc;
+	float 			dc_lim_t_ff;
 	float   		phase_curr_filted[3];
 	float           autohold_torque;
+	float			duty_raw;
+	float           duty_filterd;
 	u8 				mode_req;
 	u8 				mode_running;
+	float			mot_param_ind_freq;
 	u8      		error;
 	foc_t 			foc;
 	etcs_t          etcs;
@@ -148,15 +156,15 @@ typedef struct{
 	user_limit 		userlim;
 	hw_limit   		hwlim;
 	prot_limit 		protlim;
-	lineramp_t   	target_current;
-	lineramp_t 		cruise_vel;
-	lineramp_t 		target_vd;
-	lineramp_t 		target_vq;
-	lineramp_t		target_vel;
-	lineramp_t 		vel_lim;
-	lineramp_t 		torque_lim;
-	lineramp_t 		dc_curr_lim;
-	lineramp_t		input_torque;
+	lineramp_t   	ramp_target_current;
+	lineramp_t 		ramp_cruise_vel;
+	lineramp_t 		ramp_target_vd;
+	lineramp_t 		ramp_target_vq;
+	lineramp_t		ramp_target_vel;
+	lineramp_t 		ramp_vel_lim;
+	lineramp_t 		ramp_torque_lim;
+	lineramp_t 		ramp_dc_curr_lim;
+	lineramp_t		ramp_input_torque;
 	lineramp_t		ramp_adv_angle;
 	float 			phase_a_max;
 	float 			phase_b_max;
@@ -190,7 +198,7 @@ void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc);
 bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque);
 void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable);
 void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock);
-bool mot_contrl_energy_recovery(mot_contrl_t *ctrl, bool start);
+bool mot_contrl_set_ebreak(mot_contrl_t *ctrl, bool start);
 void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake);
 void mot_contrl_calc_current(mot_contrl_t *ctrl);
 void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd);
@@ -244,13 +252,13 @@ static __INLINE bool mot_contrl_set_target_vel(mot_contrl_t *ctrl, float vel) {
 	if (ctrl->b_cruiseEna) {
 		return false;
 	}
-	line_ramp_set_target(&ctrl->target_vel ,min(ABS(vel), ctrl->userlim.mot_vel)*SIGN(vel));
+	line_ramp_set_target(&ctrl->ramp_target_vel ,min(ABS(vel), ctrl->userlim.mot_vel)*SIGN(vel));
 	return true;
 }
 
 static __INLINE void mot_contrl_set_vel_rttime(mot_contrl_t *ctrl, u32 time) {
-	line_ramp_set_time(&ctrl->target_vel, (float)time);
-	line_ramp_update(&ctrl->target_vel);
+	line_ramp_set_time(&ctrl->ramp_target_vel, (float)time);
+	line_ramp_update(&ctrl->ramp_target_vel);
 }
 
 
@@ -284,11 +292,20 @@ static __INLINE bool mot_contrl_ebrk_is_running(mot_contrl_t *ctrl) {
 }
 
 static __INLINE float mot_contrl_get_final_torque(mot_contrl_t *ctrl) {
-	return ctrl->input_torque.target;
+	return ctrl->ramp_input_torque.target;
 }
 
 static __INLINE u32 mot_contrl_get_torque_acc_time(mot_contrl_t *ctrl) {
-	return ctrl->input_torque.time;
+	return ctrl->ramp_input_torque.time;
+}
+
+static __INLINE s16 mot_contrl_get_speed_abs(mot_contrl_t *ctrl) {
+	s16 speed = (s16)mot_contrl_get_speed(ctrl);
+	return ABS(speed);
+}
+
+static __INLINE void mot_ctrl_set_ind_freq(mot_contrl_t *ctrl, float freq) {
+	ctrl->mot_param_ind_freq = freq;
 }
 
 #endif /* _CONTROLLER_H__ */

Applications/foc/core/e_ctrl_unused.h

@@ -1,255 +0,0 @@
-#ifndef EBRAKE_CTRL_H__
-#define EBRAKE_CTRL_H__
-#include "os/os_types.h"
-#include "foc/core/ramp_ctrl.h"
-#include "foc/foc_config.h"
-#include "math/fast_math.h"
-#include "math/fix_math.h"
-
-typedef struct {
-	float start;
-	float target;
-	float interpolation;
-	float step_val;
-	float first_target;
-	float first_step;
-	float A;
-	float acct;
-	float dect;
-	float time;
-}e_Ramp;
-
-typedef struct {
-	u16  ebrk_time; //能量回收，时间越短，刹车性能或者回收越好
-	u16  accl_time; //加速时间（ms），时间越短，加速性能越好
-	u16  dec_time;  //降速时间
-	bool hw_brake;
-	bool is_ebrake;
-	u32  brake_ts;//检测到刹车开始时间
-	e_Ramp current;
-	e_Ramp torque;
-	e_Ramp speed;
-	u16  ebrk_time_shadow;
-	u16  accl_time_shadow;
-	u16  dec_time_shadow;
-	float ebrake_current;
-	float current_shadow;
-	float torque_shadow;
-	float speed_shadow;
-}e_Ctrl;
-
-static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
-	r->start = 0;
-	r->target = 0;
-	r->first_target = 0;
-	r->interpolation = 0;
-	r->step_val = 0;
-	r->first_step = 0;
-	r->acct = (float)acc;
-	r->dect = (float)dec;
-}
-
-static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
-	r->start = target;
-	r->target = target;
-	r->first_target = target;
-	r->interpolation = target;
-	r->step_val = 0;
-	r->first_step = 0;
-	r->acct = (float)acc;
-	r->dect = (float)dec;
-}
-
-static void eRamp_init_target2(e_Ramp *r, float target, u32 time) {
-	eRamp_init_target(r, target, time, time);
-} 
-
-
-static void eRamp_reset_target(e_Ramp *r, float target) {
-	r->start = target;
-	r->target = target;
-	r->first_target = target;
-	r->interpolation = target;
-	r->step_val = 0;
-	r->first_step = 0;
-}
-static void eRamp_set_time(e_Ramp *r, u32 acc, u32 dec) {
-	r->acct = (float)acc;
-	r->dect = (float)dec;
-}
-
-static void eRamp_set_target(e_Ramp *r, float target) {
-	r->target = target;
-}
-static void eRamp_set_step(e_Ramp *r, float step) {
-	r->step_val = step;
-}
-
-static void eRamp_running(e_Ramp *r) {
-	float target = r->interpolation + r->step_val;
-	if (r->step_val < 0) {
-		if (target < r->target) {
-			target = r->target;
-		}
-	}else {
-		if (target > r->target) {
-			target = r->target;
-		}
-	}
-	r->interpolation = target;	
-}
-
-static float eRamp_get_intepolation(e_Ramp *r) {
-	return r->interpolation;
-}
-
-static float eRamp_get_target(e_Ramp *r) {
-	return r->target;
-}
-
-static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
-	float c_now = eRamp_get_intepolation(ramp);
-	float step_val = 0;
-	float delta = c - c_now;
-	float step_ms = intval;
-
-	if (delta >= 0) {
-		step_val = (delta)/(ramp->acct/step_ms);
-	}else {
-		step_val = (delta)/(ramp->dect/step_ms);
-	}
-	eRamp_set_target(ramp, c);
-	eRamp_set_step(ramp, step_val);
-}
-
-#if 0
-extern float PMSM_FOC_GetSpeed(void);
-static void eRamp_X2_running(e_Ramp *r) {
-#if 1
-	float target = r->target;
-	float v_now = r->interpolation;
-	bool cross_zero = false;
-	if (target > 0) {
-		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET * 1.5f) {
-			if (PMSM_FOC_GetSpeed() <= 20.0f) {
-				step_towards(&r->interpolation, target, 0.02f);
-			}else {
-				step_towards(&r->interpolation, target, 0.04f);
-			}
-			cross_zero = true;
-		}
-	}else if (target == 0) {
-		if (v_now >= 0 && v_now <= CONFIG_RAMP_SECOND_TARGET) {
-			step_towards(&r->interpolation, target, 0.01f);
-			cross_zero = true;
-		}
-	}else {
-		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET) {
-			step_towards(&r->interpolation, target, 0.01f);
-			cross_zero = true;
-		}
-	}
-	if (!cross_zero) {
-		step_towards(&r->interpolation, target, 1.0f);
-	}
-#else
-	if (r->first_step != 0) {
-		float interpolation = r->interpolation + r->first_step;
-		if ((r->first_step > 0) && (interpolation >= r->first_target)) {
-			interpolation = r->first_target;
-			r->first_step = r->first_target = 0;
-		}else if ((r->first_step < 0) && (interpolation <= r->first_target)) {
-			interpolation = r->first_target;
-			r->first_step = r->first_target = 0;
-		}
-		r->interpolation = interpolation;
-		return;
-	}
-	
-	eRamp_running(r);
-#endif
-}
-#else
-static void eRamp_X2_running(e_Ramp *r) {
-	eRamp_running(r);
-}
-#endif
-
-#if 0
-static void eRamp_set_X2_target(e_Ramp *r, float c) {
-#if 1
-	eRamp_set_target(r, c);
-#else
-	float c_now = eRamp_get_intepolation(ramp);
-	float step_val = 0;
-	float delta = c - c_now;
-
-	float step_ms = CONFIG_eCTRL_STEP_TS;	
-	if (delta > 0) {
-		step_val = (delta)/(ramp->acct/step_ms);
-		if (step_val > CONFIG_RAMP_SECOND_STEP) {
-			float first_delta = min(delta, CONFIG_RAMP_SECOND_TARGET);
-			ramp->first_target = c_now + first_delta;
-			ramp->first_step = CONFIG_RAMP_SECOND_STEP;
-			delta -= first_delta;
-			step_val = (delta)/(ramp->acct/step_ms);
-		}else {
-			ramp->first_target = ramp->first_step = 0.0f;
-		}
-	}else if (delta < 0){
-		step_val = (delta)/(ramp->dect/step_ms);
-		if (ABS(step_val) > CONFIG_RAMP_SECOND_STEP) {
-			float first_delta = MAX(delta, -CONFIG_RAMP_SECOND_TARGET);
-			ramp->first_target = c_now + first_delta;
-			ramp->first_step = -CONFIG_RAMP_SECOND_STEP;
-			delta -= first_delta;
-			step_val = (delta)/(ramp->dect/step_ms);
-		}else {
-			ramp->first_target = ramp->first_step = 0.0f;
-		}
-	}else {
-		step_val = 0;
-		ramp->first_step = ramp->first_target = 0;
-	}
-	eRamp_set_target(ramp, c);
-	eRamp_set_step(ramp, step_val);
-
-#endif
-}
-#else
-static void eRamp_set_X2_target(e_Ramp *r, float c) {
-	eRamp_set_step_target(r, c, CONFIG_eCTRL_STEP_TS);
-}
-
-#endif
-extern e_Ctrl g_eCtrl;
-static u16 eCtrl_get_torque_acc_time(void) {
-	return (u16)g_eCtrl.torque.acct;
-}
-
-static void eCtrl_Set_eBrk_RampTime(u16 t) {
-	g_eCtrl.ebrk_time_shadow = t;
-}
-
-//y=Ax^2;
-void eCtrl_init(u16 accl_time, u16 dec_time);
-void eCtrl_brake_signal(bool hw_brake);
-bool eCtrl_is_eBrk_Running(void);
-void eCtrl_set_TgtCurrent(float c);
-void eCtrl_set_TgtTorque(float t);
-void eCtrl_set_TgtSpeed(float s);
-bool eCtrl_enable_eBrake(bool enable);
-float eCtrl_get_RefSpeed(void);
-float eCtrl_get_RefCurrent(void);
-float eCtrl_get_RefTorque(void);
-float eCtrl_get_FinalSpeed(void);
-float eCtrl_get_FinalCurrent(void);
-float eCtrl_get_FinalTorque(void);
-void eCtrl_Running(void);
-void eCtrl_Reset(void);
-void eCtrl_reset_Torque(float init_trq);
-void eCtrl_reset_Current(float init_curr);
-void eCtrl_set_accl_time(u16 time);
-
-#endif /* EBRAKE_CTRL_H__ */
-

Applications/foc/core/foc.c

@@ -72,28 +72,30 @@ void foc_update(foc_t *foc) {
 		foc->daxis.min = -max_vd;
 		float err = line_ramp_step(&foc->in.target_id) - foc->out.curr_dq.d;
 		float id_ff = id_feedforward(foc);
-		foc->in.target_vol_dq.d = PI_Controller_Current(&foc->daxis, err, id_ff);
+		foc->in.target_vol_dq.d = PI_Controller_Serial(&foc->daxis, err, id_ff);
 
 		/* limiter Vq output for PI controller */
-		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
+		float max_vq = sqrtsub2_f(max_vd, foc->in.target_vol_dq.d);
 		foc->qaxis.max = max_vq;
 		foc->qaxis.min = -max_vq;
 		err = line_ramp_step(&foc->in.target_iq) - foc->out.curr_dq.q;
 		float iq_ff = iq_feedforward(foc);
-		foc->in.target_vol_dq.q = PI_Controller_Current(&foc->qaxis, err, iq_ff);
+		foc->in.target_vol_dq.q = PI_Controller_Serial(&foc->qaxis, err, iq_ff);
 	}else {
 		float max_vd = max_Vdc * SQRT3_BY_2;
 		foc->in.target_vol_dq.d = fclamp(foc->in.target_vol_dq.d, -max_vd, max_vd);
-		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
+		float max_vq = sqrtsub2_f(max_vd, foc->in.target_vol_dq.d);
 		foc->in.target_vol_dq.q = fclamp(foc->in.target_vol_dq.q, -max_vq, max_vq);
 	}
 
 	foc->out.vol_dq.d = foc->in.target_vol_dq.d;
 	foc->out.vol_dq.q = foc->in.target_vol_dq.q;
 #ifdef CONFIG_Volvec_Delay_Comp
+	float sin_old = foc->sin;
+	float cos_old = foc->cos;
 	if (foc->mot_velocity_filterd >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
-		float vol_dq_angle = foc->in.mot_angle + foc->mot_velocity_filterd / PI * 180.0f * (foc->ts * 0.8f);
-		rand_angle(vol_dq_angle);
+		float vol_dq_angle = foc->in.mot_angle + foc->mot_vel_radusPers / PI * 180.0f * (foc->ts * 0.8f);
+		norm_angle_deg(vol_dq_angle);
 		arm_sin_cos(vol_dq_angle, &foc->sin, &foc->cos);
 	}
 #endif
@@ -105,5 +107,9 @@ void foc_update(foc_t *foc) {
 	
 	pwm_update_duty(foc->out.duty[0], foc->out.duty[1], foc->out.duty[2]);
 	pwm_update_sample(foc->out.sample1, foc->out.sample2, foc->out.sample_phase);
+#ifdef CONFIG_Volvec_Delay_Comp
+	foc->sin = sin_old;
+	foc->cos = cos_old;
+#endif
 }
 

Applications/foc/core/foc_wapper.c

@@ -1,52 +0,0 @@
-/*
- *	参考 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;
-}
-
-P *PMSM_FOC_GetParams(void) {
-	return &rtP;
-}
-
-void PMSM_FOC_Step(void) {
-	
-	PMSM_Controller_step(rtMPtr);
-}
-
-

Applications/foc/core/ladrc_observer.c

@@ -5,8 +5,6 @@
 
 static ladrc_observer observer;
 
-#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
-
 static __inline float ladrc_observer_band(float vel) {
 	float ration = vel / observer.vel_min;
 	float Wo = observer.Wo;
@@ -78,9 +76,9 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 	
 	observer.Ebeta = observer.beta.z2 * (-observer.ld);
 
-	float angle = fast_atan_2(-observer.Ealpha, observer.Ebeta);
+	float angle = fast_atan2(-observer.Ealpha, observer.Ebeta);
 	UTILS_NAN_ZERO(angle);
-	angle_clamp(angle);
+	norm_angle_rad(angle);
 
 	/* 速度计算 */
 	float delta_angle = angle - observer.angle_atan;
@@ -103,10 +101,10 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 	}
 	LowPass_Filter(observer.Vel_El, vel, observer.lpf_ceof);
 	/* 补偿ladrc相位延时,LADRC等效截止频率为Wo/2pi的两个低通滤波器串联 */
-	angle = fast_atan_2(observer.Vel_El, Wo) * 2.0f;
+	angle = fast_atan2(observer.Vel_El, Wo) * 2.0f;
 	/* 电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
-	observer.angle_out = observer.angle_atan + (angle + 0/*observer.Vel_El * observer.ts*/);
-	angle_clamp(observer.angle_out);
+	observer.angle_out = observer.angle_atan + (angle/* + observer.Vel_El * observer.ts*/);
+	norm_angle_rad(observer.angle_out);
 
 	LowPass_Filter(observer.Vel_El_filter, observer.Vel_El, 0.01f); //需要再加一级低通滤波，给计算Wo和输出使用
 	

Applications/foc/core/smo_observer.c

@@ -85,15 +85,15 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 	smo.est_eBeta_Filted = smo.est_eBeta;
 #endif
 }
-#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
+
 #ifdef USE_ARCTAN
 static void smo_arctan(void) {
 	float ealpha_in = -smo.est_eAlpha_Filted;
 	float ebeta_in  =  smo.est_eBeta_Filted;
 
-	float angle = fast_atan_2(ealpha_in, ebeta_in); //通过反正切获取电角度
+	float angle = fast_atan2(ealpha_in, ebeta_in); //通过反正切获取电角度
 	UTILS_NAN_ZERO(angle);
-	angle_clamp(angle);
+	norm_angle_rad(angle);
 	float prev_angle = smo.est_angle;
 	float comp_angle = 0.0f;
 	if (smo.dir_ccw) {
@@ -113,8 +113,8 @@ static void smo_arctan(void) {
 	}
 	smo.est_rad_pers_filted = smo.est_rad_pers;
 	/*低通滤波有相位滞后，需要补偿，同时电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
-	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc) + smo.est_rad_pers_filted * smo.ts;
-	angle_clamp(smo.est_angle_out);
+	smo.est_angle_out = smo.est_angle + fast_atan2(smo.est_rad_pers_filted, smo.lpf_wc) + smo.est_rad_pers_filted * smo.ts;
+	norm_angle_rad(smo.est_angle_out);
 	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
 	if (smo.est_rpm > CONFIG_HW_MAX_MOTOR_RPM) {
 		smo.est_rpm = CONFIG_HW_MAX_MOTOR_RPM;
@@ -124,7 +124,7 @@ static void smo_arctan(void) {
 }
 #else
 static void smo_pll(void) {
-	float eab_sqr = sqrtf(SQ(smo.est_eAlpha_Filted) + SQ(smo.est_eBeta_Filted) + (1e-10f));
+	float eab_sqr = NORM2_f(smo.est_eAlpha_Filted, smo.est_eBeta_Filted) + (1e-10f));
 	float ealpha_in = -smo.est_eAlpha_Filted/eab_sqr;
 	float ebeta_in  =  smo.est_eBeta_Filted/eab_sqr;
 	float sin, cos;
@@ -140,7 +140,7 @@ static void smo_pll(void) {
 	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
 	
 	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
-	angle_clamp(smo.est_angle);
+	norm_angle_rad(smo.est_angle);
 	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
 	if (smo.est_rpm > CONFIG_HW_MAX_MOTOR_RPM) {
 		smo.est_rpm = CONFIG_HW_MAX_MOTOR_RPM;
@@ -149,7 +149,7 @@ static void smo_pll(void) {
 	}
 	/* 电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
 	smo.est_angle_out = smo.est_angle + smo.est_rad_pers_filted * smo.ts;
-	angle_clamp(smo.est_angle_out);
+	norm_angle_rad(smo.est_angle_out);
 }
 #endif
 

Applications/foc/core/thro_torque.c

@@ -1,253 +0,0 @@
-#include "foc/core/thro_torque.h"
-#include "foc/foc_config.h"
-#include "foc/motor/motor.h"
-#include "foc/motor/motor_param.h"
-#include "foc/core/e_ctrl.h"
-#include "foc/core/PMSM_FOC_Core.h"
-#include "app/nv_storage.h"
-#include "libs/logger.h"
-#include "prot/can_foc_msg.h"
-#include "foc/core/etcs.h"
-#include "foc/motor/throttle.h"
-
-static thro_torque_t _torque;
-
-void thro_torque_reset(void) {
-	_torque.accl = false;
-	_torque.thro_filted = 0.0f;
-	_torque.throttle_opening = _torque.throttle_opening_last = 0.0f;
-	_torque.torque_req = _torque.torque_real = _torque.torque_acc_ = 0.0f;
-	_torque.gear = mc_get_internal_gear();
-}
-
-void thro_torque_init(void) {
-	thro_torque_reset();
-	_torque.spd_filted = 0.0f;
-}
-
-static __INLINE float gear_rpm_torque(u8 torque, s16 max) {
-	return (float)torque/100.0f * max;
-}
-
-float thro_torque_gear_map(s16 rpm, u8 gear) {
-	gear_t *_current_gear = mc_gear_conf_by_gear(gear);
-	if (_current_gear == NULL) {
-		return 0;
-	}
-
-	if (rpm > _current_gear->max_speed) {
-		rpm = _current_gear->max_speed;
-	}
-	if (rpm <= 1000) {
-		return gear_rpm_torque(_current_gear->torque[0], _current_gear->max_torque);
-	}
-
-	for (int i = 1; i < CONFIG_GEAR_SPEED_TRQ_NUM; i++) {
-		if (rpm <= 1000 * (i + 1)) { //线性插值
-			float trq1 = gear_rpm_torque(_current_gear->torque[i-1], _current_gear->max_torque);
-			float min_rpm = (i * 1000);
-			float trq2 = gear_rpm_torque(_current_gear->torque[i], _current_gear->max_torque);
-			float max_rpm = (i + 1) * 1000;
-			return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-		}
-	}
-	return gear_rpm_torque(_current_gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], _current_gear->max_torque);
-}
-
-/* 获取油门开度 */
-float get_throttle_ration(float f_throttle) {
-	if (f_throttle <= throttle_start_vol()) {
-		return 0;
-	}
-	float delta = f_throttle - throttle_start_vol();
-	int ration = (delta * 100.0f) / throttle_vol_range();
-	return ((float)ration)/100.0f;
-}
-
-float thro_ration_to_voltage(float r) {
-	if (r == 0) {
-		return 0;
-	}
-	float vol = throttle_start_vol() + r * throttle_vol_range();
-	return fclamp(vol, 0, throttle_end_vol());
-}
-
-static float _thro_torque_for_accelerate(float ration) {
-	float max_torque = thro_torque_gear_map((s16)_torque.spd_filted, _torque.gear);
-	float thro_torque = max_torque * ration;
-
-	float acc_r = 1.0f;
-	if (_torque.throttle_opening_last < 1.0f) {
-		acc_r = (ration - _torque.throttle_opening_last)/ (1.0f - _torque.throttle_opening_last);
-	}
-	acc_r = fclamp(acc_r, 0, 1.0f);
-	float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);
-	if (acc_torque < 0) {
-		acc_torque = 0;
-	}
-	/*
-	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
-	*/
-	float torque_acc_ = thro_torque - acc_torque;
-	float step = 0.0f;
-	if (torque_acc_ > 0) {
-		float acc_t = mc_gear_conf()->accl_time;
-		step = torque_acc_ / (acc_t + 0.00001f);
-	}else {
-		torque_acc_ = 0;
-	}
-	step_towards(&_torque.torque_acc_, torque_acc_, step);
-	return (acc_torque + _torque.torque_acc_);
-}
-
-
-static float thro_torque_for_accelerate(void) {
-	return _thro_torque_for_accelerate(_torque.throttle_opening);
-}
-
-static float thro_torque_for_decelerate(void) {
-	if (_torque.throttle_opening_last == 0.0f) {
-		return 0;
-	}
-	float dec_r = _torque.throttle_opening / _torque.throttle_opening_last;
-	dec_r = fclamp(dec_r, 0.0f, 1.0f);
-	return dec_r * _torque.torque_real;
-}
-
-static void thro_torque_request(void) {
-#ifdef CONFIG_CRUISE_ENABLE_ACCL
-	if (mc_is_cruise_enabled() && mc_throttle_released()) {
-		return;
-	}
-#endif
-	if (mc_throttle_released() && eCtrl_enable_eBrake(true)) {
-		return;
-	}
-	if (!mc_throttle_released()) {
-		float curr_rpm = PMSM_FOC_GetSpeed();
-		if (_torque.accl) { //加速需求
-			float ref_torque = thro_torque_for_accelerate();
-			float hold_torque = PMSM_FOC_Get()->out.f_autohold_trq;
-			if (hold_torque < 0) { //下坡驻车,最小给0扭矩
-				hold_torque = 0;
-			}
-			ref_torque	= MAX(hold_torque, ref_torque);
-			if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
-				if (_torque.torque_req < hold_torque) {
-					_torque.torque_req = hold_torque;
-					eCtrl_reset_Torque(hold_torque);
-				}
-			}else {
-				PMSM_FOC_Get()->out.f_autohold_trq = 0;
-			}
-			/* 处理加速ramp时间的变化，需要缓慢变小，变大可以立即处理，
-			*  加速时间缓慢变小可以防止突然大扭矩加速
-			*/
-			u16 now_ramp_time = eCtrl_get_torque_acc_time();
-			u16 next_ramp_time = mc_gear_conf()->accl_time;
-			if (curr_rpm < CONFIG_ZERO_SPEED_RAMP_RMP) {
-				next_ramp_time = mc_gear_conf()->zero_accl;
-			}
-			if (next_ramp_time == 0) {
-				next_ramp_time = 100;
-			}
-			if (now_ramp_time != next_ramp_time) {
-				if (next_ramp_time > now_ramp_time) {
-					eCtrl_set_accl_time(next_ramp_time);
-				}else {
-					float f_now = (float)now_ramp_time;
-					float f_next = (float)next_ramp_time;
-					step_towards(&f_now, f_next, 0.5f);
-					if (f_now <= 10) {
-						f_now = 10;
-					}
-					eCtrl_set_accl_time((u16)f_now);
-				}
-			}
-			_torque.torque_req = ref_torque;
-		}else {
-			float ref_torque = thro_torque_for_decelerate();
-			/* autohold 启动的情况下，转把在0位置附近小幅抖动 */
-			if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
-				float hold_torque = PMSM_FOC_Get()->out.f_autohold_trq;
-				ref_torque = MAX(hold_torque, ref_torque);
-			}
-			_torque.torque_req = ref_torque;
-		}
-		etcs_set_torque(_torque.torque_req);
-	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
-		_torque.torque_req = 0.0f;
-		etcs_set_torque(_torque.torque_req);
-	}
-}
-
-#define THRO_RPM_LP_CEOF 0.01f
-void thro_torque_process(u8 run_mode, float f_throttle) {
-
-	float thro_r = get_throttle_ration(f_throttle);
-	_torque.gear = mc_get_internal_gear();
-	LowPass_Filter(_torque.spd_filted, PMSM_FOC_GetSpeed(), THRO_RPM_LP_CEOF);
-
-	if (thro_r > _torque.throttle_opening) {
-		if (!_torque.accl) {
-			_torque.throttle_opening_last = _torque.throttle_opening;
-			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
-			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
-				_torque.torque_real = 0;
-			}
-			_torque.torque_acc_ = 0;
-		}
-		_torque.accl = true;
-	}else if (thro_r < _torque.throttle_opening) {
-		if (_torque.accl) {
-			_torque.throttle_opening_last = _torque.throttle_opening;
-			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
-			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
-				_torque.torque_real = 0;
-			}
-		}
-		_torque.accl = false;
-	}
-
-	_torque.throttle_opening = thro_r;
-	if (run_mode == CTRL_MODE_TRQ) {
-		thro_torque_request();
-	}else if (run_mode == CTRL_MODE_SPD) {
-		if (!mc_is_cruise_enabled()) {
-			float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.throttle_opening;
-			PMSM_FOC_Set_TgtSpeed(speed_Ref);
-		}
-	}else if (run_mode == CTRL_MODE_EBRAKE) {
-		float vel = PMSM_FOC_GetSpeed();
-		float ebrk_trq = motor_get_ebreak_toruqe(vel);
-
-		if (ebrk_trq >= -PMSM_FOC_GetEbrkTorque()/2){
-			eCtrl_Set_eBrk_RampTime(1);
-		}
-		if (ebrk_trq != 0) {
-			eCtrl_set_TgtTorque(ebrk_trq);
-		}
-
-		if (eCtrl_get_FinalTorque() < 0.0001f && vel < CONFIG_MIN_RPM_EXIT_EBRAKE) {
-			eCtrl_enable_eBrake(false);
-			thro_torque_reset();
-		}
-		if (!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f))) {
-			eCtrl_enable_eBrake(false);
-			thro_torque_reset();
-		}
-	}
-}
-
-/* 定速巡航需要判断是否需要加速 */
-float get_user_request_torque(void) {
-	if (_torque.accl) {
-		return thro_torque_for_accelerate();
-	}
-	return thro_torque_for_decelerate();
-}
-
-void thro_torque_log(void) {
-	sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
-	sys_debug("ration %f - %f - %f - %d\n", _torque.throttle_opening, _torque.throttle_opening_last, thro_torque_for_accelerate(), _torque.gear);
-}

Applications/foc/core/thro_torque_unused.h

@@ -1,25 +0,0 @@
-#ifndef _THRO_TORQUE_H__
-#define _THRO_TORQUE_H__
-#include "os/os_types.h"
-#include "foc/core/PMSM_FOC_Core.h"
-typedef struct {
-	bool  accl;
-	float torque_req;
-	float torque_real;
-	float torque_acc_;
-	float thro_filted;
-	float spd_filted;
-	float throttle_opening;
-	float throttle_opening_last;
-	u8    gear;
-}thro_torque_t;
-
-void thro_torque_reset(void);
-void thro_torque_init(void);
-float thro_ration_to_voltage(float r);
-void thro_torque_process(u8 run_mode, float f_throttle);
-float get_user_request_torque(void);
-float thro_torque_gear_map(s16 rpm, u8 gear);
-float get_throttle_ration(float f_thro);
-#endif /* _THRO_TORQUE_H__ */
-

Applications/foc/core/torque_unused.c

@@ -1,175 +0,0 @@
-#include "foc/core/torque.h"
-#include "foc/foc_config.h"
-#include "foc/motor/motor.h"
-#include "foc/motor/motor_param.h"
-#include "foc/core/e_ctrl.h"
-#include "foc/core/PMSM_FOC_Core.h"
-#include "app/nv_storage.h"
-#include "libs/logger.h"
-#include "prot/can_foc_msg.h"
-
-static motor_map_t gear_torques[5][5] = {
-	[0] = {{500,  100}, {1200, 100}, {1700, 80}, {2200, 75}, {2800, 50},},
-	[1] = {{800,  130}, {1600, 120}, {2400, 110}, {3200, 80}, {4300, 60},},
-	[2] = {{1200, 150}, {2200, 140}, {3200, 120}, {4200, 80}, {5300, 70},},
-	[3] = {{3000, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
-	[4] = {{4500, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
-};
-
-/*
-通过查表获取对应扭矩和速度时的Id和IQ的分配
-*/
-
-static torque_manager_t torque_ctrl;
-void torque_init(void) {
-	trq2dq_lookup_init();
-	torque_reset();
-}
-
-void torque_reset(void) {
-	memset(&torque_ctrl, 0, sizeof(torque_ctrl));
-}
-
-float torque_max_from_gear_rpm(void) {
-	u8 gear = mc_get_internal_gear();
-	if (gear > 4) {
-		gear = 0;
-	}
-	motor_map_t *map = &gear_torques[gear][0];
-	s16 rpm = (s16)torque_ctrl.spd_filted;
-	if (rpm <= map[0].rpm) {
-		return (float)map[0].torque;
-	}
-	int map_size = 5;
-	for (int i = 1; i < map_size; i++) {
-		if (rpm <= map[i].rpm) { //线性插值
-			float trq1 = map[i-1].torque;
-			float min_rpm = map[i-1].rpm;
-			float trq2 = map[i].torque;
-			float max_rpm = map[i].rpm;
-			return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-		}
-	}
-	return (float)map[map_size-1].torque;	
-}
-
-
-/* 获取油门开度 */
-static float throttle_ration(float f_throttle) {
-	if (f_throttle <= nv_get_foc_params()->n_startThroVol) {
-		return 0;
-	}
-	float delta = f_throttle - (nv_get_foc_params()->n_startThroVol);
-
-	int ration = (delta * 100.0f) / (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);
-	
-	return ((float)ration)/100.0f;
-}
-
-float throttle_to_speed(float f_throttle) {
-	return (PMSM_FOC_GetSpeedLimit() * throttle_ration(f_throttle));
-}
-
-float throttle_to_torque(float f_throttle) {
-	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
-}
-
-#define FUNC_SEG1_X_END 0.5F
-#define FUNC_SEG1_Y_END 0.25F
-#define FUNC_SEG1_K    (FUNC_SEG1_Y_END/FUNC_SEG1_X_END)
-
-#define FUNC_SEG2_X_OFF FUNC_SEG1_X_END
-#define FUNC_SEG2_Y_OFF FUNC_SEG1_Y_END
-#define FUNC_SEG2_K ((1.0F-FUNC_SEG1_Y_END)/(1.0F - FUNC_SEG1_X_END))
-
-static float __INLINE unline_func(float x) {
-	if (x <= FUNC_SEG1_X_END) {
-		return x * FUNC_SEG1_K;
-	}
-
-	return (x - FUNC_SEG1_X_END) * FUNC_SEG2_K + FUNC_SEG2_Y_OFF;
-} 
-
-#define REAL_DQ_CEOF 0.9f
-#define FINAL_DQ_CEFO 1.1F
-
-static float get_throttle_torque(float trq_ration) {
-	float curr_rpm = PMSM_FOC_GetSpeed();
-	if (curr_rpm == 0) {
-		torque_ctrl.spd_filted = 0;
-	}else {
-		LowPass_Filter(torque_ctrl.spd_filted, curr_rpm, 0.01f);
-	}
-	float torque_map = torque_max_from_gear_rpm();// (float)motor_max_torque_for_rpm((s16)torque_ctrl.spd_filted);
-	float torque_lim = PMSM_FOC_GetTorqueLimit();
-	float max_torque = min(torque_map, torque_lim);
-
-	return max_torque * unline_func(trq_ration);
-}
-
-float get_thro_request_torque(void) {
-	return get_throttle_torque(torque_ctrl.throttle_opening);
-}
-
-void request_torque(float throttle_opening) {
-	float curr_rpm = PMSM_FOC_GetSpeed();
-	float ref_torque = get_throttle_torque(throttle_opening);
-#ifdef CONFIG_CRUISE_ENABLE_ACCL
-	if (mc_is_cruise_enabled() && mc_throttle_released()) {
-		return;
-	}
-#endif
-	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
-		return;
-	}
-	
-	if (!mc_throttle_released()) {
-		if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
-			torque_ctrl.torque_req = eCtrl_get_FinalTorque() * FINAL_DQ_CEFO;
-			ref_torque  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_torque);
-		}
-		if (ref_torque > torque_ctrl.torque_req) { //加扭矩step给定
-			step_towards(&torque_ctrl.torque_req, ref_torque, 1.0f);
-		}else { //减扭矩，直接给定
-			torque_ctrl.torque_req = ref_torque;
-		}
-		PMSM_FOC_Set_Torque(torque_ctrl.torque_req);
-	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
-		float real_trq = PMSM_FOC_Get_Real_dqVector() * REAL_DQ_CEOF;
-		float ref_now = min(real_trq, eCtrl_get_RefTorque());
-		eCtrl_reset_Torque(ref_now);
-		PMSM_FOC_Set_Torque(0);
-	}
-}
-
-void request_speed(float throttle_opening) {
-	if (!mc_is_cruise_enabled()) {
-		float speed_Ref = PMSM_FOC_GetSpeedLimit() * throttle_opening;
-		PMSM_FOC_Set_TgtSpeed(speed_Ref);
-	}
-}
-
-#define THRO_REF_LP_CEOF 0.2f
-void throttle_process(u8 run_mode, float f_throttle) {
-	if (mc_throttle_released()){
-		torque_ctrl.throttle_value = 0;
-		torque_ctrl.torque_req = 0;
-	}else {
-		LowPass_Filter(torque_ctrl.throttle_value, f_throttle, THRO_REF_LP_CEOF);
-	}
-	torque_ctrl.throttle_opening = throttle_ration(f_throttle);
-	if (run_mode == CTRL_MODE_TRQ) {
-		request_torque(torque_ctrl.throttle_opening);
-	}else if (run_mode == CTRL_MODE_SPD) {
-		request_speed(torque_ctrl.throttle_opening);
-	}else if (run_mode == CTRL_MODE_EBRAKE) {
-		eCtrl_reset_Torque(0);
-		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
-			eCtrl_enable_eBrake(false);	
-		}
-		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
-			eCtrl_enable_eBrake(false);
-		}
-	}
-}
-

Applications/foc/core/torque_unused.h

@@ -1,22 +0,0 @@
-#ifndef _TORQUE_LUT_H__
-#define _TORQUE_LUT_H__
-#include "os/os_types.h"
-#include "foc/core/PMSM_FOC_Core.h"
-typedef struct {
-	bool  accl;
-	float torque_req;
-	float throttle_value;
-	float throttle_opening; //油门开度百分比
-	float spd_filted;
-	u32   count;
-}torque_manager_t;
-void torque_init(void);
-void torque_reset(void);
-void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out);
-void throttle_process(u8 run_mode, float f_throttle);
-void request_torque(float throttle_opening);
-void trq2dq_lookup_init(void);
-float get_thro_request_torque(void);
-
-#endif /*_TORQUE_LUT_H__ */
-

Applications/foc/core/trq2dq_table.c

@@ -1,104 +0,0 @@
-#include "os/os_types.h"
-#include "foc/core/PMSM_FOC_Core.h"
-#include "foc/motor/motor_param.h"
-#include "libs/logger.h"
-
-#define _DEBUG(fmt, args...) sys_debug(fmt, ##args)
-
-extern torque_map_t table_map[14][10];
-//x -> rpm
-//z -> torque
-static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float *q) {
-	float frac_z1 = 0; //对应x1索引的t_maps
-	float frac_z2 = 0; //对应x2索引的t_maps
-
-	_DEBUG("low --> %d %d\n", map[1]->torque, map[0]->torque);
-	if ((map[1]->torque != map[0]->torque)) {
-		frac_z1 = (float)(z - map[0]->torque)/(map[1]->torque - map[0]->torque);
-	}
-	_DEBUG("high --> %d %d\n", map[3]->torque, map[2]->torque);
-	if ((map[3]->torque != map[2]->torque)) {
-		frac_z2 = (float)(z - map[2]->torque)/(map[3]->torque - map[2]->torque);
-	}
-	
-	_DEBUG("%f -- %f -- %f\n", frac_x, frac_z1, frac_z2);
-
-	float c1 = (1.0f - frac_z1) * map[0]->d + frac_z1 * map[1]->d; //第一行插值
-	float c2 = (1.0f - frac_z2) * map[2]->d + frac_z2 * map[3]->d; //第二行插值
-	*d = c1 * (1.0f - frac_x) + c2 * frac_x;                //两行插值
-	
-	c1 = (1.0f - frac_z1) * map[0]->q + frac_z1 * map[1]->q;
-	c2 = (1.0f - frac_z2) * map[2]->q + frac_z2 * map[3]->q;
-	*q = c1 * (1.0f - frac_x) + c2 * frac_x;
-}
-
-static void get_torque_range(s16 z, int index, int max_index, int *left, int *right) {
-	int low_left = max_index - 1, low_right = max_index - 1;
-	if (z < table_map[index][0].torque) {
-		low_right = low_left = 0;
-		_DEBUG("---%d, %d--%d\n", z, table_map[index][0].torque, table_map[0][0].torque);
-	}else if (z > table_map[index][max_index - 1].torque) {
-		low_right = low_left = max_index - 1;
-	}else {
-		for (int i = 0; i < max_index; i++) {
-			//_DEBUG("index %d, trq %d\n", i, table_map[index][i].torque);
-			if (z >= table_map[index][i].torque) {
-				low_left = i;
-				low_right = i + 1;
-				if (i == max_index - 1) {
-					low_right = low_left;
-					break;
-				}
-			}
-		}
-	}
-	*left = low_left;
-	*right = low_right;
-}
-
-
-void trq2dq_lookup_init(void) {
-	//if (table_map == NULL) {
-		//table_map = get_torque2dq_maps();
-	//}
-}
-
-void trq2dq_lookup(int rpm, s16 torque, DQ_t *dq_out) {
-	//if (table_map == NULL) {
-	//	step_towards(&dq_out->d, 0, 10.0f);
-	//	dq_out->q = torque;
-	//	return;
-	//}
-	if (torque < 0.0f) {
-		dq_out->d = 0.0f;
-		dq_out->q = torque;
-		return;
-	}
-
-	int low = 0, high = 0;
-	s16 x1 = 0, x2 = 0;
-	rpm = motor_map_rpm_idx(rpm, &low, &high, &x1, &x2);
-
-	_DEBUG("speed %d-%d, %d-%d\n", low, high, x1, x2);
-	int max_trq_idx = motor_map_torque_max_count();
-	int low_left = max_trq_idx - 1, low_right = max_trq_idx - 1;
-	get_torque_range(torque, low, max_trq_idx, &low_left, &low_right);
-	_DEBUG("low speed torque %d-%d\n", low_left, low_right);
-	
-	int high_left = max_trq_idx - 1, high_right = max_trq_idx - 1;
-	get_torque_range(torque, high, max_trq_idx, &high_left, &high_right);
-	_DEBUG("high speed torque %d-%d\n", high_left, high_right);
-	torque_map_t *maps[4];
-	maps[0] = &table_map[low][low_left];
-	maps[1] = &table_map[low][low_right];
-	maps[2] = &table_map[high][high_left];
-	maps[3] = &table_map[high][high_right];
-	float frac_x = 0, d = 0, q = 0;
-	if (x1 != x2) {
-		frac_x = (float)(rpm - x1)/(x2 - x1);
-	}
-	intp_line2(frac_x, torque, maps, &d, &q);
-	//step_towards(&dq_out->d, d, 10.0f);
-	dq_out->d = d;
-	dq_out->q = q;
-}

Applications/foc/foc_config.h

@@ -4,7 +4,7 @@
 #include "bsp/bsp.h"
 
 #define CONFIG_ACC_MIN_VOL 30
-#define CONFIG_SVM_MODULATION       1.0f//(1.0F/SQRT3_BY_2)
+#define CONFIG_SVM_MODULATION       1.0f
 #define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
 #define CONFIG_CRUISE_EXIT_RPM    700    /* 自动推出定速巡航的最小速度*/
 #define CONFIG_MIN_RPM_FOR_EBRAKE 800 //进入电流回收的最小转速
@@ -61,7 +61,12 @@
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
 
-#define CONFIG_ENABLE_IAB_REC 0   // for phase current debug
+#define CONFIG_CONTRL_FW_ENABLE 1
+#define CONFIG_CONTRL_FW_START_DUTY 0.9F
+
+#define CONFIG_ENABLE_IAB_REC 1   // for phase current/voltage debug
+
+#define CONFIG_MOT_IND_USE_PHASE_SAMPLE 1 //电机参数离线识别使用采集的相电压
 
 #ifdef CONFIG_SPEED_LADRC
 	#define CONFIG_LADRC_Wo  200.0F

Applications/foc/limit.c

@@ -6,246 +6,235 @@
 #include "foc/mc_error.h"
 #include "libs/logger.h"
 
-static limter_t motor_temp_lim[3];
-static limter_t mos_temp_lim[3];
-static limter_t vol_under_lim[1];
+static temp_lim_t motor_tl;
+static temp_lim_t mosfet_t;
+static vol_limt_t vbus;
 static bool _inited = false;
 static bool _can_recovery = true;
-static s16 mot_temp, mos_temp;
 
 static void limiter_init(void) {
-	
-	for (int i = 0; i < CONFIG_TEMP_PROT_NUM; i++) {
-		motor_temp_lim[i].enter_pointer = mc_conf()->p_mot[i].enter_pointer;
-		motor_temp_lim[i].exit_pointer = mc_conf()->p_mot[i].exit_pointer;
-		motor_temp_lim[i].limit_value = mc_conf()->p_mot[i].limit_value;
-		sys_debug("%d-%d-%d\n", motor_temp_lim[i].enter_pointer, motor_temp_lim[i].exit_pointer, motor_temp_lim[i].limit_value);
-		mos_temp_lim[i].enter_pointer = mc_conf()->p_mos[i].enter_pointer;
-		mos_temp_lim[i].exit_pointer = mc_conf()->p_mos[i].exit_pointer;
-		mos_temp_lim[i].limit_value = mc_conf()->p_mos[i].limit_value;
-		sys_debug("%d-%d-%d\n", mos_temp_lim[i].enter_pointer, mos_temp_lim[i].exit_pointer, mos_temp_lim[i].limit_value);
-	}
-	vol_under_lim[0].enter_pointer = mc_conf()->p_vol.enter_pointer;
-	vol_under_lim[0].exit_pointer = mc_conf()->p_vol.exit_pointer;
-	vol_under_lim[0].limit_value = mc_conf()->p_vol.limit_value;
-	//sys_debug("%d-%d-%d\n", vol_under_lim[0].enter_pointer, vol_under_lim[0].exit_pointer, vol_under_lim[0].limit_value);
-	mot_temp = sample_motor_temp();
-	mos_temp = sample_mos_temp();
+	if (_inited) {
+		return;
+	}
+	motor_tl.high = mc_conf()->p_mot[0].enter_pointer;
+	motor_tl.high_lim = mc_conf()->p_mot[0].limit_value;
+	motor_tl.mid = mc_conf()->p_mot[1].enter_pointer;
+	motor_tl.mid_lim = mc_conf()->p_mot[1].limit_value;
+	motor_tl.start = mc_conf()->p_mot[2].enter_pointer;
+	motor_tl.start_lim = mc_conf()->p_mot[2].limit_value;
+	motor_tl.curr_lim = 1.0f;
+	motor_tl.temp = sample_motor_temp();
+	sys_debug("mot: %d-%d, %d-%d, %d-%d\n", motor_tl.high, motor_tl.high_lim, motor_tl.mid, motor_tl.mid_lim, motor_tl.start, motor_tl.start_lim);
+	mosfet_t.high = mc_conf()->p_mos[0].enter_pointer;
+	mosfet_t.high_lim = mc_conf()->p_mos[0].limit_value;
+	mosfet_t.mid = mc_conf()->p_mos[1].enter_pointer;
+	mosfet_t.mid_lim = mc_conf()->p_mos[1].limit_value;
+	mosfet_t.start = mc_conf()->p_mos[2].enter_pointer;
+	mosfet_t.start_lim = mc_conf()->p_mos[2].limit_value;
+	mosfet_t.curr_lim = 1.0f;
+	mosfet_t.temp = sample_mos_temp();
+	sys_debug("mos: %d-%d, %d-%d, %d-%d\n", mosfet_t.high, mosfet_t.high_lim, mosfet_t.mid, mosfet_t.mid_lim, mosfet_t.start, mosfet_t.start_lim);
+	vbus.crit_low = mc_conf()->c.min_dc_vol;
+	vbus.lower = mc_conf()->p_vol.enter_pointer;
+	vbus.lower_lim = mc_conf()->p_vol.limit_value;
+	sys_debug("vbus: %d, %d-%d\n", vbus.crit_low, vbus.lower, vbus.lower_lim);
+	_inited = true;
 }
 
-static u16 _temp_limiter(s16 temp, limter_t *lim) {
-	if (!lim->is_limit) {
-		if (temp < lim->enter_pointer) {
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
-		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 500){
-			lim->is_limit = true;
-			lim->ticks = 0;
-			return lim->limit_value;
-		}
-		return HW_LIMIT_NONE;
-	}else {
-		if (temp >= lim->exit_pointer) {
-			lim->ticks = 0;
-			return lim->limit_value;
+static bool temp_limit_check(temp_lim_t *limit, s16 temp) {
+	if (!limit->is_limit) {
+		if (temp < limit->start) {
+			limit->lim_ticks = 0;
+		}else {
+			if (++limit->lim_ticks >= 1) {
+				limit->is_limit = true;
+				limit->lim_ticks = 0;
+			}
 		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 500) {
-			lim->is_limit = false;
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
+	}
+	if (limit->is_limit) {
+		if (temp < limit->start) {
+			if (++limit->lim_ticks >= 1) {
+				limit->is_limit = false;
+				limit->lim_ticks = 0;
+			}
+		}else {
+			limit->lim_ticks = 0;
 		}
-		return lim->limit_value;
 	}
+
+	return limit->is_limit;
 }
 
-static u16 _vol_limiter(s16 vol, limter_t *lim) {
-	if (!lim->is_limit) {
-		if (vol > lim->enter_pointer) {
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
-		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 5){
-			lim->is_limit = true;
-			lim->ticks = 0;
-			return lim->limit_value;
+#define TEMP_SENSOR_ERR_CNT 20
+
+float get_temp_limit_value(temp_lim_t *limit, s16 temp, s16 err_temp) {
+	if ((temp == err_temp) || ABS(temp - limit->temp) >= 20) {
+		limit->temp = temp;
+		if (limit->temp_sensor_err < TEMP_SENSOR_ERR_CNT) {
+			limit->temp_sensor_err++;
+			return limit->curr_lim;
+		}else {
+			return min(limit->curr_lim, 0.5f); //温度传感器异常，返回上次的限流
 		}
-		return HW_LIMIT_NONE;
 	}else {
-		if (vol <= lim->exit_pointer) {
-			lim->ticks = 0;
-			return lim->limit_value;
-		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 100) {
-			lim->is_limit = false;
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
+		limit->temp_sensor_err = 0;
+	}
+	limit->temp = temp;
+	if (!temp_limit_check(limit, temp)) {
+		limit->curr_lim = 1.0f;
+	}else {
+		if (temp < limit->start) {
+			return limit->curr_lim; //keep prev limit value
+		}else if (temp > limit->high) {
+			limit->curr_lim = 0;
+		}else {
+			if (temp >= limit->start && temp <= limit->mid) {
+				limit->curr_lim = f_map(temp, limit->start, limit->mid, limit->start_lim, limit->mid_lim)/100.0f;
+			}else {
+				limit->curr_lim = f_map(temp, limit->mid, limit->high, limit->mid_lim, limit->high_lim)/100.0f;
+			}
 		}
-		return lim->limit_value;
 	}
+
+	return limit->curr_lim;
 }
 
-static u16 _motor_limit(void) {
-	static int temp_sensor_err = 0;
-	static u16 lim_value  = HW_LIMIT_NONE;
+/* this maybe limit power or torque, based on the current power */
+float motor_temp_high_limit(void) {
+	limiter_init();
 	s16 temp = get_motor_temp_raw();
-	if ((temp == 300) || ABS(temp - mot_temp) >= 20) {
-		if (temp_sensor_err < 20) {
-			temp_sensor_err++;
-		}else {
-			if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
-				mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, mot_temp);
-			}
+	float value = get_temp_limit_value(&motor_tl, temp, 300);
+	if (motor_tl.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, motor_tl.temp);
 		}
-		return lim_value; //温度传感器异常，返回上次的限流
-	}else {
-		mot_temp = temp;
-		temp_sensor_err = 0;
-	}
-	for(int i = 0; i < ARRAY_SIZE(motor_temp_lim); i++) {
-		limter_t *lim = motor_temp_lim + i;
-		lim_value = _temp_limiter(temp, lim);
-		if (lim_value != HW_LIMIT_NONE) {
-			if (lim_value == 0) {
-				if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
-					mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(&motor.controller));
-				}
-			}else if (_can_recovery){
-				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
-			}
-			gear_t *gear = mc_gear_conf();
-
-			float prv_lim_value;
-			float next_lim_tmp;
-			if (i < (ARRAY_SIZE(motor_temp_lim)-1)) {
-				prv_lim_value = (float)motor_temp_lim[i + 1].limit_value;
-			}else {
-				prv_lim_value = 100.0f; //最低一级限流
-			}
-			if (i != 0) {
-				next_lim_tmp = (float)motor_temp_lim[i - 1].enter_pointer;
-			}else {
-				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
-			}
-			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
-			float delta_value = (prv_lim_value - (float)lim->limit_value);
-			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
-			curr_value = fclamp(curr_value, 0, prv_lim_value);
-			lim_value = (u16)(((float)gear->max_torque * curr_value) / 100.0f);
-			mc_set_motor_lim_level(i + 1);
-			return lim_value;
-		}else {
-			mc_set_motor_lim_level(0);
+	}
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
 		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
 	}
-	
-	return HW_LIMIT_NONE;
+
+	return value;
 }
 
-static u16 _mos_limit(void) {
-	static int temp_sensor_err = 0;
-	static u16 lim_value  = HW_LIMIT_NONE;
-	s16 temp = get_mos_temp_raw();
-	if ((temp == -40) || ABS(temp - mos_temp) >= 20) {
-		if (temp_sensor_err < 20) {
-			temp_sensor_err++;
-		}else {
-			if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
-				mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mos_temp);
-			}
+float motor_temp_high_limit_test(s16 temp) {
+	limiter_init();
+	float value = get_temp_limit_value(&motor_tl, temp, 300);
+	if (motor_tl.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, motor_tl.temp);
 		}
-		return lim_value; //温度传感器异常，返回上次的限流
-	}else {
-		mos_temp = temp;
-		temp_sensor_err = 0;
-	}
-	for(int i = 0; i < ARRAY_SIZE(mos_temp_lim); i++) {
-		limter_t *lim = mos_temp_lim + i;
-		lim_value = _temp_limiter(temp, lim);
-		if (lim_value != HW_LIMIT_NONE) {
-			if (lim_value == 0) {
-				if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
-					mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(&motor.controller));
-				}
-			}else if (_can_recovery){
-				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
-			}
-			gear_t *gear = mc_gear_conf();
-
-			float prv_lim_value;
-			float next_lim_tmp;
-			if (i < (ARRAY_SIZE(mos_temp_lim)-1)) {
-				prv_lim_value = (float)mos_temp_lim[i + 1].limit_value;
-			}else {
-				prv_lim_value = 100.0f; //最低一级限流
-			}
-			if (i != 0) {
-				next_lim_tmp = (float)mos_temp_lim[i - 1].enter_pointer;
-			}else {
-				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
-			}
-			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
-			float delta_value = (prv_lim_value - (float)lim->limit_value);
-			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
-			curr_value = fclamp(curr_value, 0, prv_lim_value);
-			lim_value = (u16)(((float)gear->max_torque * curr_value) / 100.0f);
-			mc_set_mos_lim_level(i + 1);
-			return lim_value;
-		}else {
-			mc_set_mos_lim_level(0);
+	}
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
 		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
 	}
-	
-	return HW_LIMIT_NONE;
+
+	return value;
 }
 
-/* this maybe limit power or torque, based on the current power */
-u16 motor_temp_high_limit(void) {
-	if (!_inited) {
-		_inited = true;
-		limiter_init();
+
+/* limit the max torque(max phase current) */
+float mos_temp_high_limit(void) {
+	limiter_init();
+	s16 temp = get_mos_temp_raw();
+	float value = get_temp_limit_value(&mosfet_t, temp, -40);
+	if (mosfet_t.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mosfet_t.temp);
+		}
+	}
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
+		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
 	}
-	return _motor_limit();
+
+	return value;
 }
 
-/* limit the max torque(max phase current) */
-u16 mos_temp_high_limit(void) {
-	if (!_inited) {
-		_inited = true;
-		limiter_init();
+float mos_temp_high_limit_test(s16 temp) {
+	limiter_init();
+	float value = get_temp_limit_value(&mosfet_t, temp, -40);
+	if (mosfet_t.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mosfet_t.temp);
+		}
 	}
-	return _mos_limit();
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
+		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
+	}
+
+	return value;
 }
 
+
 /* limit the DC bus current */
-u16 vbus_under_vol_limit(void) {
-	if (!_inited) {
-		_inited = true;
-		limiter_init();
-	}
+u16 vbus_voltage_low_limit(void) {
+	limiter_init();
 	s16 vol = (s16)sample_vbus_raw();
-	for(int i = 0; i < ARRAY_SIZE(vol_under_lim); i++) {
-		limter_t *lim = vol_under_lim + i;
-		u16 lim_value = _vol_limiter(vol, lim);
-		if (lim_value != HW_LIMIT_NONE) {
-			if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
-				if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
-					mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(&motor.controller));
-				}
+	if (!vbus.is_limit && (vol <= vbus.lower)) {
+		vbus.is_limit = true;
+		if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
+			if (mot_contrl_get_speed(mot_contrl()) > CONFIG_ZERO_SPEED_RPM) {
+				mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(mot_contrl()));
 			}
-			return lim_value;
+		}
+	}else if (vbus.is_limit && (vol > vbus.lower)) {
+		vbus.is_limit = false;
+		if (_can_recovery) {
+			mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
 		}
 	}
-	if (_can_recovery) {
-		mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
+	if (vol > vbus.lower) {
+		return HW_LIMIT_NONE;
+	}else if (vol < vbus.crit_low) {
+		return 0.0f;
 	}
-	return HW_LIMIT_NONE;
+	return (u16)f_map(vol, vbus.crit_low, vbus.lower, 0.0f, vbus.lower_lim);
+}
+
+u16 vbus_voltage_low_limit_test(s16 vol) {
+	limiter_init();
+	if (!vbus.is_limit && (vol <= vbus.lower)) {
+		vbus.is_limit = true;
+		if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
+			if (mot_contrl_get_speed(mot_contrl()) > CONFIG_ZERO_SPEED_RPM) {
+				mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(mot_contrl()));
+			}
+		}
+	}else if (vbus.is_limit && (vol > vbus.lower)) {
+		vbus.is_limit = false;
+		if (_can_recovery) {
+			mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
+		}
+	}
+	if (vol > vbus.lower) {
+		return HW_LIMIT_NONE;
+	}else if (vol < vbus.crit_low) {
+		return 0.0f;
+	}
+	return (u16)f_map(vol, vbus.crit_low, vbus.lower, 0.0f, vbus.lower_lim);
+}
+
+
+bool motor_temp_limited(void) {
+	return motor_tl.is_limit;
+}
+
+bool mos_temp_limited(void) {
+	return mosfet_t.is_limit;
 }

Applications/foc/limit.h

@@ -3,17 +3,37 @@
 #include "os/os_types.h"
 
 #define HW_LIMIT_NONE ((u16)0x7FFF)
+
+typedef struct {
+	bool is_limit;
+	u32  lim_ticks;
+	s16  high; //exsit_low -> lower -> higher
+	u16  high_lim;
+	s16  mid;
+	u16  mid_lim;
+	s16  start;
+	u16  start_lim;
+	float curr_lim;
+	u16   temp_sensor_err;
+	s16   temp;
+}temp_lim_t;
+
 typedef struct {
-	u16 limit_value;
-	s16 enter_pointer;
-	s16 exit_pointer;
-	u32 ticks;
 	bool is_limit;
-}limter_t;
+	u32  ticks;
+	s16  crit_low;
+	s16  lower;
+	u16  lower_lim;
+}vol_limt_t;
 
-u16 motor_temp_high_limit(void);
-u16 mos_temp_high_limit(void);
-u16 vbus_under_vol_limit(void);
+float motor_temp_high_limit(void);
+float mos_temp_high_limit(void);
+u16   vbus_voltage_low_limit(void);
+bool motor_temp_limited(void);
+bool mos_temp_limited(void);
+float motor_temp_high_limit_test(s16 temp);
+float mos_temp_high_limit_test(s16 temp);
+u16 vbus_voltage_low_limit_test(s16 vol);
 
 #endif /* _Limit_H__ */
 

Applications/foc/mc_config.c

@@ -1,8 +1,10 @@
 #include "foc/mc_config.h"
+#include "foc/core/controller.h"
 #include "libs/utils.h"
 #include "libs/crc16.h"
 #include "app/nv_storage.h"
 #include "libs/logger.h"
+#include "foc/mc_error.h"
 
 mc_config conf;
 
@@ -119,15 +121,18 @@ void mc_conf_load(void) {
 		sys_debug("conf block 1 OK!\n");
 		memcpy(&conf, &temp, sizeof(temp));
 		nv_write_config_block(0, (u8 *)&temp, sizeof(temp));
+		mc_crit_err_add(FOC_NV_Err, 0, 0);
 	}else if (crc16 != temp.crc) {
 		if (idx0_success) {
 			sys_debug("conf block1 Bad!\n");
 			nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
+			mc_crit_err_add(FOC_NV_Err, 1, 1);
 		}else {
 			sys_debug("conf block0&1 Bad!\n");
 			mc_conf_default();
 			nv_write_config_block(0, (u8 *)&conf, sizeof(conf));
 			nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
+			mc_crit_err_add(FOC_NV_Err, 0, 1);
 		}
 	}else {
 		sys_debug("conf block 1 OK!\n");
@@ -283,6 +288,7 @@ void mc_conf_init(void) {
 		mc_conf_default();
 		mc_conf_save();
 	}
+	sys_debug("mc size %d - %d\n", sizeof(conf), sizeof(conf.c));
 	sys_debug("mc conf band %d, pid: %f, %f, %f, %f, %d\n", conf.c.dq_loop_bandwith, conf.c.pid[PID_ID_ID].kp, conf.c.pid[PID_ID_ID].ki, conf.c.pid[PID_IQ_ID].kp, conf.c.pid[PID_IQ_ID].ki, mc_conf()->m.encoder_offset);
 }
 
@@ -362,10 +368,13 @@ int mc_conf_decode_controler(u8 *buff) {
 	conf.c.thro_max_vol = decode_float(buff);buff += 4;
 	conf.c.thro_dec_time = decode_u16(buff);buff += 2;
 	conf.c.max_torque = min(conf.c.max_torque, conf.m.max_torque);
+	conf.c.max_dc_vol = min(conf.c.max_dc_vol, CONFIG_BOARD_MAX_VOLTAGE);
+	conf.c.min_dc_vol = MAX(conf.c.min_dc_vol, CONFIG_BOARD_MIN_VOLTAGE);
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_VelLim_ID], buff);
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_Vel_ID], buff);
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_AutoHold_ID], buff);
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_IDCLim_ID], buff);
+	buff += mc_conf_decode_pid(&conf.c.epm_pid, buff);
 
 	conf.c.pid[PID_ID_ID].kp = (float)conf.c.dq_loop_bandwith * 2.0f * 3.14f * conf.m.ld;
 	conf.c.pid[PID_ID_ID].ki = conf.m.rs / conf.m.ld;
@@ -402,6 +411,7 @@ int mc_conf_encode_controler(u8 *buff) {
 	buff += mc_conf_encode_pid(&conf.c.pid[PID_Vel_ID], buff);
 	buff += mc_conf_encode_pid(&conf.c.pid[PID_AutoHold_ID], buff);
 	buff += mc_conf_encode_pid(&conf.c.pid[PID_IDCLim_ID], buff);
+	buff += mc_conf_encode_pid(&conf.c.epm_pid, buff);
 	return buff - ori;
 }
 
@@ -552,11 +562,19 @@ int mc_conf_encode_configs(u8 *buff) {
 }
 
 void mc_conf_set_pid(u8 id, pid_t *pid) {
-	conf.c.pid[id] = *pid;
+	if (id < PID_Max_ID) {
+		conf.c.pid[id] = *pid;
+	}else if (id == PID_EPM_ExtID){
+		conf.c.epm_pid = *pid;
+	}
 }
 
 void mc_conf_get_pid(u8 id, pid_t *pid) {
-	*pid = conf.c.pid[id];
+	if (id < PID_Max_ID) {
+		*pid = conf.c.pid[id];
+	}else if (id == PID_EPM_ExtID){
+		*pid = conf.c.epm_pid;
+	}
 }
 
 bool mc_conf_set_gear(u8 mode, u8 *data, int len) {

Applications/foc/mc_config.h

@@ -16,7 +16,8 @@ typedef enum {
 	PID_VelLim_ID,
 	PID_IDCLim_ID,
 	PID_AutoHold_ID,
-	PID_Max_ID
+	PID_Max_ID,
+	PID_EPM_ExtID,
 }PID_id_t;
 
 typedef struct
@@ -35,7 +36,7 @@ typedef struct
 	u16	max_fw_id;
 	u16 max_torque;
 	s16 encoder_offset;
-}motor_t;
+}mot_params_t;
 
 typedef struct {
 	float kp;
@@ -63,6 +64,8 @@ typedef struct {
 	float thro_max_vol;
 	u16   thro_dec_time;
 	pid_t pid[PID_Max_ID];
+	pid_t epm_pid;
+	u8    _pad[64];
 }controller_t;
 
 typedef struct {
@@ -100,7 +103,7 @@ typedef struct {
 
 typedef struct {
 	u8 version;
-	motor_t m;
+	mot_params_t m;
 	controller_t c;
 	settings_t s;
 	gear_t g_n[CONFIG_MAX_GEARS];

Applications/foc/motor/encoder.c

@@ -72,6 +72,7 @@ void encoder_init_clear(void) {
 	g_encoder.start_dianostic_cnt = 0;
 	g_encoder.align_cnt = 0;
 	g_encoder.align_step = 0;
+	g_encoder.align_cnt_final = 0;
 	g_encoder.pwm_time_ms = get_tick_ms();
 	_init_pll();
 }
@@ -137,6 +138,12 @@ static __INLINE float _eccentricity_compensation(int cnt) {
 #endif
 }
 
+#define MAX_CPR_CNT_PER_CTL 40
+static __INLINE bool encoder_ab_is_jitter(s16 cnt) {
+	return ((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
+					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2)));
+}
+
 #define CONFIG_ENC_DETECT_ERR
 #define CONFIG_ENC_ERR_TEST
 #define CONFIG_ENC_DIANOSTIC_MIN_CNT (10.0F * ENC_MAX_RES * FOC_CTRL_US) //600rpm, 每隔控制周期 0.2232 机械角度
@@ -150,7 +157,7 @@ static s16 enc_test1 = 0;
 static s16 enc_test2 = 0;
 static s16 enc_test3 = 0;
 static s16 enc_test4 = 0;
-#define MAX_CPR_CNT_PER_CTL 40
+
 /* 9000RPM = 150 RPS = 150 * ENC_MAX_RES * FOC_CTRL_US = 39, 每个控制周期 39个count */
 static void encoder_detect_error(s16 cnt) {
 #ifdef CONFIG_ENC_DETECT_ERR
@@ -159,8 +166,7 @@ static void encoder_detect_error(s16 cnt) {
 		s16 delta_cnt = cnt - g_encoder.last_cnt;
 		bool skip = false;
 		if (g_encoder.b_timer_ov) {
-			bool is_jitter = ((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
-					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2))); //需要处理低速在overflow附近震荡
+			bool is_jitter = encoder_ab_is_jitter(cnt); //需要处理低速在overflow附近震荡
 			if (!is_jitter) {
 				s16 com = _pll_over_comp();
 				if (com > 0) {
@@ -256,8 +262,7 @@ float encoder_get_theta(bool detect_err) {
 	__NOP();__NOP();__NOP();__NOP();
 	if (ENC_OverFlow()) {
 		cnt = _abi_count();
-		if((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
-				(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2))) { //需要处理低速在overflow附近震荡
+		if(encoder_ab_is_jitter(cnt)) { //需要处理低速在overflow附近震荡
 			g_encoder.b_timer_ov = false;
 		}else {
 			g_encoder.b_timer_ov = true;
@@ -277,19 +282,16 @@ float encoder_get_theta(bool detect_err) {
 	}else {
 		if (cnt == g_encoder.last_cnt) {
 			g_encoder.interpolation += g_encoder.est_vel_cnt_filter * FOC_CTRL_US;
-			if (g_encoder.interpolation > ENC_MAX_interpolation) {
-				g_encoder.interpolation = ENC_MAX_interpolation;
-			}else if (g_encoder.interpolation < -ENC_MAX_interpolation) {
-				g_encoder.interpolation = -ENC_MAX_interpolation;
-			}
+			g_encoder.interpolation = fclamp(g_encoder.interpolation, -ENC_MAX_interpolation, ENC_MAX_interpolation);
 		}else {
 			g_encoder.interpolation = 0.0f;
 		}
 	}
-	step_towards_s16(&g_encoder.align_step, g_encoder.align_cnt, 2);
-	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)(cnt/* + g_encoder.align_step*/) + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
+	step_towards(&g_encoder.align_step, (float)g_encoder.align_cnt_final, 0.01f);
+	float angle_count = cnt + g_encoder.align_step + g_encoder.interpolation;
+	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle(angle_count) * g_encoder.motor_poles + g_encoder.enc_offset;
 	g_encoder.abi_angle += _eccentricity_compensation(cnt);
-	rand_angle(g_encoder.abi_angle);
+	norm_angle_deg(g_encoder.abi_angle);
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = task_get_usecond();
 
@@ -325,7 +327,7 @@ float encoder_get_position(void) {
 float encoder_zero_phase_detect(float *enc_off) {	
 	delay_ms(5);	
 	float total_enc_off = g_encoder.pwm_count;
-	float prev_offset = g_encoder.enc_offset;
+	s16 prev_offset = g_encoder.enc_offset;
 	float phase = encoder_get_pwm_angle();
 	float total_ph = phase;
 	int count = 0;
@@ -346,7 +348,7 @@ float encoder_zero_phase_detect(float *enc_off) {
 	}
 	sys_debug("count = %d, %f, %d\n", count, total_enc_off, g_encoder.pwm_count);
 	float offset_now = total_ph/(float)(count + 1);
-	g_encoder.enc_offset = offset_now;
+	g_encoder.enc_offset = (s16)offset_now;
 	g_encoder.enc_count_off = (u32)(total_enc_off/(float)(count + 1));
 	if (enc_off) {
 		*enc_off = (float)g_encoder.enc_count_off;
@@ -372,44 +374,64 @@ bool encoder_get_cali_error(void) {
 static void encoder_sync_pwm_abs(void) {
 	u32 mask = cpu_enter_critical();
 	ENC_COUNT = g_encoder.pwm_count;
+	g_encoder.pwm_start_cnt = g_encoder.pwm_count;
 	g_encoder.last_cnt = g_encoder.pwm_count;
 	g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
 	g_encoder.abi_angle = g_encoder.pwm_angle;
 	g_encoder.b_index_found = true;
 	g_encoder.last_delta_cnt = MAX_S16;
 	g_encoder.align_cnt = g_encoder.align_step = 0;
+	g_encoder.align_cnt_final = 0;
+	g_encoder.z_index_counter = 0;
 	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
 	cpu_exit_critical(mask);
 }
 
 /*I 信号的中断处理，一圈一个中断*/
-static int abi_I_delta = 0xFFFFFFF;
-static int abi_z_count = 0;
-void ENC_ABI_IRQHandler(void) {
-	g_encoder.z_index_cnt = _abi_count();
-	abi_z_count++;
-#if 0
-	if (!g_encoder.b_index_found){
-		encoder_sync_pwm_abs();
-	}
-	if (g_encoder.z_index_cnt > 10 && g_encoder.z_index_cnt < (g_encoder.cpr - 10)) {
-		if (abi_I_delta == 0xFFFFFFF) {
-			abi_I_delta = g_encoder.z_index_cnt;
-		}
+#define CONFIG_Z_IDX_MARGIN_B 4
+#define CONFIG_Z_IDX_MARGIN_F 1
+#define CONFIG_Z_IDX_FIXED_COUNT 30  /* Z信号最大允许的补偿机角度为 (30-5)/4096*360=2.2度*/
+#define CONFIG_Z_IDX_MAX_CNT_PER_IRQ 4 /* 每次z信号中断，最大补偿机械角度(4)/4096*360=0.34，类似做低通滤波 */
+#define CONFIG_Z_IDX_IGNORE_MAX_CNT 10
+void ENC_ABI_IRQHandler(u32 count) {
+	s16 z_margin;
+#ifdef ENCODER_CC_INVERT
+	g_encoder.z_index_cnt = (g_encoder.cpr -1) - count;
+	if (g_encoder.est_vel_cnt_filter > 0){
+		z_margin = CONFIG_Z_IDX_MARGIN_B;
+	}else {
+		z_margin = CONFIG_Z_IDX_MARGIN_F;
 	}
 #else
-	if (g_encoder.z_index_cnt > 10 && g_encoder.z_index_cnt < 50) {
-		g_encoder.align_cnt = -(g_encoder.z_index_cnt - 5);
-	}else if (g_encoder.z_index_cnt > (g_encoder.cpr - 50) && g_encoder.z_index_cnt < (g_encoder.cpr - 10)) {
-		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.z_index_cnt - 5;
+	g_encoder.z_index_cnt = count;
+	if (g_encoder.est_vel_cnt_filter > 0){
+		z_margin = CONFIG_Z_IDX_MARGIN_F;
 	}else {
-		if (g_encoder.z_index_cnt <=10 || g_encoder.z_index_cnt >= (g_encoder.cpr - 10)) {
-			g_encoder.align_cnt = 0;
-		}else if (g_encoder.enc_maybe_err == ENCODER_NO_ERR){
-			abi_I_delta = g_encoder.z_index_cnt;
-		}
+		z_margin = CONFIG_Z_IDX_MARGIN_B;
 	}
 #endif
+  	g_encoder.z_index_counter++;
+	s16 pre_cnt = g_encoder.align_cnt;
+	if (g_encoder.z_index_cnt <= z_margin) {
+		g_encoder.align_cnt = 0;
+	}else if (g_encoder.z_index_cnt >= (g_encoder.cpr - z_margin)) {
+		g_encoder.align_cnt = 0;
+	}else if (g_encoder.z_index_cnt > z_margin && g_encoder.z_index_cnt < CONFIG_Z_IDX_FIXED_COUNT) {
+		g_encoder.align_cnt = -(g_encoder.z_index_cnt - z_margin);
+	}else if (g_encoder.z_index_cnt > (g_encoder.cpr - CONFIG_Z_IDX_FIXED_COUNT) && g_encoder.z_index_cnt < (g_encoder.cpr - z_margin)) {
+		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.z_index_cnt - z_margin;
+	}else {
+		//maybe error?
+		g_encoder.z_index_err_counter++;
+	}
+	s16 delta = pre_cnt - g_encoder.align_cnt;
+	if (ABS(delta) >= CONFIG_Z_IDX_IGNORE_MAX_CNT) {
+		g_encoder.align_cnt = pre_cnt;
+	}
+	/* 编码器没有做零位置校准不能补偿 */
+	if (g_encoder.enc_offset == 0) {
+		step_towards_s16(&g_encoder.align_cnt_final, g_encoder.align_cnt, CONFIG_Z_IDX_MAX_CNT_PER_IRQ);
+	}
 }
 
 /* 编码器AB信号读书溢出处理 */
@@ -449,7 +471,7 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 		g_encoder.pwm_count = n_nr;
 	}
 	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * g_encoder.motor_poles + g_encoder.enc_offset;	
-	rand_angle(g_encoder.pwm_angle);
+	norm_angle_deg(g_encoder.pwm_angle);
 	if (!g_encoder.b_index_found && pwm_count++ >= 10) {
 		encoder_sync_pwm_abs();
 	}
@@ -478,7 +500,7 @@ bool ENC_Check_error(void) {
 float encoder_get_pwm_angle(void) {
 #ifdef ENCODER_CC_INVERT
 	g_encoder.pwm_angle = 360.0f - (g_encoder.pwm_angle - g_encoder.enc_offset) + g_encoder.enc_offset;
-	rand_angle(g_encoder.pwm_angle);
+	norm_angle_deg(g_encoder.pwm_angle);
 #endif
 	return g_encoder.pwm_angle;
 }
@@ -486,13 +508,14 @@ float encoder_get_pwm_angle(void) {
 float encoder_get_abi_angle(void) {
 	s16 cnt = _abi_count();
 	float angle = ENC_Pluse_Nr_2_angle((float)(cnt + g_encoder.align_step)) * g_encoder.motor_poles + g_encoder.enc_offset;
-	rand_angle(angle);
+	norm_angle_deg(angle);
 	return angle;
 }
 
 void encoder_log(void) {
 	sys_debug("pwm %f, abi %f\n", encoder_get_pwm_angle(), encoder_get_abi_angle());
-	sys_debug("pwm count %d, I count %d,%d,%d\n", g_encoder.pwm_count, abi_I_delta, g_encoder.z_index_cnt, abi_z_count);
+	sys_debug("Z idx %d,%d,%d,%d\n", g_encoder.z_index_err_counter, g_encoder.pwm_start_cnt, g_encoder.z_index_cnt, g_encoder.z_index_counter);
+	sys_debug("Z err: %f, %d,%d\n", g_encoder.align_step, g_encoder.align_cnt, g_encoder.align_cnt_final);
 	sys_debug("pwm freq %f, err %d, %f, %f\n", enc_get_pwm_freq(), pwm_duty_err, pwm_err_min, pwm_err_max);
 	if (g_encoder.enc_maybe_err) {
 		sys_debug("E:%d,%d,%d,%d,%d,%d\n", enc_test1, enc_test2, enc_test3, enc_cnt, enc_last_cnt, enc_test4);

Applications/foc/motor/encoder.h

@@ -3,7 +3,7 @@
 #include "foc/core/PI_Controller.h"
 typedef struct {
 	bool  b_index_found; //I 对齐
-	float enc_offset;
+	s16   enc_offset;
 	u32   enc_count_off;
 	u8    motor_poles;
 	float pwm_angle;
@@ -21,8 +21,12 @@ typedef struct {
 	s16   cpr;
 	s16   last_cnt;
 	s16   align_cnt; //z 中断的时候的cnt数，需要计算角度的是时候处理这个align
-	s16   align_step;
+	s16   align_cnt_final; //最终需要补偿的计数，类似对align_cnt 做低通滤波，防止Z信号的干扰导致错误补偿
+	float align_step;
 	s16   z_index_cnt;
+	u32   z_index_counter;
+	u32   z_index_err_counter;
+	s16   pwm_start_cnt;
 	u32   last_us;
 	s8    direction; //motor running dir, 逆时针 正，顺时针负
 	float est_vel_counts; //每秒多少个计数

Applications/foc/motor/hall.c

@@ -1,488 +1,363 @@
-#include <string.h>
-#include "bsp/bsp_driver.h"
-#include "os/os_task.h"
-#include "libs/utils.h"
-#include "libs/logger.h"
-#include "math/fast_math.h"
-#include "foc/motor/hall.h"
-#include "app/nv_storage.h"
-#include "libs/time_measure.h"
-#include "app/nv_storage.h"
-#include "libs/logger.h"
-
-//#define USE_DETECTED_ANGLE 1
-
-#define HALL_READ_TIMES 9
-
-static u32 _hall_detect_task(void *args);
-static void _hall_init_el_angle(void);
-
-
-//#define HALL_PLACE_OFFSET (360-25)//(230) //(345) //315
-#define HALL_PLACE_OFFSET (230)
-
-/* 
-4,5,1,3,2,6,4
-*/
-
-hall_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) //s32q14
-
-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 u32 __inline__ _hall_get_angle_ticks(void) {
-	hall_sample_t *s = &_sensor_hander.samples;
-	if (!s->full) {
-		return s->ticks[s->index-1];
-	}else {
-		return s->ticks_sum/SAMPLE_MAX_COUNT;
-	}
-}
-
-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) {
-	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(s8 direction) {
-	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
-	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
-	for (int i = 0; i < 8; i++) {
-		_sensor_hander.angle_table[i] = (nv_get_motor_params()->hall_table[i]);
-	}
-	_sensor_hander.manual_angle = 0x3FF;
-	_sensor_hander.direction = direction;
-	_sensor_hander.running_dir = _sensor_hander.direction;
-	_hall_init_el_angle();
-}
-
-void hall_sensor_init(void) {
-	hall_sensor_default(POSITIVE);
-	mc_hall_init();
-	shark_task_create(_hall_detect_task, NULL);
-}
-
-void hall_set_direction(s8 direction) {
-	hall_sensor_default(direction);
-}
-
-void hall_sensor_clear(void) {
-	hall_set_direction(POSITIVE);
-}
-
-static u32 _hall_detect_task(void *args) {
-	if (_sensor_hander.el_speed != 0) {
-		u32 ticks_now = task_ticks_abs();
-		if (ticks_now > _sensor_hander.hall_ticks) {
-			if (task_delta_ticks(ticks_now) > 2000*1000) {
-				hall_sensor_clear();
-			}
-		}
-	}
-	return 0;
-}
-
-#if 1
-int hall_e_count = 0;
-float hall_sensor_get_theta(bool detect_err){
-	hall_e_count++;
-	float angle_add = _sensor_hander.delta_angle_ts;
-	if (_sensor_hander.comp_count > 0) {
-		_sensor_hander.comp_count--;
-		angle_add += _sensor_hander.angle_comp_ts;
-	}
-	_sensor_hander.estimate_delta_angle += angle_add;
-
-	float el_angle = _sensor_hander.estimate_delta_angle;
-	if (el_angle > _sensor_hander.next_delta_angle) {
-		el_angle = _sensor_hander.next_delta_angle;
-	}
-	if (_sensor_hander.direction == POSITIVE) {
-		el_angle = _sensor_hander.estimate_el_angle + el_angle;
-	}else {
-		el_angle = _sensor_hander.estimate_el_angle - el_angle;
-	}
-	rand_angle(el_angle);
-	if (hall_e_count%5 == 0) {
-		plot_2data16((s16)el_angle, _sensor_hander.hall_stat*10);
-	}
-	return (el_angle);
-}
-#else
-static int test_loop = 0;
-float hall_sensor_get_theta(void){
-	float angle_add = _sensor_hander.delta_angle_ts;
-	float comp_angle = 0;
-	if (_sensor_hander.comp_count > 0) {
-		_sensor_hander.comp_count--;
-		comp_angle = _sensor_hander.angle_comp_ts;
-	}
-
-	if (_sensor_hander.direction == POSITIVE) {
-		_sensor_hander.estimate_delta_angle += angle_add;
-		_sensor_hander.estimate_el_angle += (angle_add + comp_angle);
-	}else {
-		_sensor_hander.estimate_delta_angle -= angle_add;
-		_sensor_hander.estimate_el_angle -= (angle_add + comp_angle);
-	}
-	rand_angle(_sensor_hander.estimate_el_angle);
-	test_loop ++;
-	if (test_loop % 20 == 0) {
-		//plot_3data16(_sensor_hander.estimate_el_angle, _sensor_hander.angle_comp_ts * 1000, _sensor_hander.comp_count);
-	}
-	return ( _sensor_hander.estimate_el_angle);
-}
-
-#endif
-
-float hall_sensor_get_speed(void) {
-	return (_sensor_hander.rpm) * _sensor_hander.running_dir;
-}
-
-int hall_offset_increase(int inc) {
-	inc = inc << 19;
-	if (_sensor_hander.phase_offset + inc >= PHASE_360_DEGREE) {
-		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - PHASE_360_DEGREE;
-	}else {
-		_sensor_hander.phase_offset += inc;
-	}
-	return _sensor_hander.phase_offset;
-}
-
-s32 *hall_get_table(void) {
-	return _sensor_hander.angle_table;
-}
-
-static float sin_hall[8];
-static float cos_hall[8];
-static int hall_iterations[8];
-void hall_detect_angle(s16 angle) {
-	_sensor_hander.manual_angle = (angle);
-	int hall = get_hall_stat(HALL_READ_TIMES);
-	float s, c;
-	normal_sincosf(degree_2_pi(angle), &s, &c);
-	sin_hall[hall] += s;
-	cos_hall[hall] += c;
-	hall_iterations[hall]++;
-}
-
-bool hall_detect_angle_finish(void) {
-	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] = (ang);
-		} else {
-			_sensor_hander.angle_table[i] = (0x3FF);
-			fails++;
-		}
-	}
-	if (fails == 2) {
-		nv_save_hall_table(_sensor_hander.angle_table);
-	}
-	memset(sin_hall, 0, sizeof(sin_hall));
-	memset(cos_hall, 0, sizeof(cos_hall));
-	memset(hall_iterations, 0, sizeof(hall_iterations));	
-	return fails == 2;
-}
-
-void hall_detect_offset(s16 angle) {
-	_sensor_hander.manual_angle = (angle);
-}
-
-bool hall_detect_offset_finish(void) {
-	int fails = 0;
-	for(int i = 0;i < 8;i++) {
-		if (hall_iterations[i] > 20) {
-			_sensor_hander.angle_table[i] = _sensor_hander.angle_table[i] / hall_iterations[i];
-		}
-	}
-	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 */
-	_sensor_hander.measured_el_angle += PHASE_60_DEGREE;
-	rand_angle(_sensor_hander.measured_el_angle);
-  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
-	_sensor_hander.hall_ticks = task_ticks_abs();
-}
-
-static __inline__ float _get_angle(u8 state, float 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 float _hall_position(u8 state_now, u8 state_prev) {
-	float theta_now =  0xFFFFFFFF;
-	switch (state_now) {
-		case STATE_1:
-			if (state_prev == STATE_5) {
-				_sensor_hander.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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.running_dir = 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;
-	}
-	if (theta_now != 0xFFFFFFFF) {
-		rand_angle(theta_now);
-	}
-	return theta_now;
-}
-
-#ifdef USE_DETECTED_ANGLE
-static __inline u8 _next_hall(u8 hall_now) {
-	switch (hall_now) {
-		case STATE_1:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_3;
-			}else {
-				return STATE_5;
-			}
-		case STATE_2:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_6;
-			}else {
-				return STATE_3;
-			}
-		case STATE_3:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_2;
-			}else {
-				return STATE_1;
-			}
-		case STATE_4:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_5;
-			}else {
-				return STATE_6;
-			}
-		case STATE_5:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_1;
-			}else {
-				return STATE_4;
-			}
-		case STATE_6:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_4;
-			}else {
-				return STATE_2;
-			}
-		default: //not reached here
-			return STATE_1;
-	}
-} 
-
-
-static __inline__ s32 _get_delta_angle(u8 now, u8 next) {
-	s32 delta_angle = _sensor_hander.angle_table[next] - _sensor_hander.angle_table[now];
-	if (_sensor_hander.direction == POSITIVE) {
-		if (delta_angle < 0) { //process cross 360 degree
-			delta_angle += PHASE_360_DEGREE;
-		}
-	}else if (_sensor_hander.direction == NEGATIVE) {
-		if (delta_angle > 0) { //process cross 360 degree
-			delta_angle -= PHASE_360_DEGREE;
-		}
-		delta_angle = -delta_angle;
-	}
-	return delta_angle;
-}
-#endif
-
-void HALL_IRQHandler(void) {
-	time_measure_start(&g_meas_hall);
-	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
-	u8 hall_stat_prev = _sensor_hander.hall_stat;
-	u32 hall_ticks_now = task_ticks_abs();	
-
-	/*获取当前转子角度*/
-	float theta_now = _hall_position(hall_stat_now, hall_stat_prev);
-	if (theta_now == 0xFFFFFFFF) {
-		return;
-	}
-	//plot_2data16(hall_stat_now*60, _sensor_hander.manual_angle);
-	//获取两次中断的时间间隔，估计速度
-	u32 delta_us = task_delta_ticks(hall_ticks_now);
-	if (delta_us == 0) {
-		return;
-	}
-	_sensor_hander.hall_ticks = hall_ticks_now;
-	//获取两次中断之间转子转过的角度,获取预期的下次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 = 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 + 1;
-	_sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
-	float delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
-	if (delta_el_speed*100/prev_imme_el_speed >= 40) { //即时速度增加40%,认为不稳定，需要使用即时速度估计转子位置
-		_sensor_hander.trns_detect = true;
-	}else {
-		_sensor_hander.trns_detect = false;
-	}
-	_hall_put_sample(delta_us, delta_angle);
-	u32 mask = cpu_enter_critical();
-	if (_sensor_hander.samples.full) {
-		//float estimate_delta_angle =  _sensor_hander.next_delta_angle - _sensor_hander.estimate_delta_angle;
-		//plot_2data16(estimate_delta_angle>>19, (estimate_delta_angle/((s32)(delta_us/FOC_CTRL_US)))>>10);//, _sensor_hander.estimate_delta_angle>>19);
-		/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
-		delta_us = _hall_get_angle_ticks();
-		//_sensor_hander.comp_count = 50;
-		//_sensor_hander.angle_comp_ts = estimate_delta_angle/(float)_sensor_hander.comp_count;
-		_sensor_hander.estimate_el_angle = theta_now;
-	}else {
-		_sensor_hander.comp_count = 0;
-		_sensor_hander.angle_comp_ts = 0;
-		_sensor_hander.estimate_el_angle = theta_now;
-	}
-	_sensor_hander.estimate_delta_angle = 0;
-	_sensor_hander.delta_angle_ts = (next_delta_angle/(us_2_s(delta_us)/FOC_CTRL_US));
-	_sensor_hander.next_delta_angle = next_delta_angle;
-	_sensor_hander.measured_el_angle = theta_now;
-
-	cpu_exit_critical(mask);
-	_sensor_hander.hall_stat = hall_stat_now;
-	_sensor_hander.hall_ticks = hall_ticks_now;
-	_sensor_hander.el_speed = _hall_angle_speed();	//s32q5
-	_sensor_hander.rpm = (_sensor_hander.el_speed / 360 * 60) * nv_get_motor_params()->poles; //s32q5
-	//plot_3data16(_sensor_hander.rpm >> 5, _sensor_hander.el_speed>>5, delta_us);
-	time_measure_end(&g_meas_hall);
-
-	//plot_3data16(delta_us/10, hall_stat_prev * 10, _sensor_hander.hall_stat * 10);
-}
-
-
+#include <string.h>
+#include "bsp/bsp_driver.h"
+#include "os/os_task.h"
+#include "libs/utils.h"
+#include "libs/logger.h"
+#include "math/fast_math.h"
+#include "foc/motor/hall.h"
+#include "foc/mc_config.h"
+#include "app/nv_storage.h"
+#include "libs/logger.h"
+
+#define HALL_READ_TIMES 5
+#define SMOOTH_COUNT 0
+#define HALL_DETECT_OFFSET 0
+/* 
+1,5,4,6,2,3
+0,1,2,3,4,5
+//////
+2,6,4,5,1,3
+0,1,2,3,4,5
+////
+2,3,1,5,4,6
+0,1,2,3,4,5
+*/
+static s8 hall_2_pos[] = {7,2,0,1,4,3,5,7};
+static hall_t g_hall;
+
+#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
+#define HALL_MAX_TIME 1000000UL
+
+static u32 g_delta_cnt = 0;
+static u32 g_trns_cnt = 0;
+static u32 g_min_delta;
+
+#define COUNT_2_US(c) (c/(SYSTEM_CLOCK/1000000))
+
+#if HALL_DETECT_OFFSET > 0
+static float hall_off[6];
+static u32	 hall_off_cnt[6];
+
+static void hall_offset_dec_init(void) {
+	for (int i = 0; i < 6; i++) {
+		hall_off[i] = 0;
+		hall_off_cnt[i] = 0;
+	}
+}
+float mc_get_mot_angle(void);
+static __INLINE void hall_offset_update(void) {
+	int pos_int = (int)g_hall.low_res_pos;
+	float hall_pos = g_hall.low_res_pos * PHASE_60_DEGREE;
+	float delta_pos = hall_pos - (mc_get_mot_angle() + 90.0f);
+	norm_angle_deg(delta_pos);
+	hall_off[pos_int] = delta_pos;
+	hall_off_cnt[pos_int] += 1;
+}
+static void hall_offset_log(void) {
+	if (hall_off_cnt[0] < 20) {
+		return;
+	}
+	s16 value[6];
+	u32 mask = cpu_enter_critical();
+	for (int i = 0; i < 6; i++) {
+		value[i] = (s16)(hall_off[i]/(float)hall_off_cnt[i]);
+		hall_off_cnt[i] = 0;
+	}
+	cpu_exit_critical(mask);
+	sys_debug("off:%d,%d,%d,%d,%d,%d\n", value[0], value[1], value[2], value[3], value[4], value[5]);
+}
+
+#else
+static void hall_offset_dec_init(void) {
+}
+static __INLINE void hall_offset_update(void) {
+}
+static void hall_offset_log(void) {
+}
+#endif
+static __INLINE u32 hall_delta_us(u32 count) {
+	u32 now = task_ticks_abs();
+	u32 delta = now - count;
+	if (now < count) { //wrap
+		delta = 0xFFFFFFFF - count + now + 1;
+	}
+	return COUNT_2_US(delta);
+}
+
+
+static u8 __INLINE hall_read_state(void) {
+	u8 hall_a = 0, hall_b = 0, hall_c = 0;
+	for (int i = 0; i < HALL_READ_TIMES; i++) {
+		hall_a += gpio_hall_a_value();
+		hall_b += gpio_hall_b_value();
+		hall_c += gpio_hall_c_value();
+	}
+	u8 state = 0;
+	if (hall_a > (HALL_READ_TIMES/2 + 1)) {
+		state = 1;
+	}
+	if (hall_b > (HALL_READ_TIMES/2 + 1)) {
+		state = state | (1<<1);
+	}
+	if (hall_c > (HALL_READ_TIMES/2 + 1)) {
+		state = state | (1 << 2);
+	}
+	return state;
+}
+
+static void hall_init_low_pos(void) {
+	u8 state = hall_read_state();
+	s16 pos = hall_2_pos[state];
+	if (pos == 7) {
+		g_hall.sig_errors ++;
+		return;
+	}
+	g_hall.state = state;
+	if (g_hall.dir == POSITIVE) {
+		g_hall.low_res_pos = pos + 0.5f;
+	}else {
+		g_hall.low_res_pos = pos - 0.5f;
+	}
+}
+
+
+static void __INLINE hall_put_sample(u32 ticks) {
+	hsample_t *s = &g_hall.samples;
+	g_hall.last_delta_ticks = ticks;
+	s->ticks_sum -= s->ticks[s->index];
+	s->ticks[s->index] = ticks;
+	s->ticks_sum += s->ticks[s->index];
+	s->index += 1;
+	if (s->index >= SAMPLE_MAX_COUNT) {
+		s->index = 0;
+	}
+	if (s->filled < SAMPLE_MAX_COUNT) {
+		s->filled++;
+	}
+}
+
+
+static float __INLINE hall_elec_angle_vel(void){
+	hsample_t *s = &g_hall.samples;
+	if (s->filled < SAMPLE_MAX_COUNT) {
+		return (PHASE_60_DEGREE) / us_2_s(g_hall.last_delta_ticks);
+	}
+	if (s->ticks_sum == 0) {
+		return 0.0f;
+	}
+	return (PHASE_60_DEGREE * SAMPLE_MAX_COUNT) / us_2_s(s->ticks_sum);
+}
+
+void hall_debug_log(void) {
+	sys_debug("angle dir %d, stat %d, lowres %f, err %d,%d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.sig_errors, g_hall.noise_errors);
+	sys_debug("D: %d,%d,%d\n", g_delta_cnt, g_trns_cnt, g_min_delta);
+	sys_debug("RPM: %f\n", hall_get_velocity());
+	hall_offset_log();
+}
+
+void hall_init(void) {
+	g_hall.phase_offset = mc_conf()->m.encoder_offset;
+	g_hall.mot_poles = mc_conf()->m.poles;
+	g_hall.b_trns_det = false;
+	g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
+	g_hall.angle_smooth_step = 0;
+	g_hall.samples.ticks_sum = 0;
+	g_hall.position = 0;
+	g_hall.samples.index = 0;
+	g_hall.samples.filled = 0;
+	g_hall.elec_angle_vel = 0;
+	g_hall.dir_set = g_hall.prev_dir = g_hall.dir = POSITIVE;
+	g_min_delta = 1000000 * 100;
+	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
+		g_hall.samples.ticks[i] = HALL_MAX_TIME;
+	}
+	g_hall.samples.ticks_sum = HALL_MAX_TIME * SAMPLE_MAX_COUNT;
+
+	if (!g_hall.inited) {
+		g_hall.inited = true;
+#ifdef HALL_TIMER
+		hall_tmr_init();
+#else
+		gpio_hall_init();
+#endif
+	}
+	hall_offset_dec_init();
+	hall_init_low_pos();
+}
+
+void  hall_set_direction(s8 dir) {
+	if (dir == g_hall.dir_set) {
+		return;
+	}
+	g_hall.dir_set = g_hall.dir = g_hall.prev_dir = dir;
+	hall_init_low_pos();
+}
+
+#if SMOOTH_COUNT > 0
+static float get_angle_diff(float a1, float a2) {
+	float diff = a1 - a2;
+	float abs_diff = ABS(diff);
+	if (abs_diff >= PHASE_180_DEGREE) {
+		return (PHASE_360_DEGREE - abs_diff);
+	}else {
+		return diff;
+	}
+}
+#endif
+
+static __INLINE bool hall_update_low_pos(void) {
+	u8 state = hall_read_state();
+	s16 pos = hall_2_pos[state];
+	if (pos == 7) {
+		g_hall.sig_errors ++;
+		return false;
+	}
+	s16 pos_prev = hall_2_pos[g_hall.state];
+	s16 delta_pos = pos - pos_prev;
+	if (delta_pos != 0) {
+		s8  prev_dir = g_hall.dir;
+		if (delta_pos == 1 || delta_pos == -5) {
+			g_hall.dir = POSITIVE;
+			g_hall.prev_dir = prev_dir;
+		}else if (delta_pos == -1 || delta_pos == 5){
+			g_hall.dir = NEGATIVE;
+			g_hall.prev_dir = prev_dir;
+		}else {
+			if (g_hall.samples.filled > 0) {
+				g_hall.sig_errors ++;
+				return false;
+			}
+		}
+		g_hall.edge_ticks = task_ticks_abs();
+	}else {
+		g_hall.sig_errors ++;
+		return false;
+	}	
+	g_hall.state = state;
+	
+	g_hall.low_res_pos = pos;
+
+	return true;
+}
+
+hall_t *hall_get(void) {
+	return &g_hall;
+}
+
+float hall_get_elec_angle(void) {
+	float phase_offset = g_hall.phase_offset;
+	if (g_hall.dir == NEGATIVE) {
+		phase_offset = PHASE_60_DEGREE + phase_offset;
+	}
+	float angle = g_hall.elec_angle + phase_offset;
+	norm_angle_deg(angle);
+	return angle;
+}
+
+float hall_update_elec_angle(void) {
+	float delta_ticks = (float)hall_delta_us(g_hall.edge_ticks);//上次hall变换到目前的时间
+	float low_res = g_hall.low_res_pos;
+	float delta_pos = g_hall.elec_angle_vel / PHASE_60_DEGREE * us_2_s(delta_ticks) * g_hall.dir;//上次hall变换到目前走过的角度(对60度的比值，小于1)，通过速度插值
+	if (delta_pos > 1.0f) {
+		delta_pos = 1.0f;
+	}else if (delta_pos < -1.0f) {
+		delta_pos = -1.0f;
+	}
+	float high_res_pos = delta_pos  + low_res;
+	float elec_angle = high_res_pos * PHASE_60_DEGREE;
+	float elec_smooth_angle;
+#if SMOOTH_COUNT>0
+	float delta_angle = delta_pos * PHASE_60_DEGREE;
+	if (g_hall.angle_smooth_cnt < (SMOOTH_COUNT + 1)) {
+		elec_smooth_angle = g_hall.elec_angle_edge + g_hall.angle_smooth_step * g_hall.angle_smooth_cnt + delta_angle;
+		g_hall.angle_smooth_cnt++;
+		if (g_hall.angle_smooth_step >= 0) {
+			elec_smooth_angle = min(elec_smooth_angle, elec_angle);
+		}else {
+			elec_smooth_angle = MAX(elec_smooth_angle, elec_angle);
+		}
+	}else {
+		elec_smooth_angle = elec_angle;
+	}
+#else
+	elec_smooth_angle = elec_angle;
+#endif
+	norm_angle_deg(elec_smooth_angle);
+	g_hall.elec_angle = elec_smooth_angle;
+	g_hall.position += g_hall.elec_angle_vel * FOC_CTRL_US / g_hall.mot_poles;
+	if ((g_hall.samples.filled > 1) && (delta_ticks / g_hall.last_delta_ticks >= 1.4f)) {
+		g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.95f;
+		if (g_hall.elec_angle_vel < 30) {
+			g_hall.elec_angle_vel = 0;
+			g_hall.samples.filled = 0;
+			g_hall.samples.index = 0;
+			g_hall.dir = g_hall.prev_dir = g_hall.dir_set;
+			hall_init_low_pos();
+		}
+		float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
+		g_hall.velocity_filted = velocity_raw;
+	}
+	return hall_get_elec_angle();
+}
+
+float hall_get_vel_counts(void) {
+	return g_hall.elec_angle_vel/PHASE_60_DEGREE * g_hall.dir;
+}
+
+float hall_get_velocity(void) {
+	return g_hall.velocity_filted;
+}
+
+float hall_get_position(void) {
+	return g_hall.position;
+}
+
+static __INLINE void hall_calc_mot_velocity(u32 delta_cnt) {
+	if (g_hall.samples.filled == 0) {
+		hall_put_sample(HALL_MAX_TIME/10);
+		return;
+	}
+	if (delta_cnt < g_min_delta) {
+		g_min_delta = delta_cnt;
+	}
+	hall_put_sample(delta_cnt);
+	g_hall.elec_angle_vel = hall_elec_angle_vel();
+	float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
+	LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.5F);
+}
+
+
+float hall_offset_detect(float *off) {
+	return 0.0f;
+}
+
+void HALL_IRQHandler(void) {
+	g_delta_cnt++;
+	u32 mask = cpu_enter_critical();
+	u32 delta_cnt = hall_delta_us(g_hall.edge_ticks);
+	if (delta_cnt < 200) {
+		g_hall.noise_errors++;
+		goto hall_end;
+	}
+	if (!hall_update_low_pos()) {
+		goto hall_end;
+	}
+	hall_offset_update();
+	g_hall.elec_angle_edge = g_hall.elec_angle;
+#if SMOOTH_COUNT>0
+	g_hall.delta_angle_edge = get_angle_diff(g_hall.low_res_pos * PHASE_60_DEGREE, g_hall.elec_angle_edge);
+	if (ABS(g_hall.delta_angle_edge) >= 2.0f) {
+		g_hall.angle_smooth_step = g_hall.delta_angle_edge/SMOOTH_COUNT;
+		g_hall.angle_smooth_cnt = 1;
+	}else {
+		g_hall.angle_smooth_step = 0;
+		g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
+	}
+#endif
+	hall_calc_mot_velocity(delta_cnt);
+hall_end:
+	cpu_exit_critical(mask);
+	return;
+}
+
+

Applications/foc/motor/hall.h

@@ -3,6 +3,7 @@
 #include "os/os_types.h"
 #include "bsp/bsp.h"
 #include "math/fix_math.h"
+#include "foc/core/PI_Controller.h"
 
 
 #define STATE_0 (uint8_t)0
@@ -15,51 +16,49 @@
 #define STATE_7 (uint8_t)7
 
 #define THETA_NONE        (float)0xFFFF
-#define SAMPLE_MAX_COUNT 16
+#define SAMPLE_MAX_COUNT 6
 
 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;
+	u32			filled;
+}hsample_t;
 
 typedef struct {
-	float 		estimate_el_angle; //60度区间内的估计电角度
-	float 		estimate_delta_angle;		
-	float   		measured_el_angle; //hall测量到的电角度
-	float 		next_delta_angle;
-	float        delta_angle_ts;    //每个控制周期角度的增加量，通过上次的速度计算得到
-	float        angle_comp_ts;     //每个控制周期角度增加的补偿量，主要用在加减速，hall角度和60度有偏差的情况
-	int             comp_count;
-	float 		immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
-	float 		el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
-	float  		rpm;        		//当前的电速度, 单位：RPM
-	bool  			trns_detect;     //速度变化超过阈值
-	u8    			hall_stat;
-	u32   			hall_ticks;	
-	s8    			direction;
-	s8              running_dir;
+	bool			inited;
 	float   		phase_offset;
-	hall_sample_t 	samples;
-	u32  			sensor_error;
-	float         manual_angle;
-	s32  		angle_table[8];
+	float			mot_poles;
+	u8				state; // state = A <<2+ B <<1 + C
+	float			low_res_pos; //每个霍尔中断更新一次，60度
+	s8				dir;
+	s8				prev_dir;
+	s8				dir_set;
+	u32				edge_ticks;
+	u32				last_delta_ticks;
+	float			elec_angle; //经过插值的高分辨率角度
+	float			elec_angle_vel;
+	float			elec_angle_edge; //霍尔中断的时候，上面插值高分辨率的角度所存
+	float			delta_angle_edge;
+	float			angle_smooth_step;
+	float			angle_smooth_cnt;
+	float			velocity_filted;
+	float			position;
+	hsample_t		samples;
+	bool			b_trns_det; //速度突变
+	u32				sig_errors;
+	u32				noise_errors;
 }hall_t;
 
-void hall_sensor_init(void);
-void hall_sensor_clear(void);
-float hall_sensor_get_theta(bool detect_err); //return degree
-float hall_sensor_get_speed(void); //return rpm;
-int hall_offset_increase(int inc);
-s32 *hall_get_table(void);
-bool hall_detect_angle_finish(void);
-void hall_detect_angle(s16 angle);
-bool hall_detect_offset_finish(void);
-void hall_detect_offset(s16 angle);
-void hall_set_direction(s8 direction);
+void hall_init(void);
+float hall_update_elec_angle(void);
+float hall_get_elec_angle(void);
+float hall_get_velocity(void);
+float hall_get_vel_counts(void);
+float hall_get_position(void);
+float hall_offset_detect(float *off);
+void  hall_set_direction(s8 dir);
+hall_t *hall_get(void);
 
 #endif /* _HALL_SENSOR_H__ */
 

Applications/foc/motor/mot_params_ind.c

@@ -3,6 +3,7 @@
 #include "math/fast_math.h"
 #include "foc/motor/mot_params_ind.h"
 #include "libs/logger.h"
+#include "foc/samples.h"
 #include "prot/can_foc_msg.h"
 
 /*
@@ -17,10 +18,11 @@ static shark_timer_t _ldq_ind_timer = TIMER_INIT(_ldq_ind_timer, _ldq_ind_timer_
 static void _flux_ind_timer_handler(shark_timer_t *);
 static shark_timer_t _flux_ind_timer = TIMER_INIT(_flux_ind_timer, _flux_ind_timer_handler);
 
-static float rs_id_max, rs_vd_max, rs_vd_now, rs_est_value;
+static float rs_id_max, rs_vd_max, rs_vd_now, rs_est_value = 0.011f;
 static s32 rs_meas_time;
 static bool b_rs_ind = false, b_rs_ested = false, b_ldq_ind = false, b_ld_ested = false, b_lq_ested = false, b_flux_ind = false, b_flux_ested = false;
 static u8   rs_ind_step = 0;
+static u16  old_max_fw_id = 0xFFFF;
 void mot_params_ind_rs(float vd_max, float id_max, s32 time) {
 	if (b_rs_ind || b_ldq_ind || b_flux_ind) {
 		return;
@@ -43,9 +45,14 @@ void mot_params_ind_stop(void) {
 	shark_timer_cancel(&_ldq_ind_timer);
 	shark_timer_cancel(&_flux_ind_timer);
 	u32 mask = cpu_enter_critical();
+	if (old_max_fw_id != 0xFFFF) {
+		mc_conf()->m.max_fw_id = old_max_fw_id;
+		old_max_fw_id = 0xFFFF;
+	}
 	b_rs_ind = false;
 	b_ldq_ind = false;
 	b_flux_ind = false;
+	mot_ctrl_set_ind_freq(mot_contrl(), 0);
 	cpu_exit_critical(mask);
 	mot_contrl_set_vdq(&motor.controller, 0, 0);
 	mot_contrl_set_current(&motor.controller, 0);
@@ -87,17 +94,16 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 			}
 		}else {
 			wait_iq_0_cnt = 0;
+			delay_ms(100);
 			rs_ind_step = 3;
 			sys_debug("start rs calc, %d\n", rs_meas_time);
 		}
 		break;
 	case 3: {
-		float *iabc = motor.controller.foc.in.curr_abc;
-		float d, q;
-		foc_abc_2_dq(SIGN(iabc[0]), SIGN(iabc[1]), SIGN(iabc[2]), &d, &q);
-		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * motor.controller.foc.in.dc_vol * 1.5f;
-		float vd = (rs_vd_now - dtc) * TWO_BY_THREE;
-		float rs = vd / (motor.controller.foc.out.curr_dq.d + 0.0001f);
+		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_period) * motor.controller.foc.in.dc_vol * TWO_BY_THREE;
+		float vd = rs_vd_now * TWO_BY_THREE - dtc;
+		float id = motor.controller.foc.out.curr_dq.d;
+		float rs = vd / (id + 0.0001f);
 		rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
 		if (rs_meas_time-- <= 0) {
 			mot_params_ind_stop();
@@ -105,8 +111,8 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 			mc_ind_motor_start(false);
 			finish = true;
 			b_rs_ested = true;
-			sys_debug("est rs = %f\n", rs_est_value);
-			sys_debug("vd is %f, wait %d\n", rs_vd_now, wait_iq_0_cnt);
+			sys_debug("est rs = %f-%f\n", rs_est_value, rs);
+			sys_debug("vd-id is %f-%f-%f-%f, wait %d\n", rs_vd_now, id, dtc, vd, wait_iq_0_cnt);
 		}
 	}
 	default:
@@ -144,7 +150,7 @@ void mot_params_ind_inductance(float v, float freq, u16 l_type) {
 	hj_n = (float)FOC_PWM_FS / hj_freq;
 	hj_samples = hj_n * 50;
 	K_terms = (s32) (0.5f + hj_samples*hj_freq/(float)FOC_PWM_FS);
-	Vdead = motor.controller.foc.in.dc_vol * 0.5f * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
+	Vdead = motor.controller.foc.in.dc_vol * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
 	hj_w = 360.0f / hj_n;
 	sys_debug("hj %f, %f, %f, %f, %f, %f, %f\n", hj_v, hj_freq, hj_n, hj_samples, K_terms, Vdead, hj_w);
 	float fft_angle = 360.0f / hj_samples * K_terms;
@@ -162,6 +168,7 @@ void mot_params_ind_inductance(float v, float freq, u16 l_type) {
 	v_samples = os_alloc(sizeof(float) * hj_samples);
 	i_samples = os_alloc(sizeof(float) * hj_samples);
 	if (v_samples != NULL && i_samples != NULL) {
+		mot_ctrl_set_ind_freq(mot_contrl(), freq);
 		b_ldq_ind = true;
 		shark_timer_post(&_ldq_ind_timer, 10);
 	}else {
@@ -202,7 +209,7 @@ void mot_params_high_freq_inject(void) {
 		return;
 	}
 	float hj_angle = hj_w * (float)n_samples;
-	rand_angle(hj_angle);
+	norm_angle_deg(hj_angle);
 	float s, c;
 	arm_sin_cos(hj_angle, &s, &c);
 	float vd = 0, vq = 0;
@@ -220,10 +227,10 @@ bool mot_params_hj_sample_vi(float vd, float vq, float id, float iq) {
 	}
 	if ((n_samples >= 1) && (n_samples <= hj_samples)) {
 		if (n_ind_ld == L_TYPE_D) {
-			v_samples[n_samples - 1] = vd * TWO_BY_THREE;
+			v_samples[n_samples - 1] = vd;
 			i_samples[n_samples - 1] = id;
 		}else {
-			v_samples[n_samples - 1] = vq * TWO_BY_THREE;
+			v_samples[n_samples - 1] = vq;
 			i_samples[n_samples - 1] = iq;
 		}
 	}
@@ -240,7 +247,7 @@ void goertzel_dft(float *x, float *real, float *image, float *mag) {
 	}
 	*real = d1 - (d2 * 0.5f * hj_real);
 	*image = -d2 * hj_image;
-	*mag = sqrtf(SQ(*real) + SQ(*image));
+	*mag = NORM2_f(*real, *image);
 }
 
 void mot_params_calc_inductance(void) {
@@ -253,9 +260,11 @@ void mot_params_calc_inductance(void) {
 	goertzel_dft(i_samples, &i_real, &i_image, &i_mag);
 	sys_debug("v %f, %f, %f\n", v_mag/(hj_samples*0.5f), v_real, v_image);
 	sys_debug("i %f, %f, %f\n", i_mag/(hj_samples*0.5f), i_real, i_image);
+	sys_debug("vmag %f, %f\n", v_mag, Vdead * hj_samples*0.5f);
+#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==0
 	v_mag -= Vdead * hj_samples*0.5f;
-	
-	float z_angle = fast_atan_2(i_image, i_real) - fast_atan_2(v_image, v_real);
+#endif
+	float z_angle = fast_atan2(i_image, i_real) - fast_atan2(v_image, v_real);
 	float s,c;
 	arm_sin_cos(pi_2_degree(z_angle), &s, &c);
 
@@ -264,7 +273,7 @@ void mot_params_calc_inductance(void) {
 	float z_image = z_mag * s;
 	float Rs = rs_est_value;
 	float Ri = (SQ(z_real - Rs) + SQ(z_image))/(z_real - Rs + 0.0000001f);
-	float l = Ri * (z_real - Rs)/(hj_freq * 2 * PI * z_image + 0.0000001f) * 0.83f; //0.83f just for v3 board
+	float l = Ri * (z_real - Rs)/(hj_freq * 2 * PI * z_image + 0.0000001f);
 	if (n_ind_ld == L_TYPE_D) {
 		ld_est_value = l;
 		b_ld_ested = true;
@@ -283,8 +292,12 @@ static void _flux_ind_timer_handler(shark_timer_t *t) {
 	float delta = We - motVelRadusPers;
 	motVelRadusPers = motor.controller.foc.mot_vel_radusPers;
 	if (We > 100 && ABS(delta) < 40) {
-		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * motor.controller.foc.in.dc_vol * 1.5f;
+#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==0
+		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_period) * motor.controller.foc.in.dc_vol * 1.5f;
 		float vq = (motor.controller.foc.out.vol_dq.q - dtc) * TWO_BY_THREE;
+#else
+		float vq = motor.controller.phase_v_dq.q;
+#endif
 		float flux = vq / We;
 		flux_est_value = LowPass_Filter(flux_est_value, flux, 0.1f);
 		flux_do_cnt ++;
@@ -318,7 +331,9 @@ void mot_params_ind_flux(float id, float iq) {
 	flux_wait_cnt = 0;
 	flux_do_cnt = 0;
 	b_flux_ested = false;
-	mc_set_foc_mode(CTRL_MODE_CURRENT);
+	old_max_fw_id = mc_conf()->m.max_fw_id;
+	mc_conf()->m.max_fw_id = 0;
+	mc_set_ctrl_mode(CTRL_MODE_CURRENT);
 	mot_contrl_set_current(&motor.controller ,iq);
 	shark_timer_post(&_flux_ind_timer, 10);
 	motVelRadusPers = motor.controller.foc.mot_vel_radusPers;

Applications/foc/motor/motor.c

@@ -14,6 +14,7 @@
 #include "foc/core/F_Calc.h"
 #include "foc/core/etcs.h"
 #include "app/nv_storage.h"
+#include "app/factory.h"
 #include "foc/motor/mot_params_ind.h"
 #include "foc/motor/throttle.h"
 #include "foc/limit.h"
@@ -32,7 +33,7 @@ static void _encoder_zero_off_timer_handler(shark_timer_t *);
 static shark_timer_t _encoder_zero_off_timer = TIMER_INIT(_encoder_zero_off_timer, _encoder_zero_off_timer_handler);
 static void _led_off_timer_handler(shark_timer_t *);
 static shark_timer_t _led_off_timer = TIMER_INIT(_led_off_timer, _led_off_timer_handler);
-m_contrl_t motor = {
+motor_t motor = {
 	.s_direction = POSITIVE,
 	.n_gear = 0,
 	.b_high_vol_mode = false,
@@ -46,7 +47,7 @@ m_contrl_t motor = {
 	.u_set.rpm_lim = RPM_USER_LIMIT_NONE,
 	.u_set.ebrk_lvl = 0,
 	.u_set.n_brkShutPower = CONFIG_Settings_BrkShutPower,
-	.u_set.b_tcs = CONFIG_Settings_TcsEnable,
+	.u_set.b_tcs = 0xFF,
 };
 /* 无感运行的挡位，限制速度，母线电流，最大扭矩 */
 static gear_t sensorless_gear = {
@@ -87,6 +88,10 @@ static void MC_Check_MosVbusThrottle(void) {
 	sys_debug("ibus offset %d\n", offset);
 	sample_ibus_offset(offset);
 
+	if (factory_is_running()) {
+		return;
+	}
+
 	gpio_phase_u_detect(false);
 	float abc[3];
 	get_phase_vols_filtered(abc);
@@ -125,6 +130,9 @@ static void MC_Check_MosVbusThrottle(void) {
 static int hw_brk_err_cnt = 0;
 static int hw_brk_no_err_cnt = 0;
 static u32 _self_check_task(void *p) {
+	if (factory_is_running()) {
+		return 1000;
+	}
 	if (get_tick_ms() < 500) { //启动500ms内检测刹车状态是否正常，应该是没有刹车
 		if (mc_detect_hwbrake() && hw_brk_err_cnt++ >= 60) {
 			mc_set_critical_error(FOC_CRIT_BRK_Err);
@@ -140,8 +148,9 @@ static u32 _self_check_task(void *p) {
 	}
 
 	if (ENC_Check_error()) {
-		mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, -1);
-		mc_set_critical_error(FOC_CRIT_Encoder_Err);
+		if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
+			mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, -1);
+		}
 	}
 	if (get_tick_ms() < 5000) { //启动后5s内检测锁电机线
 		if (mc_is_gpio_mlock()) {
@@ -186,7 +195,7 @@ static bool mc_detect_vbus_mode(void) {
 #endif
 }
 
-static void _mc_internal_init(u8 mode, bool start) {
+static void mc_internal_init(u8 mode, bool start) {
 	motor.mode = mode;
 	motor.throttle = 0;
 	motor.b_start = start;
@@ -204,7 +213,9 @@ static void _mc_internal_init(u8 mode, bool start) {
 	motor.b_limit_pending = false;
 	motor.f_epm_trq = 0;
 	motor.f_epm_vel = 0;
+	motor.vbus_le_cnt = motor.vbus_he_cnt = 0;
 	motor.s_vbus_hw_min = mc_conf()->c.min_dc_vol;
+	motor.s_vbus_hw_max = mc_conf()->c.max_dc_vol;
 }
 
 static void _led_off_timer_handler(shark_timer_t *t) {
@@ -212,22 +223,36 @@ static void _led_off_timer_handler(shark_timer_t *t) {
 }
 
 
-static void mc_gear_mode_set(void) {
+static void mc_gear_mode_set(bool set_imd) {
 	gear_t *gears = mc_gear_conf();
-	if (gears != &sensorless_gear) {
-		sensorless_gear.max_torque = gears->max_torque;
-	}else { //slowly changed
-		line_ramp_set_time(&motor.controller.vel_lim, CONFIG_SENSORLESS_RAMP_TIME);
-		line_ramp_set_time(&motor.controller.dc_curr_lim, CONFIG_SENSORLESS_RAMP_TIME);
+	float max_vel = (float)min(gears->max_speed, motor.u_set.rpm_lim);
+	float max_idc = (float)min(gears->max_idc, motor.u_set.idc_lim);
+	float max_torque = (float)gears->max_torque;
+	if (!foc_observer_is_encoder()) { //无感模式，受限运行
+		line_ramp_set_time(&motor.controller.ramp_vel_lim, CONFIG_SENSORLESS_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.ramp_dc_curr_lim, CONFIG_SENSORLESS_RAMP_TIME);
+		max_vel = min(max_vel, (float)sensorless_gear.max_speed);
+		max_idc = min(max_idc, (float)sensorless_gear.max_idc);
+		max_torque = min(max_torque, (float)sensorless_gear.max_torque);
+	}
+	if (set_imd) {
+		line_ramp_set_time(&motor.controller.ramp_vel_lim, gears->accl_time);
+		line_ramp_set_time(&motor.controller.ramp_dc_curr_lim, gears->accl_time);
+		line_ramp_set_time(&motor.controller.ramp_torque_lim, gears->accl_time);
+	}else {
+		line_ramp_set_time(&motor.controller.ramp_vel_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.ramp_dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.ramp_torque_lim, CONFIG_LIMIT_RAMP_TIME);
 	}
-	mot_contrl_set_vel_limit(&motor.controller, (float)min(gears->max_speed, motor.u_set.rpm_lim));
-	mot_contrl_set_dccurr_limit(&motor.controller, (float)min(gears->max_idc, motor.u_set.idc_lim));
-	mot_contrl_set_torque_limit(&motor.controller, (float)gears->max_torque);
+	mot_contrl_set_vel_limit(&motor.controller, max_vel);
+	mot_contrl_set_dccurr_limit(&motor.controller, max_idc);
+	mot_contrl_set_torque_limit(&motor.controller, max_torque);
 }
 
 void mc_init(void) {
+	mc_internal_init(CTRL_MODE_OPEN, false);
 	fan_pwm_init();
-	adc_init();
+	adc_init(false);
 	pwm_3phase_init();
 	samples_init();
 	motor_encoder_init();
@@ -244,15 +269,12 @@ void mc_init(void) {
 	shark_timer_post(&_led_off_timer, 5000);
 }
 
-m_contrl_t * mc_params(void) {
+motor_t * get_motor(void) {
 	return &motor;
 }
 
 gear_t *mc_gear_conf_by_gear(u8 n_gear) {
 	gear_t *gears;
-	if (!foc_observer_is_encoder()) { //无感模式，受限运行
-		return &sensorless_gear;
-	}
 	if (motor.b_high_vol_mode) {
 		gears = &mc_conf()->g_n[0];
 	}else {
@@ -283,17 +305,20 @@ float mc_gear_max_torque(s16 vel, u8 gear_n) {
 	if (vel_idx >= CONFIG_GEAR_SPEED_TRQ_NUM -1 ) {
 		return gear_rpm_2_torque(gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], gear->max_torque);
 	}
-	float torque_1 = gear_rpm_2_torque(gear->torque[vel_idx-1], gear->max_torque);
+	float torque_1 = gear_rpm_2_torque(gear->torque[vel_idx], gear->max_torque);
 	float min_rpm = vel_idx * 1000;
-	float torque_2 = gear_rpm_2_torque(gear->torque[vel_idx], gear->max_torque);
+	float torque_2 = gear_rpm_2_torque(gear->torque[vel_idx + 1], gear->max_torque);
 	float max_rpm = min_rpm + 1000;
 	return f_map((float)vel, min_rpm, max_rpm, torque_1, torque_2);
 }
 
-
+float mc_get_max_torque_now(void) {
+	return mc_gear_max_torque(motor.controller.foc.mot_velocity_filterd, motor.n_gear);
+}
 /* 必须立即停机 */
 bool mc_critical_need_stop(void) {
-	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Vol_HW_Err);
+	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Phase_Err) |
+				FOC_Cri_Err_Mask(FOC_CRIT_Vol_HW_Err) | FOC_Cri_Err_Mask(FOC_CRIT_OV_Vol_Err);
 	u32 err = motor.n_CritiCalErrMask & mask;
 	return (err != 0);
 }
@@ -341,7 +366,7 @@ bool mc_start(u8 mode) {
 #endif
 	motor.s_force_torque = MAX_S16;
 	mc_detect_vbus_mode();
-	etcs_enable(&motor.controller.etcs, motor.u_set.b_tcs);
+	etcs_enable(&motor.controller.etcs, mc_tcs_is_enabled());
 	if (motor.b_lock_motor) {
 		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
@@ -356,7 +381,7 @@ bool mc_start(u8 mode) {
 		mot_contrl_set_error(&motor.controller, FOC_Param_Err);
 		return false;
 	}
-	if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+	if (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
 		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -369,13 +394,13 @@ bool mc_start(u8 mode) {
 
 	u32 mask = cpu_enter_critical();
 
-	_mc_internal_init(mode, true);
+	mc_internal_init(mode, true);
 	throttle_torque_reset();
 	motor_encoder_start(true);
 	mot_contrl_start(&motor.controller, mode);
 	mot_contrl_set_torque_ramp_time(&motor.controller, mc_gear_conf()->zero_accl, mc_conf()->c.thro_dec_time);
 	mc_set_ebrk_level(motor.u_set.ebrk_lvl);
-	mc_gear_mode_set();
+	mc_gear_mode_set(false);
 	cpu_exit_critical(mask);
 
 	pwm_turn_on_low_side();
@@ -411,17 +436,13 @@ bool mc_stop(void) {
 		return false;
 	}
 
-	if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+	if (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
 		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
 		return false;
 	}
-	if (!mc_throttle_released()) {
-		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
-		return false;
-	}
 
 	u32 mask = cpu_enter_critical();
-	_mc_internal_init(CTRL_MODE_OPEN, false);
+	mc_internal_init(CTRL_MODE_OPEN, false);
 	adc_stop_convert();
 	pwm_stop();
 	mot_contrl_stop(&motor.controller);
@@ -452,14 +473,49 @@ bool mc_set_gear(u8 gear) {
 		return false;
 	}
 	if (motor.n_gear != gear) {
+#if TURBO_GEAR_AUTO_EXIT_TIME>0
+		if (gear == TURBO_GEAR) {
+			if (motor.mos_lim != 0 || motor.motor_lim != 0) {
+				mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
+				return false;
+			}
+			if (get_motor_temp_raw() >= TURBO_MIN_MOTOR_TEMP || get_mos_temp_raw() >= TURBO_MIN_MOS_TEMP) {
+				mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
+				return false;
+			}
+			motor.gear_last = motor.n_gear;
+			motor.turbo_time = get_tick_ms();
+		}
+#endif
 		u32 mask = cpu_enter_critical();
 		motor.n_gear = gear;
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 		cpu_exit_critical(mask);
 	}
 	return true;
 }
 
+
+static void mc_turbo_auto_exit(void) {
+#if TURBO_GEAR_AUTO_EXIT_TIME>0
+	if (motor.n_gear != TURBO_GEAR) {
+		return;
+	}
+	if (motor.mos_lim != 0 || motor.motor_lim != 0) {
+		mc_set_gear(motor.gear_last);
+		return;
+	}
+	u32 delta_time = get_delta_ms(motor.turbo_time);
+	if (delta_time >= TURBO_GEAR_AUTO_EXIT_TIME) {
+		motor.turbo_remain_sec = 0;
+		mc_set_gear(motor.gear_last);
+		return;
+	}else {
+		motor.turbo_remain_sec = (TURBO_GEAR_AUTO_EXIT_TIME - delta_time) / 1000;
+	}
+#endif
+}
+
 u8 mc_get_gear(void) {
 	if (motor.n_gear == 3){
 		return 0;
@@ -471,6 +527,15 @@ u8 mc_get_internal_gear(void) {
 	return motor.n_gear;
 }
 
+bool mc_tcs_is_enabled(void) {
+	bool tcs_enabled = mc_conf()->s.tcs_enable;
+	if (motor.u_set.b_tcs == 0) {
+		tcs_enabled = false;
+	}else if (motor.u_set.b_tcs == 1) {
+		tcs_enabled = true;
+	}
+	return tcs_enabled;
+}
 
 bool mc_hwbrk_can_shutpower(void) {
 	if (motor.u_set.n_brkShutPower != MAX_U8) {
@@ -535,7 +600,7 @@ void mc_set_idc_limit(s16 limit) {
 		return;
 	}
 	motor.u_set.idc_lim = limit;
-	mc_gear_mode_set();
+	mc_gear_mode_set(false);
 }
 
 void mc_set_rpm_limit(s16 limit) {
@@ -543,7 +608,7 @@ void mc_set_rpm_limit(s16 limit) {
 		return;
 	}
 	motor.u_set.rpm_lim = limit;
-	mc_gear_mode_set();
+	mc_gear_mode_set(false);
 }
 
 
@@ -562,7 +627,7 @@ void mc_enable_brkshutpower(u8 shut) {
 }
 
 void mc_enable_tcs(bool enable) {
-	motor.u_set.b_tcs = enable;
+	motor.u_set.b_tcs = enable?1:0;
 	etcs_enable(&motor.controller.etcs, enable);
 }
 
@@ -580,7 +645,7 @@ u16 mc_get_ebrk_time(void) {
 	return 0xFFFF;
 }
 
-bool mc_set_foc_mode(u8 mode) {
+bool mc_set_ctrl_mode(u8 mode) {
 	if (mode == motor.mode) {
 		return true;
 	}
@@ -590,7 +655,7 @@ bool mc_set_foc_mode(u8 mode) {
 	if (mc_critical_can_not_run()) {
 		return false;
 	}
-	if ((mode == CTRL_MODE_OPEN) && (ABS(mot_contrl_get_speed(&motor.controller)) > CONFIG_ZERO_SPEED_RPM)) {
+	if ((mode == CTRL_MODE_OPEN) && (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM)) {
 		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
@@ -643,7 +708,7 @@ bool mc_start_epm(bool epm) {
 		mot_contrl_request_mode(&motor.controller, CTRL_MODE_TRQ);
 		mot_contrl_set_torque_limit_rttime(&motor.controller, CONFIG_LIMIT_RAMP_TIME);
 		mot_contrl_set_vel_rttime(&motor.controller, CONFIG_CRUISE_RAMP_TIME);
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 	}
 	cpu_exit_critical(mask);
 	
@@ -677,16 +742,12 @@ bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
 		
 		if (!mot_contrl_is_start(&motor.controller)) {
 			mot_contrl_start(&motor.controller, motor.mode);
-			mc_gear_mode_set();
+			mc_gear_mode_set(false);
 			pwm_enable_channel();
 		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
 		}
-		if (dir == EPM_Dir_Back) {
-			mot_contrl_velloop_params(&motor.controller, 0.2f, 7.5f);
-		}else {
-			mot_contrl_velloop_params(&motor.controller, 0.2f, 7.5f);
-		}
+		mot_contrl_velloop_params(&motor.controller, mc_conf()->c.epm_pid.kp, mc_conf()->c.epm_pid.ki);
 		mot_contrl_set_torque_limit_rttime(&motor.controller, 2.0);
 		mot_contrl_set_torque_limit(&motor.controller, motor.f_epm_trq);
 		mot_contrl_set_vel_rttime(&motor.controller, 1000.0f);
@@ -823,6 +884,8 @@ bool mc_ind_motor_start(bool start) {
 			mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 			return false;
 		}
+		adc_init(true);
+		task_udelay(500);
 		pwm_up_enable(false);
 		pwm_turn_on_low_side();
 		task_udelay(500);
@@ -843,6 +906,7 @@ bool mc_ind_motor_start(bool start) {
 		wdog_reload();
 		adc_stop_convert();
 		pwm_stop();
+		adc_init(false);
 		mot_contrl_stop(&motor.controller);
 		motor.mode = CTRL_MODE_OPEN;
 		pwm_up_enable(true);
@@ -863,7 +927,7 @@ static void _encoder_zero_off_timer_handler(shark_timer_t *t){
 	adc_stop_convert();
 	pwm_stop();
 	mot_contrl_stop(&motor.controller);
-	_mc_internal_init(CTRL_MODE_OPEN, false);
+	mc_internal_init(CTRL_MODE_OPEN, false);
 	motor.b_calibrate = false;
 }
 
@@ -877,7 +941,7 @@ bool mc_encoder_zero_calibrate(s16 vd) {
 			adc_stop_convert();
 			pwm_stop();
 			mot_contrl_stop(&motor.controller);
-			_mc_internal_init(CTRL_MODE_OPEN, false);
+			mc_internal_init(CTRL_MODE_OPEN, false);
 			motor.b_calibrate = false;
 			motor.b_ignor_throttle = false;
 		}
@@ -890,7 +954,7 @@ bool mc_encoder_zero_calibrate(s16 vd) {
 		mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 		return false;
 	}
-	_mc_internal_init(CTRL_MODE_OPEN, true);
+	mc_internal_init(CTRL_MODE_OPEN, true);
 	motor.b_calibrate = true;
 	pwm_turn_on_low_side();
 	task_udelay(500);
@@ -1136,7 +1200,7 @@ void MC_Protect_IRQHandler(void){
 	}
 	mc_set_critical_error(FOC_CRIT_Phase_Err);
 	mc_save_err_runtime();
-	_mc_internal_init(CTRL_MODE_OPEN, false);
+	mc_internal_init(CTRL_MODE_OPEN, false);
 	adc_stop_convert();
 	pwm_stop();
 	mot_contrl_stop(&motor.controller);
@@ -1152,36 +1216,49 @@ void motor_debug(void) {
 	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.sensorless_rpm, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
 }
 
-static void motor_vbus_crit_low(s16 curr_vbus) {
-	static u16 _vbus_e_count = 0;
-
-	if (curr_vbus < motor.s_vbus_hw_min) {
-		_vbus_e_count ++;
-		if (_vbus_e_count >= 2) {
+static void motor_vbus_crit_check(s16 curr_vbus) {
+	if (curr_vbus <= motor.s_vbus_hw_min) {
+		motor.vbus_le_cnt ++;
+		if (motor.vbus_le_cnt >= 2) {
 			if (mot_contrl_is_start(&motor.controller)) {
 				pwm_disable_channel();
 				mc_save_err_runtime();
 				mot_contrl_stop(&motor.controller);
 			}
 			if (mc_set_critical_error(FOC_CRIT_Vol_HW_Err)) {
-				if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+				if (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
 					mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
 				}
 			}
 		}
 	}else {
-		_vbus_e_count = 0;
+		motor.vbus_le_cnt = 0;
+	}
+	if (curr_vbus >= motor.s_vbus_hw_max) {
+		motor.vbus_he_cnt ++;
+		if (motor.vbus_he_cnt >= 1) {
+			if (mot_contrl_is_start(&motor.controller)) {
+				pwm_disable_channel();
+				mc_save_err_runtime();
+				mot_contrl_stop(&motor.controller);
+			}
+			if (mc_set_critical_error(FOC_CRIT_OV_Vol_Err)) {
+				mc_crit_err_add_s16(FOC_CRIT_OV_Vol_Err, curr_vbus);
+			}
+		}
+	}else {
+		motor.vbus_he_cnt = 0;
 	}
 }
 
 void TIMER_UP_IRQHandler(void){
 	if (!motor.b_start && !mot_contrl_is_start(&motor.controller)) {
 		motor_encoder_update(false);
-		motor_vbus_crit_low((s16)get_vbus_int());
+		motor_vbus_crit_check((s16)get_vbus_int());
 	}
 }
 
-measure_time_t g_meas_foc = {.exec_max_time = 25, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
+measure_time_t g_meas_foc = {.exec_max_time = 31, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 #define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
 #define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
 #if (CONFIG_ENABLE_IAB_REC==1)
@@ -1189,6 +1266,7 @@ measure_time_t g_meas_foc = {.exec_max_time = 25, .intval_max_time = 62,  .intva
 static s16 ia[CONFIG_IAB_REC_COUNT], ib[CONFIG_IAB_REC_COUNT];
 static int iab_w_count = 0, iab_r_count = 0;
 static bool b_iab_rec = false;
+static bool b_vab_rec = false;
 extern void can_plot2(s16 v1, s16 v2);
 #endif
 /*ADC 电流采集中断，调用FOC的核心处理函数*/
@@ -1208,14 +1286,20 @@ void ADC_IRQHandler(void) {
 		ia[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[0];
 		ib[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[1];
 		iab_w_count ++;
+	}else if (b_vab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
+		ia[iab_w_count] = (s16)motor.controller.phase_v_ab.a;
+		ib[iab_w_count] = (s16)motor.controller.foc.out.vol_albeta.a*TWO_BY_THREE;
+		iab_w_count ++;
 	}
 #endif
-	motor_vbus_crit_low((s16)sample_vbus_raw()); //need fast detect vbus very low, to stop the motor
+	motor_vbus_crit_check((s16)sample_vbus_raw()); //need fast detect vbus very low, to stop the motor
 	float vd, vq;
 	if (motor.b_ind_start) {
 		mot_params_high_freq_inject();
-		vd = motor.controller.foc.out.vol_dq.d;
-		vq = motor.controller.foc.out.vol_dq.q;
+#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==0
+		vd = motor.controller.foc.out.vol_dq.d * TWO_BY_THREE;
+		vq = motor.controller.foc.out.vol_dq.q * TWO_BY_THREE;
+#endif
 	}
 	if (!mot_contrl_update(&motor.controller)) {/* FOC 角度错误，立即停机 */
 		if (mot_contrl_is_start(&motor.controller)) {
@@ -1245,6 +1329,10 @@ void ADC_IRQHandler(void) {
 	if (motor.b_ind_start) {
 		float id = motor.controller.foc.out.curr_dq.d;
 		float iq = motor.controller.foc.out.curr_dq.q;
+#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==1
+		vd = motor.controller.phase_v_dq.d;
+		vq = motor.controller.phase_v_dq.q;
+#endif
 		mot_params_hj_sample_vi(vd, vq, id, iq);
 	}
 	TIME_MEATURE_END();
@@ -1252,7 +1340,7 @@ void ADC_IRQHandler(void) {
 
 #if (CONFIG_ENABLE_IAB_REC==1)
 static void _iab_plot_timer_handler(shark_timer_t *t) {
-	if (!b_iab_rec) {
+	if (!b_iab_rec && !b_vab_rec) {
 		return;
 	}
 	if (iab_r_count < iab_w_count) {
@@ -1262,18 +1350,26 @@ static void _iab_plot_timer_handler(shark_timer_t *t) {
 	}
 }
 static shark_timer_t _iab_plot_timer = TIMER_INIT(_iab_plot_timer, _iab_plot_timer_handler);
-void mc_start_current_rec(bool rec) {
-	if (b_iab_rec == rec) {
-		return;
-	}
+void mc_start_current_rec(bool is_i, bool rec) {
+
 	if (!rec) {
 		b_iab_rec = false;
+		b_vab_rec = false;
 		shark_timer_cancel(&_iab_plot_timer);
 		return;
 	}
+
+	if (b_iab_rec == true || b_vab_rec == true) {
+		return;
+	}
+
 	iab_w_count = 0;
 	iab_r_count = 0;
-	b_iab_rec = true;
+	if (is_i) {
+		b_iab_rec = true;
+	}else {
+		b_vab_rec = true;
+	}
 	shark_timer_post(&_iab_plot_timer, 100);
 }
 #endif
@@ -1287,7 +1383,7 @@ static bool mc_run_stall_process(u8 run_mode) {
 			motor.runStall_time = 0;
 			motor.b_runStall = false; //转把释放，清除堵转标志
 		}else if (mot_contrl_get_current_vector(&motor.controller) >= CONFIG_STALL_MAX_CURRENT){
-			if (ABS(mot_contrl_get_speed(&motor.controller)) < 1.0f && (motor.runStall_time == 0)) {
+			if (mot_contrl_get_speed_abs(&motor.controller) < 1 && (motor.runStall_time == 0)) {
 				motor.runStall_time = get_tick_ms();
 				motor.runStall_pos = motor_encoder_get_position();
 			}
@@ -1377,7 +1473,7 @@ static bool mc_process_force_running(void) {
 				--_force_wait;
 			}else {
 				_force_angle += 1.5f;
-				rand_angle(_force_angle);
+				norm_angle_deg(_force_angle);
 				mot_contrl_set_angle(&motor.controller, _force_angle);
 			}
 		}
@@ -1398,7 +1494,7 @@ static void mc_process_brake_light(void) {
 static void mc_process_curise(void) {
 	static bool can_pause_resume = false;
 	if (motor.b_cruise) {
-		if (mot_contrl_get_speed(&motor.controller) < CONFIG_CRUISE_EXIT_RPM) {
+		if (mot_contrl_get_speed_abs(&motor.controller) < CONFIG_CRUISE_EXIT_RPM) {
 			mot_contrl_set_cruise(&motor.controller, false);
 			return;
 		}
@@ -1446,7 +1542,7 @@ static bool mc_can_stop_foc(void) {
 			return true;
 		}
 	}
-	if (mc_throttle_released() && mot_contrl_get_speed(&motor.controller) == 0.0f) {
+	if (mc_throttle_released() && ((mot_contrl_get_speed_abs(&motor.controller)) == 0)) {
 		if (!mot_contrl_is_auto_holdding(&motor.controller) && motor.epm_dir == EPM_Dir_None) {
 			return true;
 		}
@@ -1459,11 +1555,14 @@ static bool mc_can_stop_foc(void) {
 }
 
 static bool mc_can_restart_foc(void) {
-	bool can_start = (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None)) && (!mc_critical_can_not_run());
+	if (mc_critical_can_not_run()) {
+		return false;
+	}
+	bool can_start = (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None));
 	if (!foc_observer_is_encoder() && !foc_observer_sensorless_stable()){
 		return false;
 	}
-	if ((motor.s_target_speed != MAX_S16 && motor.s_target_speed != 0) && (!mc_critical_can_not_run()) && motor.mode == CTRL_MODE_SPD) {
+	if ((motor.s_target_speed != MAX_S16 && motor.s_target_speed != 0) && motor.mode == CTRL_MODE_SPD) {
 		return true;
 	}
 	return can_start;
@@ -1476,18 +1575,21 @@ static void mc_motor_runstop(void) {
 		mot_contrl_stop(&motor.controller);
 		pwm_disable_channel();
 		g_meas_foc.first = true;
+		motor.foc_stop_cnt ++;
 		cpu_exit_critical(mask);
 	}
 	if (!mot_contrl_is_start(&motor.controller) && mc_can_restart_foc()) {
 		mask = cpu_enter_critical();
 		mot_contrl_start(&motor.controller, motor.mode);
-		mc_gear_mode_set();
+		mc_gear_mode_set(true);
 		throttle_torque_reset();
 		pwm_enable_channel();
 		g_meas_foc.first = true;
+		motor.foc_start_cnt ++;
 		cpu_exit_critical(mask);
 	}
 }
+
 static void mc_process_throttle_torque(float vol) __attribute__((unused));
 static void mc_process_throttle_torque(float vol) {
 	float torque = throttle_get_torque(&motor.controller, vol);
@@ -1498,7 +1600,7 @@ static void mc_process_throttle_torque(float vol) {
 			return;
 		}
 #endif
-		if (mot_contrl_energy_recovery(&motor.controller, true)) {
+		if (!mot_contrl_ebrk_is_running(&motor.controller) && mot_contrl_set_ebreak(&motor.controller, true)) {
 			return;
 		}
 	}
@@ -1511,18 +1613,16 @@ static void mc_process_throttle_torque(float vol) {
 			mot_contrl_set_target_vel(&motor.controller, vel_ref);
 		}
 	}else if (motor.controller.mode_running == CTRL_MODE_EBRAKE){
+		/* 等待输入扭矩先减小到0 */
+		if (line_ramp_get_interp(&motor.controller.ramp_input_torque) >= 0.001f) {
+			return;
+		}
+		mot_contrl_set_ebrk_time(&motor.controller, motor.controller.ebrk_ramp_time);
 		float vel = mot_contrl_get_speed(&motor.controller);
 		float ebrk_trq = motor_get_ebreak_toruqe(vel);
-			
-		if (ebrk_trq >= -mot_contrl_get_ebrk_torque(&motor.controller)/2){
-			mot_contrl_set_ebrk_time(&motor.controller, 1);
-		}
-		if (ebrk_trq != 0) {
-			mot_contrl_set_torque(&motor.controller, ebrk_trq);
-		}	
-		if ((mot_contrl_get_final_torque(&motor.controller) < 0.0001f && vel < CONFIG_MIN_RPM_EXIT_EBRAKE) ||
-			(!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f)))) {
-			mot_contrl_energy_recovery(&motor.controller, false);
+		mot_contrl_set_torque(&motor.controller, ebrk_trq);
+		if ((vel < CONFIG_MIN_RPM_EXIT_EBRAKE) || (!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f)))) {
+			mot_contrl_set_ebreak(&motor.controller, false);
 			throttle_torque_reset();
 		}
 	}
@@ -1569,14 +1669,16 @@ void Sched_MC_mTask(void) {
 	}
 	bool sensor_less = !foc_observer_is_encoder();
 	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L) || (_sensorless_run != sensor_less)) {
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 		if (sensor_less && foc_observer_sensorless_stable()) {//unstable 记录在ADC中断处理中
 			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
-				mc_set_critical_error(FOC_CRIT_Encoder_Err);
-				mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
+				if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
+					mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
+				}
 			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
-				mc_set_critical_error(FOC_CRIT_ENC_AB_Err);
-				mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
+				if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
+					mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
+				}
 			}
 		}
 		motor.b_limit_pending = false;
@@ -1588,7 +1690,7 @@ void Sched_MC_mTask(void) {
 	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
 	if (motor.b_limit_pending && mc_throttle_released()) {
 		motor.b_limit_pending = false;
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 	}
 	/* 堵转处理 */
 	if (mc_run_stall_process(runMode) || (motor.mode == CTRL_MODE_CURRENT)) {
@@ -1608,6 +1710,7 @@ void Sched_MC_mTask(void) {
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
 #ifndef CONFIG_DQ_STEP_RESPONSE
 		mc_autohold_process();
+		mc_turbo_auto_exit();
 		if (motor.mode != CTRL_MODE_OPEN) {
 			mc_motor_runstop();
 		}

Applications/foc/motor/motor.h

@@ -10,6 +10,10 @@
 #define IDC_USER_LIMIT_NONE ((s16)0x3fff)
 #define RPM_USER_LIMIT_NONE ((s16)0x3fff)
 
+#define TURBO_GEAR 3
+#define TURBO_GEAR_AUTO_EXIT_TIME 0//30*1000 //30S
+#define TURBO_MIN_MOTOR_TEMP 90
+#define TURBO_MIN_MOS_TEMP 80
 typedef struct {
 	bool running;
 	u32  start_ts;
@@ -23,7 +27,7 @@ typedef struct {
 	s16 rpm_lim;
 	u8  ebrk_lvl;
 	u8  n_brkShutPower;
-	bool  b_tcs;
+	u8  b_tcs;
 }user_rt_set;
 
 typedef struct {
@@ -61,14 +65,25 @@ typedef struct {
 	u8     mos_lim;
 	u8     motor_lim;
 	s16    s_vbus_hw_min;
+	s16    s_vbus_hw_max;
+	u16    vbus_le_cnt;
+    u16    vbus_he_cnt;
 	s16    s_target_speed;
 	s16    s_force_torque;
+	u8     gear_last;
+	u32    turbo_time;
+	u8 	   turbo_remain_sec;
 	mot_contrl_t controller;
 	user_rt_set u_set; //用户运行时设置
 	fan_t  fan[2];
-}m_contrl_t;
-extern m_contrl_t motor;
-m_contrl_t * mc_params(void);
+
+	//for debug
+	int foc_stop_cnt;
+	int foc_start_cnt;
+}motor_t;
+
+extern motor_t motor;
+motor_t * get_motor(void);
 void mc_init(void);
 bool mc_start(u8 mode);
 bool mc_stop(void);
@@ -80,7 +95,7 @@ void mc_set_throttle_r(bool use, u8 r);
 void mc_use_throttle(void);
 bool mc_current_sensor_calibrate(float current);
 bool mc_encoder_zero_calibrate(s16 vd);
-bool mc_set_foc_mode(u8 mode);
+bool mc_set_ctrl_mode(u8 mode);
 bool mc_is_epm(void);
 bool mc_start_epm(bool epm);
 bool mc_start_epm_move(epm_dir_t dir, bool is_command);
@@ -113,13 +128,15 @@ s16 mc_get_ebrk_torque(void);
 u16 mc_get_ebrk_time(void);
 bool mc_critical_err_is_set(u8 err);
 bool mc_hwbrk_can_shutpower(void);
-void mc_start_current_rec(bool rec);
+void mc_start_current_rec(bool is_i, bool rec);
 bool mc_ind_motor_start(bool start);
 void mc_enable_brkshutpower(u8 shut);
 void mc_enable_tcs(bool enable);
 bool mc_set_target_vel(s16 vel);
 float mc_gear_max_torque(s16 vel, u8 gear);
 bool mc_set_force_torque(s16 torque);
+bool mc_tcs_is_enabled(void);
+float mc_get_max_torque_now(void);
 
 static __INLINE mot_contrl_t *mot_contrl(void) {
 	return &motor.controller;

Applications/foc/motor/motor_param.c

@@ -97,6 +97,148 @@ int motor_map_torque_max_count(void) {
 
 #ifdef MOT_HAVE_MAPS
 
+vol_rpm_torque_map vol_rpm_curr_map[CONFIG_MAX_VOL_COUNT];
+u8 vol_n = CONFIG_MAX_VOL_COUNT;
+static void motor_vol_lookup(s16 vol, vol_rpm_torque_map *table, vol_rpm_torque_map **out_mapl, vol_rpm_torque_map **out_maph) {
+	if (vol_n == 1) {
+		*out_mapl = table;
+        *out_maph = table;
+		return;
+	}
+	if (vol <= table[0].vol) {
+		*out_mapl = table;
+        *out_maph = table + 1;
+		return;
+	}else if (vol >= table[vol_n - 1].vol){
+		*out_mapl = table + vol_n - 2;
+        *out_maph = table + vol_n - 1;
+		return;
+	}
+	for (int i = 1; i < vol_n; i++) {
+		if ((vol <= table[i].vol)) {
+			*out_mapl = table + i - 1;
+			*out_maph = table + i;
+			break;
+		}
+	}
+}
+
+static void motor_rpm_lookup(s16 rpm, vol_rpm_torque_map *map, rpm_torque_map **out_map1, rpm_torque_map **out_maph) {
+	rpm_torque_map *table = map->rpm_torque;
+	if (rpm <= table[0].rpm) {
+		*out_map1 = table;
+        *out_maph = table + 1;
+		return;
+	}else if (rpm >= table[map->n - 1].rpm) {
+		*out_map1 = *out_maph = table + map->n - 1;
+		return;
+	}
+	for (int i = 1; i < map->n; i++) {
+		if (rpm <= table[i].rpm) {
+			*out_map1 = table + i - 1;
+			*out_maph = table + i;
+			break;
+		}
+	}
+}
+
+static void motor_current_lookup(s16 torque, rpm_torque_map *map,  torque2dq_t**currl, torque2dq_t **currh) {
+	torque2dq_t *table = map->dqmap;
+	if (torque <= table[0].torque) {
+		*currl = table;
+        *currh = table + 1;
+	}else if (torque >= table[map->n - 1].torque) {
+		*currl = table + map->n - 2;
+        *currh = table + map->n - 1;
+	}
+	for (int i = 1; i < map->n; i++) {
+		if (torque <= table[i].torque) {
+			*currl = table + i - 1;
+			*currh = table + i;
+			break;
+		}
+	}
+}
+
+static void motor_torque_lookup(s16 vel, s16 torque, rpm_torque_map *mapl, rpm_torque_map *maph, dq_t *dq_out) {
+	torque2dq_t *currl, *currh;
+	motor_current_lookup(torque, mapl, &currl, &currh);
+	float dl = line_intp(torque, currl->torque, currh->torque, currl->d, currh->d);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", mapl->rpm, currl->torque, currh->torque, dl);
+	motor_current_lookup(torque, maph, &currl, &currh);
+	float dh = line_intp(torque, currl->torque, currh->torque, currl->d, currh->d);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->torque, currh->torque, dh);
+	float d = line_intp(vel, mapl->rpm, maph->rpm, dl, dh);
+    _DEBUG("id = %f\n", d);
+
+	float ql = line_intp(torque, currl->torque, currh->torque, currl->q, currh->q);
+    _DEBUG("rpm %d -> curr %d : %d, iq=%f\n", mapl->rpm, currl->torque, currh->torque, ql);
+	motor_current_lookup(torque, maph, &currl, &currh);
+	float qh = line_intp(torque, currl->torque, currh->torque, currl->q, currh->q);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->torque, currh->torque, qh);
+	float q = line_intp(vel, mapl->rpm, maph->rpm, ql, qh);
+    _DEBUG("id = %f\n", d);
+	dq_out->d = d;
+	dq_out->q = q;
+}
+
+#define SCALE(X) (X/10)
+
+void motor_mpta_fw_lookup2(float rpm, float vdc, float torque, dq_t *dq_out) {
+	vol_rpm_torque_map *vmapl, *vmaph;
+	rpm_torque_map *rmapl, *rmaph;
+	dq_t dql, dqh;
+	s16 vel = ABS(((s16)rpm));
+	s16 bus_vol = vdc;
+    torque = torque * 10;
+	/* 通过当前电压查找 对应的转速扭矩 */
+	motor_vol_lookup(bus_vol, vol_rpm_curr_map, &vmapl, &vmaph);
+	_DEBUG("vol %d : %d\n", vmapl->vol, vmaph->vol);
+	/* 通过当前转速查找对应的扭矩 */
+	motor_rpm_lookup(vel, vmapl, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmapl->vol, rmapl->rpm, rmaph->rpm);
+	/* 通过当前扭矩需求插值计算dq电流 */
+	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &dql);
+
+	motor_rpm_lookup(vel, vmaph, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmaph->vol, rmapl->rpm, rmaph->rpm);
+
+	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &dqh);
+
+	float d = line_intp(bus_vol, vmapl->vol, vmaph->vol, dql.d, dqh.d);
+	float q = line_intp(bus_vol, vmapl->vol, vmaph->vol, dql.q, dqh.q);
+
+	dq_out->d = SCALE(d);
+	dq_out->q = SCALE(q) * SIGN(torque);
+}
+
+
+/* 通过电机外特性曲线获取当前转速和母线电压下的最大扭矩 */
+s16 motor_torque_ext_char_curve(s16 rpm, s16 vdc) {
+	vol_rpm_torque_map *vmapl, *vmaph;
+	rpm_torque_map *rmapl, *rmaph;
+	s16 vel = ABS(rpm);
+	/* 通过当前电压查找 对应的转速扭矩 */
+	motor_vol_lookup(vdc, vol_rpm_curr_map, &vmapl, &vmaph);
+	_DEBUG("vol %d : %d\n", vmapl->vol, vmaph->vol);
+	/* 获取低电压对应当前转速的最大扭矩 */
+	motor_rpm_lookup(vel, vmapl, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmapl->vol, rmapl->rpm, rmaph->rpm);
+	s16 max_t1 = rmapl->dqmap[rmapl->n - 1].torque;
+	s16 max_t2 = rmaph->dqmap[rmaph->n - 1].torque;
+	s16 low_vel_max_t = line_intp(vel, rmapl->rpm, rmaph->rpm, max_t1, max_t2);
+
+	/* 获取高电压对应当前转速的最大扭矩 */
+	motor_rpm_lookup(vel, vmaph, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmaph->vol, rmapl->rpm, rmaph->rpm);
+	max_t1 = rmapl->dqmap[rmapl->n - 1].torque;
+	max_t2 = rmaph->dqmap[rmaph->n - 1].torque;
+	s16 high_vel_max_t = line_intp(vel, rmaph->rpm, rmaph->rpm, max_t1, max_t2);
+
+	/* 对两个电压的扭矩插值，获取最终的最大扭矩 */
+	return line_intp(vdc, vmapl->vol, vmaph->vol, low_vel_max_t, high_vel_max_t);
+}
+
 //x -> rpm
 //z -> torque
 static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float *q) {
@@ -120,7 +262,7 @@ static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float
 #ifdef MOT_USE_PHASE_I
 	if (z != 0) {
 		if (z >= ABS(*d)) {
-			*q = sqrtf(z*z - (*d)*(*d));
+			*q = sqrtsub2_f(z, (*d));
 		}else {
 			c1 = (1.0f - frac_z1) * map[0]->q + frac_z1 * map[1]->q;
 			c2 = (1.0f - frac_z2) * map[2]->q + frac_z2 * map[3]->q;
@@ -215,7 +357,7 @@ void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out) {
 		arm_sin_cos(advanced_angle + 90.0f, &s, &c);
 		d = ABS(torque) * c;
 		d = fclamp(d, -mc_conf()->m.max_fw_id, mc_conf()->m.max_fw_id);
-		q = sqrtf(SQ(torque) - SQ(d));
+		q = sqrtsub2_f(torque, d);
 		if (torque < 0) {
 			q = -q;
 		}
@@ -231,10 +373,15 @@ void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out) {
 		}
 	}
 #else
+#if CONFIG_CONTRL_FW_ENABLE
+	if ((mot_contrl()->duty_filterd >= CONFIG_CONTRL_FW_START_DUTY) && (CONFIG_CONTRL_FW_START_DUTY < CONFIG_SVM_MODULATION)) {
+		d = -f_map(mot_contrl()->duty_filterd, CONFIG_CONTRL_FW_START_DUTY, CONFIG_SVM_MODULATION, 0, mc_conf()->m.max_fw_id);
+	}
+#endif
 	q = torque;
 #endif
-	step_towards(&dq_out->d, d, 1.0f);
-	step_towards(&dq_out->q, q, 0.7f);
+ 	step_towards(&dq_out->d, d, 10.0f);
+	step_towards(&dq_out->q, q, 6.0f);
 }
 
 #endif

Applications/foc/motor/motor_param.h

@@ -4,6 +4,9 @@
 #include "os/os_types.h"
 #include "foc/core/controller.h"
 
+#define CONFIG_MAX_D_COUNT 20
+#define CONFIG_MAX_VEL_COUNT 20
+#define CONFIG_MAX_VOL_COUNT 5
 /* 电机外特性map, 每个转速点对应的最大扭矩 */
 typedef struct {
 	s16 rpm;
@@ -21,6 +24,26 @@ typedef struct {
 	float lq;
 }iq_lq_map_t;
 typedef torque2dq_t torque_map_t;
+
+typedef struct {
+	s16 curr;
+	s16 id;
+}curr_pair_t;
+
+/* 转速对应的d轴电流 */
+typedef struct {
+	s16 rpm;
+	u8  n;
+	torque2dq_t dqmap[CONFIG_MAX_D_COUNT];
+}rpm_torque_map;
+
+/* 电压、转速对应的d轴电流 */
+typedef struct {
+	s16 vol;
+	u8  n;
+	rpm_torque_map rpm_torque[CONFIG_MAX_VEL_COUNT];
+}vol_rpm_torque_map;
+
 s16 motor_max_torque_for_rpm(s16 rpm);
 void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out);
 float motor_get_lq_from_iq(s16 iq);

Applications/foc/motor/throttle.c

@@ -15,13 +15,13 @@ static bool _auto_detect_sv = true;
 static int  _auto_detect_sv_cnt = 0;
 static float _auto_detect_sv_totle = 0;
 static int _detect_release_cnt = 0;
-#define CONFIG_SAFE_INV_V   0.06f
+#define CONFIG_SAFE_INV_V   0.1f
 static throttle_torque_t _throttle;
 
 void throttle_torque_reset(void) {
 	_throttle.accl = false;
-	_throttle.throttle_opening = _throttle.throttle_opening_last = 0.0f;
-	_throttle.torque_req = _throttle.torque_real = _throttle.torque_acc_ = 0.0f;
+	_throttle.pedal_opening = _throttle.prev_pedal_opening = 0.0f;
+	_throttle.torque_req = _throttle.torque_real = _throttle.torque_compensation = 0.0f;
 	_throttle.gear = mc_get_internal_gear();
 }
 
@@ -93,7 +93,9 @@ float throttle_get_signal(void) {
 bool throttle_is_released(void) {
 #if CONFIG_DAUL_THROTTLE==1
 	float signal = 0;
-	if (throttle1_is_error() && !throttle2_is_error()) {
+	if (throttle1_is_error() && throttle2_is_error()) {
+		return true;
+	}else if (throttle1_is_error() && !throttle2_is_error()) {
 		float thr = get_thro2_5v_float() - get_throttle2_float();
 		signal = fclamp(thr, _start_v, _end_v);
 	}else if (!throttle1_is_error() && throttle2_is_error()) {
@@ -208,71 +210,71 @@ static float _throttle_torque_for_accelerate(float ration) {
 	float max_torque = mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear);
 	float thro_torque = max_torque * ration;
 
-	float acc_r = 1.0f;
-	if (_throttle.throttle_opening_last < 1.0f) {
-		acc_r = (ration - _throttle.throttle_opening_last)/ (1.0f - _throttle.throttle_opening_last);
+	float pedal_acc = 1.0f;
+	if (_throttle.prev_pedal_opening < 1.0f) {
+		pedal_acc = (ration - _throttle.prev_pedal_opening)/ (1.0f - _throttle.prev_pedal_opening);
 	}
-	acc_r = fclamp(acc_r, 0, 1.0f);
-	float acc_torque = _throttle.torque_real + acc_r * (max_torque - _throttle.torque_real);
+	pedal_acc = fclamp(pedal_acc, 0, 1.0f);
+	float acc_torque = _throttle.torque_real + pedal_acc * (max_torque - _throttle.torque_real);
 	if (acc_torque < 0) {
 		acc_torque = 0;
 	}
 	/*
 	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
 	*/
-	float torque_acc_ = thro_torque - acc_torque;
+	float torque_compensation = thro_torque - acc_torque;
 	float step = 0.0f;
-	if (torque_acc_ > 0) {
+	if (torque_compensation > 0) {
 		float acc_t = mc_gear_conf()->accl_time;
-		step = torque_acc_ / (acc_t + 0.00001f);
+		step = torque_compensation / (acc_t + 0.00001f);
 	}else {
-		torque_acc_ = 0;
+		torque_compensation = 0;
 	}
-	step_towards(&_throttle.torque_acc_, torque_acc_, step);
-	return (acc_torque + _throttle.torque_acc_);
+	step_towards(&_throttle.torque_compensation, torque_compensation, step);
+	return (acc_torque + _throttle.torque_compensation);
 }
 
 
 static float throttle_torque_for_accelerate(void) {
-	return _throttle_torque_for_accelerate(_throttle.throttle_opening);
+	return _throttle_torque_for_accelerate(_throttle.pedal_opening);
 }
 
 static float throttle_torque_for_decelerate(void) {
-	if (_throttle.throttle_opening_last == 0.0f) {
+	if (_throttle.prev_pedal_opening == 0.0f) {
 		return 0;
 	}
-	float dec_r = _throttle.throttle_opening / _throttle.throttle_opening_last;
-	dec_r = fclamp(dec_r, 0.0f, 1.0f);
-	return dec_r * _throttle.torque_real;
+	float pedal_dec = _throttle.pedal_opening / _throttle.prev_pedal_opening;
+	pedal_dec = fclamp(pedal_dec, 0.0f, 1.0f);
+	return pedal_dec * _throttle.torque_real;
 }
 
 float throttle_get_open_ration_filted(void) {
-	return _throttle.throttle_opening;
+	return _throttle.pedal_opening;
 }
 
 #define THRO_RPM_LP_CEOF 0.01f
 float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
-	float throttle_opening = throttle_vol_to_opening(vol);
+	float pedal_opening = throttle_vol_to_opening(vol);
 	float vel = mot_contrl_get_speed(ctrl);
 	_throttle.gear = mc_get_internal_gear();
 	LowPass_Filter(_throttle.vel_filted, vel, THRO_RPM_LP_CEOF);
 
-	if (throttle_opening > _throttle.throttle_opening) {
+	if (pedal_opening > _throttle.pedal_opening) {
 		if (!_throttle.accl) {
-			_throttle.throttle_opening_last = _throttle.throttle_opening;
+			_throttle.prev_pedal_opening = _throttle.pedal_opening;
 			_throttle.torque_real = _throttle.torque_req;
 			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
 				_throttle.torque_real = 0;
 			}
-			_throttle.torque_acc_ = 0;
+			_throttle.torque_compensation = 0;
 		}
 		_throttle.accl = true;
-	}else if (throttle_opening < _throttle.throttle_opening) {
+	}else if (pedal_opening < _throttle.pedal_opening) {
 		if (_throttle.accl) {
-			_throttle.throttle_opening_last = _throttle.throttle_opening;
+			_throttle.prev_pedal_opening = _throttle.pedal_opening;
 			_throttle.torque_real = ctrl->target_torque_raw;
 			/* 如果扭矩给定的ramp没有结束，使用原始扭矩请求作为减扭矩的起始点 */
-			if (_throttle.torque_req - line_ramp_get_interp(&ctrl->input_torque) >= 10.0f ) {
+			if (_throttle.torque_req - line_ramp_get_interp(&ctrl->ramp_input_torque) >= 10.0f ) {
 				_throttle.torque_real = _throttle.torque_req;
 			}
 			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
@@ -281,7 +283,7 @@ float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
 		}
 		_throttle.accl = false;
 	}
-	_throttle.throttle_opening = throttle_opening;
+	_throttle.pedal_opening = pedal_opening;
 	if (_throttle.accl) {
 		return throttle_torque_for_accelerate();
 	}else {
@@ -301,7 +303,7 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
 		if (curr_vel <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
 			if (_throttle.torque_req < hold_torque) {
 				_throttle.torque_req = hold_torque;
-				line_ramp_reset(&ctrl->input_torque, hold_torque);
+				line_ramp_reset(&ctrl->ramp_input_torque, hold_torque);
 			}
 		}else {
 			ctrl->autohold_torque = 0;
@@ -321,7 +323,7 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
 			}else {
 				float f_now = (float)now_ramp_time;
 				float f_next = (float)next_ramp_time;
-				step_towards(&f_now, f_next, 0.5f);
+				step_towards(&f_now, f_next, 5.0f);
 				mot_contrl_set_torque_acc_time(ctrl ,(u16)f_now);
 			}
 		}
@@ -348,6 +350,6 @@ float get_user_request_torque(void) {
 
 
 void throttle_log(void) {
-	sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.throttle_opening, _throttle.throttle_opening_last, _throttle.torque_real, _throttle.torque_req);
+	sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.pedal_opening, _throttle.prev_pedal_opening, _throttle.torque_real, _throttle.torque_req);
 	sys_debug("thro: %f-%f, %f\n", throttle_start_vol(), throttle_end_vol(), mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear));
 }

Applications/foc/motor/throttle.h

@@ -10,10 +10,10 @@ typedef struct {
 	bool  accl;
 	float torque_req;
 	float torque_real;
-	float torque_acc_;
+	float torque_compensation;
 	float vel_filted;
-	float throttle_opening;
-	float throttle_opening_last;
+	float pedal_opening;
+	float prev_pedal_opening;
 	u8    gear;
 }throttle_torque_t;
 

Applications/math/fast_math.c

@@ -4,6 +4,41 @@
 #include <arm_math.h>
 #include "math/fast_math.h"
 #include "os/os_types.h"
+
+void avg_filter_init(avg_filter_t *f, int len) {
+	f->len = len;
+	f->idx = f->sum = 0;
+	f->buff = os_alloc(len * sizeof(float));
+	if (f->buff == NULL) {
+		return;
+	}
+	for (int i = 0; i < len; i++) {
+		f->buff[i] = 0;
+	}
+}
+
+float avg_filter_do(avg_filter_t *f, float sample) {
+	if (f->buff == NULL) {
+		return 0;
+	}
+	f->sum -= f->buff[f->idx];
+	f->buff[f->idx] = sample;
+	f->sum += f->buff[f->idx];
+	f->idx = (f->idx + 1) % f->len;
+	return f->sum/f->len;
+}
+
+
+float throttle_curve(float K, float x) {
+	float y1 = 1.0f - (expf(K*( 1- x)) - 1)/(expf(K) - 1);
+	if (y1 >= 0.999f) {
+		y1 = 1.0f;
+	}
+	y1 = fclamp(y1, 0, 1.0f);
+	int yi = y1 * 100.0f;
+	return (float)yi/100.0f;
+}
+
 #if 0
 /*
 生成sin/cos 查找表，(0-90°，间隔0.1°)

Applications/math/fast_math.h

@@ -19,6 +19,8 @@
 #ifndef SQ
 #define SQ(x) ((x)*(x))
 #endif
+#define NORM2_f(x,y)		(sqrtf(SQ(x) + SQ(y)))
+#define sqrtsub2_f(x,y)       (sqrtf(SQ(x) - SQ(y)))
 // nan and infinity check for floats
 #define UTILS_IS_INF(x)		((x) == (1.0F / 0.0F) || (x) == (-1.0F / 0.0F))
 #define UTILS_IS_NAN(x)		((x) != (x))
@@ -96,11 +98,15 @@ static __INLINE void step_towards_s16(s16 *value, s16 goal, s16 step) {
     }
 }
 
+static __INLINE s16 line_intp(s16 x, s16 x_l, s16 x_h, s16 y_l, s16 y_h) {
+	float r = (float)(x - x_l) /(float)(x_h - x_l);
+	return (s16)(r * (y_h - y_l)) + y_l;
+}
 
 /**
  * Fast atan2
  *
- * See http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
+ * See based on https://math.stackexchange.com/a/1105038/81278
  *
  * @param y
  * y
@@ -112,29 +118,6 @@ static __INLINE void step_towards_s16(s16 *value, s16 goal, s16 step) {
  * The angle in radians
  */
 static __INLINE float fast_atan2(float y, float x) {
-	float abs_y = fabsf(y) + 1e-20f; // kludge to prevent 0/0 condition
-	float angle;
-
-	if (x >= 0) {
-		float r = (x - abs_y) / (x + abs_y);
-		float rsq = r * r;
-		angle = ((0.1963f * rsq) - 0.9817f) * r + (M_PI / 4.0f);
-	} else {
-		float r = (x + abs_y) / (abs_y - x);
-		float rsq = r * r;
-		angle = ((0.1963f * rsq) - 0.9817f) * r + (3.0f * M_PI / 4.0f);
-	}
-
-	UTILS_NAN_ZERO(angle);
-
-	if (y < 0) {
-		return(-angle);
-	} else {
-		return(angle);
-	}
-}
-
-static __INLINE float fast_atan_2(float y, float x) {
     // a := min (|x|, |y|) / max (|x|, |y|)
     float abs_y = ABS(y);
     float abs_x = ABS(x);
@@ -163,6 +146,8 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 }
 #define degree_2_pi(d) ((float)(d) * M_PI / 180.0f)
 #define pi_2_degree(d) ((float)(d) * 180.0f / M_PI)
+#define norm_angle_rad(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
+#define norm_angle_deg(a) {while (a >= 360.0f) a-=360.0f;while (a < 0) a +=360.0f;};
 
 #define INVALID_ANGLE 0x3DFF
 
@@ -186,5 +171,19 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 /* 后向差分离散化 */
 #define do_lpf(value, sample, filter_constant)	((sample * filter_constant + value)/(1.0f + filter_constant))
 
+/**
+ * A simple avarage filter
+*/
+typedef struct {
+	int len;
+	float *buff;
+	int idx;
+	int sum;
+}avg_filter_t;
+void avg_filter_init(avg_filter_t *f, int len);
+float avg_filter_do(avg_filter_t *f, float sample);
+
+float throttle_curve(float K, float x);
+
 #endif /* _Fast_Math_H__ */
 

Applications/os/os_types.h

@@ -48,7 +48,6 @@ typedef unsigned int	uint32;
 #define PHASE_300_DEGREE (300.0f)
 #define PHASE_360_DEGREE (360.0f)
 
-#define rand_angle(a) {while (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;while (a < 0) a +=PHASE_360_DEGREE;};
 #define ABS(x) 					( (x)>0?(x):-(x) )
 
 #define MAX_S16 (0x7FFF)

Applications/prot/can_foc_msg.c

@@ -23,7 +23,7 @@ void can_report_power(u8 can) {
 	s16 v = S16Q5(vDC);
 	s16 i = S16Q5(iDC);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Power));
-	encode_s16(data + 2, ABS(rpm));
+	encode_s16(data + 2, rpm);
 	encode_s16(data + 4, v);
 	encode_s16(data + 6, i);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
@@ -79,18 +79,6 @@ void can_response_hall_offset(u8 can, int offset) {
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
-void can_report_pid_value(u8 can, u8 id) {
-	float kp, ki, kd;
-	mot_contrl_get_pid(&motor.controller, id, &kp, &ki, &kd);
-	u8 data[15];
-	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Pid));
-	data[2] = id;
-	encode_float(data + 3, kp);
-	encode_float(data + 7, ki);
-	encode_float(data + 11, kd);
-	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
-}
-
 void can_report_foc_status(u8 can) {
 	u8 data[16];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Status));
@@ -158,26 +146,20 @@ void can_report_mot_params_ested(float v, u8 type) {
 
 void can_response_vols(u8 can, u8 key) {
 	u8 data[16];
-	int len;
+	int len = 0;
 	encoder_can_key(data, CMD_2_CAN_KEY(key));
-	len = 2;
-	float thro = get_throttle_float() * 10.0f;
-	encode_s16(data + len, (s16)thro);
 	len += 2;
-	float acc = get_acc_vol() * 10.0f;
-	encode_s16(data + len, (s16)acc);
+	encode_s16(data + len, S16Q5(get_acc_vol()));
+	len += 2;
+	encode_s16(data + len, S16Q5(get_vbus_float()));
 	len += 2;
-	float vbus = get_vbus_float() * 10.0f;
-	encode_s16(data + len, (s16)vbus);
+	encode_s16(data + len, S16Q5(get_throttle_float()));
 	len += 2;
-	float thro_5v = get_thro_5v_float() * 10.0f;
-	encode_s16(data + len, (s16)thro_5v);
+	encode_s16(data + len, S16Q5(get_thro_5v_float()));
 	len += 2;
-	thro = get_throttle2_float() * 10.0f;
-	encode_s16(data + len, (s16)thro);
+	encode_s16(data + len, S16Q5(get_throttle2_float()));
 	len += 2;
-	thro_5v = get_thro2_5v_float() * 10.0f;
-	encode_s16(data + len, (s16)thro_5v);
+	encode_s16(data + len, S16Q5(get_thro2_5v_float()));
 	len += 2;
 	can_send_response(can, data, len);
 }

Applications/prot/can_foc_msg.h

@@ -8,7 +8,6 @@ void can_report_phase_voltage(u8 can);
 void can_report_dq_current(u8 can);
 void can_response_hall_offset(u8 can, int offset);
 void can_report_power(u8 can);
-void can_report_pid_value(u8 can, u8 id);
 void can_report_foc_status(u8 can);
 void can_report_mpta_values(u8 can);
 void can_report_ext_status(u8 can);

Project/MC124.uvoptx

@@ -77,7 +77,7 @@
         <tvExpOptDlg>0</tvExpOptDlg>
         <IsCurrentTarget>1</IsCurrentTarget>
       </OPTFL>
-      <CpuCode>0</CpuCode>
+      <CpuCode>255</CpuCode>
       <DebugOpt>
         <uSim>0</uSim>
         <uTrg>1</uTrg>

configs/autogen_config_A1.h

@@ -0,0 +1,218 @@
+/* auto gen 2023/10/25 18:57:17*/
+#ifndef _AUTOGEN_CONFIG_H__
+#define _AUTOGEN_CONFIG_H__
+
+#define CONFIG_Version 1
+#define CONFIG_Motor_Poles  4
+#define CONFIG_Motor_Ld  5E-05
+#define CONFIG_Motor_Lq  0.00012f
+#define CONFIG_Motor_Rs  0.009f
+#define CONFIG_Motor_Flux  0.0158f
+#define CONFIG_Motor_PLLBand  200
+#define CONFIG_Motor_EpmPLL  400
+#define CONFIG_Motor_PosPLL  500
+#define CONFIG_Motor_VehicleW  190
+#define CONFIG_Motor_WheelC  145
+#define CONFIG_Motor_GearRatio  6.25f
+#define CONFIG_Motor_MaxFwDCurr  100
+#define CONFIG_Motor_MaxTorque  300
+#define CONFIG_Motor_EncOffset  0
+#define CONFIG_Foc_MaxDCVol  120
+#define CONFIG_Foc_MinDCVol  70
+#define CONFIG_Foc_MaxPhaseCurr  300
+#define CONFIG_Foc_MaxRPM  9000
+#define CONFIG_Foc_MaxEPMRPM  300
+#define CONFIG_Foc_MaxEPMTorque  100
+#define CONFIG_Foc_MaxEPMRPMBk  170
+#define CONFIG_Foc_MaxEPMTorqueBk  80
+#define CONFIG_Foc_MaxTorque  300
+#define CONFIG_Foc_MaxEbrkTorque  40
+#define CONFIG_Foc_MaxIDC  200
+#define CONFIG_Foc_MaxAutoHoldTorque  100
+#define CONFIG_Foc_ThroStartVol  0.85f
+#define CONFIG_Foc_ThroEndVol  4.15f
+#define CONFIG_Foc_ThroMinVol  0.4f
+#define CONFIG_Foc_ThroMaxVol  4.6f
+#define CONFIG_Foc_CurrCtrlBandWith  800
+#define CONFIG_Foc_ThroDecTime  10
+#define CONFIG_Foc_PID_VelLim_Kp 0.5f
+#define CONFIG_Foc_PID_VelLim_Ki 2.5f
+#define CONFIG_Foc_PID_VelLim_Kd 0
+#define CONFIG_Foc_PID_VelCtrl_Kp 0.1f
+#define CONFIG_Foc_PID_VelCtrl_Ki 3.5f
+#define CONFIG_Foc_PID_VelCtrl_Kd 0
+#define CONFIG_Foc_PID_Autohold_Kp 0.01f
+#define CONFIG_Foc_PID_Autohold_Ki 0.2f
+#define CONFIG_Foc_PID_Autohold_Kd 0
+#define CONFIG_Foc_PID_IDCLim_Kp 5
+#define CONFIG_Foc_PID_IDCLim_Ki 15
+#define CONFIG_Foc_PID_IDCLim_Kd 0
+#define CONFIG_Settings_AutoHold  1
+#define CONFIG_Settings_BrkShutPower  1
+#define CONFIG_Settings_TcsEnable  0
+#define CONFIG_Gear0_MaxSpeed 3000
+#define CONFIG_Gear0_MaxTorque 200
+#define CONFIG_Gear0_MaxIdc 100
+#define CONFIG_Gear0_ZeroAccl 500
+#define CONFIG_Gear0_NormalAccl 100
+#define CONFIG_Gear0_Torque0 100
+#define CONFIG_Gear0_Torque1 100
+#define CONFIG_Gear0_Torque2 80
+#define CONFIG_Gear0_Torque3 0
+#define CONFIG_Gear0_Torque4 0
+#define CONFIG_Gear0_Torque5 0
+#define CONFIG_Gear0_Torque6 0
+#define CONFIG_Gear0_Torque7 0
+#define CONFIG_Gear0_Torque8 0
+#define CONFIG_Gear0_Torque9 0
+#define CONFIG_Gear1_MaxSpeed 4000
+#define CONFIG_Gear1_MaxTorque 200
+#define CONFIG_Gear1_MaxIdc 100
+#define CONFIG_Gear1_ZeroAccl 500
+#define CONFIG_Gear1_NormalAccl 100
+#define CONFIG_Gear1_Torque0 100
+#define CONFIG_Gear1_Torque1 100
+#define CONFIG_Gear1_Torque2 80
+#define CONFIG_Gear1_Torque3 70
+#define CONFIG_Gear1_Torque4 0
+#define CONFIG_Gear1_Torque5 0
+#define CONFIG_Gear1_Torque6 0
+#define CONFIG_Gear1_Torque7 0
+#define CONFIG_Gear1_Torque8 0
+#define CONFIG_Gear1_Torque9 0
+#define CONFIG_Gear2_MaxSpeed 8300
+#define CONFIG_Gear2_MaxTorque 300
+#define CONFIG_Gear2_MaxIdc 200
+#define CONFIG_Gear2_ZeroAccl 500
+#define CONFIG_Gear2_NormalAccl 100
+#define CONFIG_Gear2_Torque0 100
+#define CONFIG_Gear2_Torque1 100
+#define CONFIG_Gear2_Torque2 100
+#define CONFIG_Gear2_Torque3 100
+#define CONFIG_Gear2_Torque4 100
+#define CONFIG_Gear2_Torque5 90
+#define CONFIG_Gear2_Torque6 80
+#define CONFIG_Gear2_Torque7 80
+#define CONFIG_Gear2_Torque8 80
+#define CONFIG_Gear2_Torque9 0
+#define CONFIG_Gear3_MaxSpeed 1000
+#define CONFIG_Gear3_MaxTorque 100
+#define CONFIG_Gear3_MaxIdc 30
+#define CONFIG_Gear3_ZeroAccl 500
+#define CONFIG_Gear3_NormalAccl 100
+#define CONFIG_Gear3_Torque0 100
+#define CONFIG_Gear3_Torque1 100
+#define CONFIG_Gear3_Torque2 100
+#define CONFIG_Gear3_Torque3 100
+#define CONFIG_Gear3_Torque4 100
+#define CONFIG_Gear3_Torque5 100
+#define CONFIG_Gear3_Torque6 100
+#define CONFIG_Gear3_Torque7 100
+#define CONFIG_Gear3_Torque8 0
+#define CONFIG_Gear3_Torque9 0
+#define CONFIG_GearLow0_MaxSpeed 1000
+#define CONFIG_GearLow0_MaxTorque 100
+#define CONFIG_GearLow0_MaxIdc 30
+#define CONFIG_GearLow0_ZeroAccl 500
+#define CONFIG_GearLow0_NormalAccl 100
+#define CONFIG_GearLow0_Torque0 100
+#define CONFIG_GearLow0_Torque1 100
+#define CONFIG_GearLow0_Torque2 100
+#define CONFIG_GearLow0_Torque3 100
+#define CONFIG_GearLow0_Torque4 100
+#define CONFIG_GearLow0_Torque5 100
+#define CONFIG_GearLow0_Torque6 100
+#define CONFIG_GearLow0_Torque7 100
+#define CONFIG_GearLow0_Torque8 0
+#define CONFIG_GearLow0_Torque9 0
+#define CONFIG_GearLow1_MaxSpeed 1000
+#define CONFIG_GearLow1_MaxTorque 100
+#define CONFIG_GearLow1_MaxIdc 30
+#define CONFIG_GearLow1_ZeroAccl 500
+#define CONFIG_GearLow1_NormalAccl 100
+#define CONFIG_GearLow1_Torque0 100
+#define CONFIG_GearLow1_Torque1 100
+#define CONFIG_GearLow1_Torque2 100
+#define CONFIG_GearLow1_Torque3 100
+#define CONFIG_GearLow1_Torque4 100
+#define CONFIG_GearLow1_Torque5 100
+#define CONFIG_GearLow1_Torque6 100
+#define CONFIG_GearLow1_Torque7 100
+#define CONFIG_GearLow1_Torque8 0
+#define CONFIG_GearLow1_Torque9 0
+#define CONFIG_GearLow2_MaxSpeed 1000
+#define CONFIG_GearLow2_MaxTorque 100
+#define CONFIG_GearLow2_MaxIdc 30
+#define CONFIG_GearLow2_ZeroAccl 500
+#define CONFIG_GearLow2_NormalAccl 100
+#define CONFIG_GearLow2_Torque0 100
+#define CONFIG_GearLow2_Torque1 100
+#define CONFIG_GearLow2_Torque2 100
+#define CONFIG_GearLow2_Torque3 100
+#define CONFIG_GearLow2_Torque4 100
+#define CONFIG_GearLow2_Torque5 100
+#define CONFIG_GearLow2_Torque6 100
+#define CONFIG_GearLow2_Torque7 100
+#define CONFIG_GearLow2_Torque8 0
+#define CONFIG_GearLow2_Torque9 0
+#define CONFIG_GearLow3_MaxSpeed 1000
+#define CONFIG_GearLow3_MaxTorque 100
+#define CONFIG_GearLow3_MaxIdc 30
+#define CONFIG_GearLow3_ZeroAccl 500
+#define CONFIG_GearLow3_NormalAccl 100
+#define CONFIG_GearLow3_Torque0 100
+#define CONFIG_GearLow3_Torque1 100
+#define CONFIG_GearLow3_Torque2 100
+#define CONFIG_GearLow3_Torque3 100
+#define CONFIG_GearLow3_Torque4 100
+#define CONFIG_GearLow3_Torque5 100
+#define CONFIG_GearLow3_Torque6 100
+#define CONFIG_GearLow3_Torque7 100
+#define CONFIG_GearLow3_Torque8 0
+#define CONFIG_GearLow3_Torque9 0
+#define CONFIG_Protect_Motor_Level0_Entry 130
+#define CONFIG_Protect_Motor_Level0_Exit 120
+#define CONFIG_Protect_Motor_Level0_Value 0
+#define CONFIG_Protect_Motor_Level1_Entry 120
+#define CONFIG_Protect_Motor_Level1_Exit 110
+#define CONFIG_Protect_Motor_Level1_Value 34
+#define CONFIG_Protect_Motor_Level2_Entry 110
+#define CONFIG_Protect_Motor_Level2_Exit 100
+#define CONFIG_Protect_Motor_Level2_Value 66
+#define CONFIG_Protect_MosFet_Level0_Entry 100
+#define CONFIG_Protect_MosFet_Level0_Exit 95
+#define CONFIG_Protect_MosFet_Level0_Value 0
+#define CONFIG_Protect_MosFet_Level1_Entry 95
+#define CONFIG_Protect_MosFet_Level1_Exit 90
+#define CONFIG_Protect_MosFet_Level1_Value 34
+#define CONFIG_Protect_MosFet_Level2_Entry 90
+#define CONFIG_Protect_MosFet_Level2_Exit 80
+#define CONFIG_Protect_MosFet_Level2_Value 66
+#define CONFIG_Protect_Voltage_Level0_Entry 60
+#define CONFIG_Protect_Voltage_Level0_Exit 70
+#define CONFIG_Protect_Voltage_Level0_Value 0
+#define CONFIG_EnergyRecovery_Level0_Torque 0
+#define CONFIG_EnergyRecovery_Level0_Time 1000
+#define CONFIG_EnergyRecovery_Level1_Torque 10
+#define CONFIG_EnergyRecovery_Level1_Time 500
+#define CONFIG_EnergyRecovery_Level2_Torque 15
+#define CONFIG_EnergyRecovery_Level2_Time 400
+#define CONFIG_EnergyRecovery_Level3_Torque 20
+#define CONFIG_EnergyRecovery_Level3_Time 300
+#define CONFIG_EnergyRecovery_Level4_Torque 35
+#define CONFIG_EnergyRecovery_Level4_Time 200
+#define CONFIG_EnergyRecovery_Level5_Torque 40
+#define CONFIG_EnergyRecovery_Level5_Time 200
+#define CONFIG_EnergyRecovery_Level6_Torque 40
+#define CONFIG_EnergyRecovery_Level6_Time 200
+#define CONFIG_EnergyRecovery_Level7_Torque 40
+#define CONFIG_EnergyRecovery_Level7_Time 200
+#define CONFIG_EnergyRecovery_Level8_Torque 40
+#define CONFIG_EnergyRecovery_Level8_Time 200
+#define CONFIG_EnergyRecovery_Level9_Torque 40
+#define CONFIG_EnergyRecovery_Level9_Time 200
+#define CONFIG_CrossZero_Low  1
+#define CONFIG_CrossZero_High  7
+#define CONFIG_CrossZero_MinStep  0.03f
+#define CONFIG_CrossZero_NorStep  5
+#endif /* _AUTOGEN_CONFIG_H__ */ 

configs/mc105_A1.yml

@@ -0,0 +1,243 @@
+##### 配置文件自动生成，不要手动修改!! 2023/10/25 18:58:37
+Version: 1
+CheckCrc: 0
+Motor:
+  Poles: 4
+  Ld: 5E-05
+  Lq: 0.00012
+  Rs: 0.009
+  Flux: 0.0158
+  PLLBand: 200
+  EpmPLL: 400
+  PosPLL: 500
+  VehicleW: 190
+  WheelC: 145
+  GearRatio: 6.25
+  MaxFwDCurr: 100
+  MaxTorque: 300
+  EncOffset: 0
+Foc:
+  MaxDCVol: 120
+  MinDCVol: 70
+  MaxPhaseCurr: 300
+  MaxRPM: 9000
+  MaxEPMRPM: 300
+  MaxEPMTorque: 100
+  MaxEPMRPMBk: 170
+  MaxEPMTorqueBk: 80
+  MaxTorque: 300
+  MaxEbrkTorque: 40
+  MaxIDC: 200
+  MaxAutoHoldTorque: 100
+  ThroStartVol: 0.85
+  ThroEndVol: 4.15
+  ThroMinVol: 0.4
+  ThroMaxVol: 4.6
+  CurrCtrlBandWith: 800
+  ThroDecTime: 10
+  PID:
+    VelLim:
+      Kp: 0.5
+      Ki: 2.5
+      Kd: 0
+    VelCtrl:
+      Kp: 0.1
+      Ki: 3.5
+      Kd: 0
+    Autohold:
+      Kp: 0.01
+      Ki: 0.2
+      Kd: 0
+    IDCLim:
+      Kp: 5
+      Ki: 15
+      Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
+Settings:
+  AutoHold: 1
+  BrkShutPower: 1
+  TcsEnable: 0
+Gear:
+- MaxSpeed: 3000
+  MaxTorque: 200
+  MaxIdc: 100
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 80
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 4000
+  MaxTorque: 200
+  MaxIdc: 100
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 80
+  - 70
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 8300
+  MaxTorque: 300
+  MaxIdc: 200
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 90
+  - 80
+  - 80
+  - 80
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+GearLow:
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+Protect:
+  Motor:
+  - Entry: 130
+    Exit: 120
+    Value: 0
+  - Entry: 120
+    Exit: 110
+    Value: 34
+  - Entry: 110
+    Exit: 100
+    Value: 66
+  MosFet:
+  - Entry: 100
+    Exit: 95
+    Value: 0
+  - Entry: 95
+    Exit: 90
+    Value: 34
+  - Entry: 90
+    Exit: 80
+    Value: 66
+  Voltage:
+  - Entry: 60
+    Exit: 70
+    Value: 0
+EnergyRecovery:
+- Torque: 0
+  Time: 1000
+- Torque: 10
+  Time: 500
+- Torque: 15
+  Time: 400
+- Torque: 20
+  Time: 300
+- Torque: 35
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+CrossZero:
+  Low: 1
+  High: 7
+  MinStep: 0.03
+  NorStep: 5

configs/mc124_R1.yml

@@ -0,0 +1,243 @@
+##### 配置文件自动生成，不要手动修改!! 2023/11/15 16:04:00
+Version: 1
+CheckCrc: 0
+Motor:
+  Poles: 5
+  Ld: 4.8E-05
+  Lq: 6.5E-05
+  Rs: 0.011
+  Flux: 0.018
+  PLLBand: 200
+  EpmPLL: 400
+  PosPLL: 500
+  VehicleW: 190
+  WheelC: 145
+  GearRatio: 6.25
+  MaxFwDCurr: 100
+  MaxTorque: 350
+  EncOffset: 0
+Foc:
+  MaxDCVol: 90
+  MinDCVol: 40
+  MaxPhaseCurr: 350
+  MaxRPM: 9000
+  MaxEPMRPM: 300
+  MaxEPMTorque: 100
+  MaxEPMRPMBk: 170
+  MaxEPMTorqueBk: 80
+  MaxTorque: 350
+  MaxEbrkTorque: 40
+  MaxIDC: 100
+  MaxAutoHoldTorque: 100
+  ThroStartVol: 0.9
+  ThroEndVol: 4
+  ThroMinVol: 0.4
+  ThroMaxVol: 4.6
+  CurrCtrlBandWith: 800
+  ThroDecTime: 50
+  PID:
+    VelLim:
+      Kp: 0.1
+      Ki: 3.5
+      Kd: 0
+    VelCtrl:
+      Kp: 0.1
+      Ki: 3.5
+      Kd: 0
+    Autohold:
+      Kp: 0.01
+      Ki: 0.2
+      Kd: 0
+    IDCLim:
+      Kp: 5
+      Ki: 15
+      Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
+Settings:
+  AutoHold: 1
+  BrkShutPower: 1
+  TcsEnable: 0
+Gear:
+- MaxSpeed: 3500
+  MaxTorque: 250
+  MaxIdc: 45
+  ZeroAccl: 300
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 80
+  - 60
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 4500
+  MaxTorque: 250
+  MaxIdc: 50
+  ZeroAccl: 300
+  NormalAccl: 80
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 80
+  - 75
+  - 60
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 5500
+  MaxTorque: 300
+  MaxIdc: 65
+  ZeroAccl: 500
+  NormalAccl: 50
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 90
+  - 80
+  - 70
+  - 50
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 6500
+  MaxTorque: 300
+  MaxIdc: 100
+  ZeroAccl: 500
+  NormalAccl: 30
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 90
+  - 80
+  - 70
+  - 70
+  - 50
+  - 0
+  - 0
+GearLow:
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+Protect:
+  Motor:
+  - Entry: 130
+    Exit: 120
+    Value: 0
+  - Entry: 120
+    Exit: 110
+    Value: 34
+  - Entry: 110
+    Exit: 100
+    Value: 66
+  MosFet:
+  - Entry: 100
+    Exit: 95
+    Value: 0
+  - Entry: 95
+    Exit: 90
+    Value: 34
+  - Entry: 90
+    Exit: 85
+    Value: 66
+  Voltage:
+  - Entry: 40
+    Exit: 42
+    Value: 0
+EnergyRecovery:
+- Torque: 0
+  Time: 600
+- Torque: 10
+  Time: 500
+- Torque: 15
+  Time: 400
+- Torque: 20
+  Time: 300
+- Torque: 35
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+CrossZero:
+  Low: 2
+  High: 7
+  MinStep: 0.05
+  NorStep: 5

configs/mc124_R1_V2.yml

@@ -0,0 +1,243 @@
+##### 配置文件自动生成，不要手动修改!! 2024/1/18 14:54:54
+Version: 1
+CheckCrc: 0
+Motor:
+  Poles: 5
+  Ld: 5.4E-05
+  Lq: 7E-05
+  Rs: 0.015
+  Flux: 0.018
+  PLLBand: 200
+  EpmPLL: 400
+  PosPLL: 500
+  VehicleW: 190
+  WheelC: 145
+  GearRatio: 6.25
+  MaxFwDCurr: 100
+  MaxTorque: 350
+  EncOffset: -2
+Foc:
+  MaxDCVol: 90
+  MinDCVol: 40
+  MaxPhaseCurr: 350
+  MaxRPM: 9000
+  MaxEPMRPM: 300
+  MaxEPMTorque: 100
+  MaxEPMRPMBk: 170
+  MaxEPMTorqueBk: 80
+  MaxTorque: 350
+  MaxEbrkTorque: 40
+  MaxIDC: 200
+  MaxAutoHoldTorque: 100
+  ThroStartVol: 0.9
+  ThroEndVol: 4
+  ThroMinVol: 0.4
+  ThroMaxVol: 4.6
+  CurrCtrlBandWith: 400
+  ThroDecTime: 50
+  PID:
+    VelLim:
+      Kp: 0.15
+      Ki: 2.5
+      Kd: 0
+    VelCtrl:
+      Kp: 0.2
+      Ki: 0.02
+      Kd: 0
+    Autohold:
+      Kp: 0.01
+      Ki: 0.2
+      Kd: 0
+    IDCLim:
+      Kp: 0
+      Ki: 5
+      Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
+Settings:
+  AutoHold: 1
+  BrkShutPower: 1
+  TcsEnable: 0
+Gear:
+- MaxSpeed: 3500
+  MaxTorque: 250
+  MaxIdc: 30
+  ZeroAccl: 300
+  NormalAccl: 1000
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 80
+  - 20
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 4500
+  MaxTorque: 250
+  MaxIdc: 45
+  ZeroAccl: 300
+  NormalAccl: 500
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 80
+  - 60
+  - 50
+  - 50
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 5500
+  MaxTorque: 300
+  MaxIdc: 60
+  ZeroAccl: 500
+  NormalAccl: 250
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 90
+  - 80
+  - 70
+  - 50
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 7000
+  MaxTorque: 300
+  MaxIdc: 150
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 90
+  - 80
+  - 70
+  - 70
+  - 50
+  - 50
+  - 0
+GearLow:
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+- MaxSpeed: 1000
+  MaxTorque: 100
+  MaxIdc: 30
+  ZeroAccl: 500
+  NormalAccl: 100
+  Torque:
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 100
+  - 0
+  - 0
+Protect:
+  Motor:
+  - Entry: 130
+    Exit: 120
+    Value: 0
+  - Entry: 120
+    Exit: 110
+    Value: 34
+  - Entry: 100
+    Exit: 90
+    Value: 66
+  MosFet:
+  - Entry: 100
+    Exit: 95
+    Value: 0
+  - Entry: 95
+    Exit: 90
+    Value: 34
+  - Entry: 90
+    Exit: 85
+    Value: 66
+  Voltage:
+  - Entry: 40
+    Exit: 42
+    Value: 0
+EnergyRecovery:
+- Torque: 0
+  Time: 600
+- Torque: 10
+  Time: 500
+- Torque: 15
+  Time: 400
+- Torque: 20
+  Time: 300
+- Torque: 35
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+- Torque: 40
+  Time: 200
+CrossZero:
+  Low: 2
+  High: 7
+  MinStep: 0.1
+  NorStep: 5

configs/mc_servo.yml

@@ -1,11 +1,11 @@
-##### 配置文件自动生成，不要手动修改!! 2023/10/12 14:29:02
+##### 配置文件自动生成，不要手动修改!! 2023/10/27 17:44:45
 Version: 1
 CheckCrc: 0
 Motor:
   Poles: 4
   Ld: 3.2E-05
   Lq: 7.3E-05
-  Rs: 0.009
+  Rs: 0.015
   Flux: 0.019
   PLLBand: 200
   EpmPLL: 400
@@ -17,8 +17,8 @@ Motor:
   MaxTorque: 300
   EncOffset: -103
 Foc:
-  MaxDCVol: 110
-  MinDCVol: 60
+  MaxDCVol: 120
+  MinDCVol: 50
   MaxPhaseCurr: 300
   MaxRPM: 9000
   MaxEPMRPM: 300
@@ -27,7 +27,7 @@ Foc:
   MaxEPMTorqueBk: 80
   MaxTorque: 300
   MaxEbrkTorque: 40
-  MaxIDC: 100
+  MaxIDC: 200
   MaxAutoHoldTorque: 100
   ThroStartVol: 0.85
   ThroEndVol: 4.15
@@ -52,46 +52,50 @@ Foc:
       Kp: 5
       Ki: 15
       Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
 Settings:
   AutoHold: 1
   BrkShutPower: 1
   TcsEnable: 0
 Gear:
-- MaxSpeed: 1000
-  MaxTorque: 100
-  MaxIdc: 30
+- MaxSpeed: 3000
+  MaxTorque: 200
+  MaxIdc: 100
   ZeroAccl: 500
   NormalAccl: 100
   Torque:
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
+  - 80
   - 0
   - 0
-- MaxSpeed: 1000
-  MaxTorque: 100
-  MaxIdc: 30
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 4000
+  MaxTorque: 200
+  MaxIdc: 100
   ZeroAccl: 500
   NormalAccl: 100
   Torque:
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
+  - 80
+  - 70
   - 0
   - 0
-- MaxSpeed: 1000
-  MaxTorque: 100
-  MaxIdc: 30
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 8300
+  MaxTorque: 300
+  MaxIdc: 200
   ZeroAccl: 500
   NormalAccl: 100
   Torque:
@@ -100,10 +104,10 @@ Gear:
   - 100
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 0
+  - 90
+  - 80
+  - 80
+  - 80
   - 0
 - MaxSpeed: 1000
   MaxTorque: 100
@@ -208,8 +212,8 @@ Protect:
     Exit: 80
     Value: 66
   Voltage:
-  - Entry: 60
-    Exit: 70
+  - Entry: 55
+    Exit: 60
     Value: 0
 EnergyRecovery:
 - Torque: 0

configs/mc_zd100.yml

@@ -52,6 +52,10 @@ Foc:
       Kp: 5.0
       Ki: 15.0
       Kd: 0.0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
 Settings:
   AutoHold: 1
   BrkShutPower: 1