整理代码

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

Application/Bsp/serial.c

@@ -86,7 +86,7 @@ static void uart_rx_poll(uart_t *uart) {
 }
 
 static u32 uart_poll_task(void){
-	//uart_rx_poll(&uart2);
+	uart_rx_poll(&uart2);
 	return 0;
 }
 

Application/FOC/PI_Controller.c

@@ -1,6 +1,6 @@
 #include "pi_controller.h"
 
-float pi_control(PI_ctrl_t *pi, float error){
+float pi_control(pi_controller_t *pi, float error){
 	float output = 0.0f;
 	/* 积分抗饱和处理 */
 	if (pi->output > pi->max_output) {
@@ -23,7 +23,7 @@ float pi_control(PI_ctrl_t *pi, float error){
 	return output;
 }
 
-void pi_clear(PI_ctrl_t *pi) {
+void pi_clear(pi_controller_t *pi) {
 	pi->i_errors = 0.0f;
 	pi->output = 0.0f;
 }

Application/FOC/PI_Controller.h

@@ -7,10 +7,10 @@ typedef struct _pi {
 	float output;
 	float max_output;
 	float min_output;
-}PI_ctrl_t;
+}pi_controller_t;
 
-float pi_control(PI_ctrl_t *pi, float error);
-void pi_clear(PI_ctrl_t *pi);
+float pi_control(pi_controller_t *pi, float error);
+void pi_clear(pi_controller_t *pi);
 
 #endif  /* _PI_CONTROLLER_H__ */
 

Application/FOC/foc_api.c

@@ -12,7 +12,7 @@
 #include "foc/ntc_sensor.h"
 #include "foc/gas_sensor.h"
 
-extern motor_foc_t mFOC;
+extern motor_foc_t g_foc;
 static u32 foc_measure_task(void);
 static void foc_defulat_value(void);
 
@@ -33,29 +33,29 @@ void foc_init(void) {
 }
 
 static void foc_defulat_value(void){
-	mFOC.state = IDLE;
-	mFOC.mosGate = false;
-	mFOC.vbus = MAX_VBUS_VOLTAGE;
-	mFOC.state = IDLE;
-	mFOC.mode = FOC_MODE_OPEN_LOOP;
-	mFOC.alpha_beta.alpha = 0;
-	mFOC.alpha_beta.beta = 0;
-	mFOC.dq_ref.Id = 0;
-	mFOC.dq_ref.Iq = 0;
-	mFOC.foc_fault = foc_success;
-	mFOC.is_ready = false;
-	mFOC.rpm_ref = -1;
-	memset(&mFOC.phase_time, 0, sizeof(mFOC.phase_time));
-	mFOC.sector = 0;
-	mFOC.dq_v.Id = 0;
-	mFOC.dq_v.Iq = 0;
-	phase_current_init(&mFOC.current_samp);
-	ramp_ctrl_init(&mFOC.voltage_ramp);
-	ramp_ctrl_init(&mFOC.current_ramp);
-	ramp_ctrl_init(&mFOC.speed_ramp);
-	pi_clear(&mFOC.PI_id);
-	pi_clear(&mFOC.PI_iq);
-	pi_clear(&mFOC.PI_speed);	
+	g_foc.state = IDLE;
+	g_foc.mosfec_gate = false;
+	g_foc.vbus = MAX_VBUS_VOLTAGE;
+	g_foc.state = IDLE;
+	g_foc.mode = FOC_MODE_OPEN_LOOP;
+	g_foc.alpha_beta.alpha = 0;
+	g_foc.alpha_beta.beta = 0;
+	g_foc.dq_command.Id = 0;
+	g_foc.dq_command.Iq = 0;
+	g_foc.foc_fault = foc_success;
+	g_foc.is_ready = false;
+	g_foc.speed_command = -1;
+	memset(&g_foc.phase_time, 0, sizeof(g_foc.phase_time));
+	g_foc.sector = 0;
+	g_foc.dq_v.Id = 0;
+	g_foc.dq_v.Iq = 0;
+	phase_current_init(&g_foc.current_samp);
+	ramp_ctrl_init(&g_foc.voltage_ramp);
+	ramp_ctrl_init(&g_foc.current_ramp);
+	ramp_ctrl_init(&g_foc.speed_ramp);
+	pi_clear(&g_foc.id_controller);
+	pi_clear(&g_foc.iq_controller);
+	pi_clear(&g_foc.speed_controller);	
 }
 
 float speed_to_voltage(u16 rpm) {
@@ -68,19 +68,19 @@ float speed_to_current(u16 rpm) {
 
 void foc_clear(void) {
 	PWM_Stop();
-	mFOC.mosGate = false;
+	g_foc.mosfec_gate = false;
 	foc_defulat_value();
 	hall_sensor_init();
 }
 
 u32 foc_get_speed(void) {
-	float speed = hall_sensor_avg_speed()/(mFOC.motor_p.poles);
-	//printf("avg speed %f, %d\n", speed, mFOC.motor_p.poles);
+	float speed = hall_sensor_avg_speed()/(g_foc.motor_param.poles);
+	//printf("avg speed %f, %d\n", speed, g_foc.motor_p.poles);
 	return abs(speed);
 }
 
 bool foc_is_ready(void){
-	return mFOC.is_ready;
+	return g_foc.is_ready;
 }
 
 foc_fault_t foc_set_ready(bool ready) {
@@ -89,7 +89,7 @@ foc_fault_t foc_set_ready(bool ready) {
 	}
 	normal_task_enable(false);
 	if (foc_stm_nextstate(ready?START:ANY_STOP) == NoError) {
-		mFOC.is_ready = ready;
+		g_foc.is_ready = ready;
 		normal_task_enable(true);
 		return foc_success;
 	}	
@@ -97,32 +97,39 @@ foc_fault_t foc_set_ready(bool ready) {
 	return foc_not_allowed;
 }
 
+void foc_set_dq_command(float d, float q) {
+	g_foc.dq_command.Vd = d;
+	g_foc.dq_command.Vq = q;
+}
 
 void foc_set_voltage_ramp(float final){
 	printf("voltage %f\n", final);
-	ramp_set_target(&mFOC.voltage_ramp, mFOC.dq_v.Vq, final, START_RAMP_DURATION);
+	ramp_set_target(&g_foc.voltage_ramp, g_foc.dq_v.Vq, final, START_RAMP_DURATION);
 }
 
 void foc_set_speed_ramp(u16 rpm){
-	ramp_set_target(&mFOC.speed_ramp, foc_get_speed(), rpm, SPEED_RAMP_DURATION);
+	ramp_set_target(&g_foc.speed_ramp, foc_get_speed(), rpm, SPEED_RAMP_DURATION);
 }
 
+void foc_open_loop(void) {
+	g_foc.mode = FOC_MODE_OPEN_LOOP;
+}
 /*
 void foc_set_start_ramp(float v) {
-	ramp_set_target(&mFOC.voltage_ramp, mFOC.dq_v.Vq, v, START_RAMP_DURATION);
+	ramp_set_target(&g_foc.voltage_ramp, g_foc.dq_v.Vq, v, START_RAMP_DURATION);
 }*/
 
 void foc_set_ref_speed(u16 rpm) {
 	normal_task_enable(false);
-	if (mFOC.state == IDLE || mFOC.state == ANY_STOP) {
+	if (g_foc.state == IDLE || g_foc.state == ANY_STOP) {
 		normal_task_enable(true);
 		return;
 	}
-	mFOC.rpm_ref = rpm;
-	if (mFOC.mode == FOC_MODE_OPEN_LOOP) {
+	g_foc.speed_command = rpm;
+	if (g_foc.mode == FOC_MODE_OPEN_LOOP) {
 		foc_set_voltage_ramp(speed_to_voltage(rpm));
-		ramp_set_target(&mFOC.speed_ramp, rpm, rpm, SPEED_RAMP_DURATION);
-		ramp_exc(&mFOC.voltage_ramp);
+		ramp_set_target(&g_foc.speed_ramp, rpm, rpm, SPEED_RAMP_DURATION);
+		ramp_exc(&g_foc.voltage_ramp);
 	}
 	normal_task_enable(true);
 }
@@ -136,59 +143,40 @@ foc_fault_t foc_stop_motor(void) {
 }
 
 void foc_current_calibrate(void){
-	mFOC.current_samp.adc_offset_a = 0;
-	mFOC.current_samp.adc_offset_b = 0;
-	mFOC.current_samp.adc_offset_c = 0;
+	g_foc.current_samp.adc_offset_a = 0;
+	g_foc.current_samp.adc_offset_b = 0;
+	g_foc.current_samp.adc_offset_c = 0;
 
 	PWM_Disable_Channels();
 	//foc_pwm_start(false);
 
 	task_udelay(10);
 	
-	phase_current_init(&mFOC.current_samp);
-	mFOC.current_samp.is_calibrating = true;
-	mFOC.current_samp.sector = SECTOR_5;
+	phase_current_init(&g_foc.current_samp);
+	g_foc.current_samp.is_calibrating = true;
+	g_foc.current_samp.sector = SECTOR_5;
 	foc_pwm_start(true);
 	HAL_ADC1_InJ_StartConvert();
-	while(mFOC.current_samp.offset_sample_count != 0){};
+	while(g_foc.current_samp.offset_sample_count != 0){};
 	
 	foc_pwm_start(false);
 	task_udelay(100);
-	phase_current_init(&mFOC.current_samp);	
-	mFOC.current_samp.sector = SECTOR_1;
+	phase_current_init(&g_foc.current_samp);	
+	g_foc.current_samp.sector = SECTOR_1;
 	foc_pwm_start(true);
-	while(mFOC.current_samp.offset_sample_count != 0){};
-	mFOC.current_samp.is_calibrating = false;
+	while(g_foc.current_samp.offset_sample_count != 0){};
+	g_foc.current_samp.is_calibrating = false;
 	foc_pwm_start(false);
 	PWM_Enable_Channels();
 }
 
-void foc_overide_theta(bool enable){
-	mFOC.override.is_theta = enable;
-}
-
-void foc_overide_vdq(bool enable){
-	mFOC.override.is_vdq = enable;
-}
-
-void foc_overide_set_theta(float theta){
-	mFOC.override.theta = theta;
-}
-
-
-void foc_overide_set_vdq(float d, float q){
-	mFOC.override.vdq.Vd = d;
-	mFOC.override.vdq.Vq = q;
-	//printf("%f, %f\n", d, q);
-}
-
 float foc_get_vbus_voltage(void){
-	return mFOC.vbus;
+	return g_foc.vbus;
 }
 static u32 foc_measure_task(void){
 	vbus_sample_voltage();
 	ntc_sensor_sample();
-	LowPass_Filter(mFOC.vbus, vbus_get_voltage(), 0.1f);
+	LowPass_Filter(g_foc.vbus, vbus_get_voltage(), 0.1f);
 	wdog_reload();
 	return 1;
 }

Application/FOC/foc_api.h

@@ -5,15 +5,19 @@
 
 void foc_init(void);
 void foc_clear(void);
+void foc_open_loop(void);
 void set_dq_voltage(float d_v, float q_v);
 void foc_pwm_start(bool start);
 foc_fault_t foc_start_motor(void);
 foc_fault_t foc_stop_motor(void);
 int foc_hall_detect(float current, u16 *hall_table);
+void foc_set_dq_command(float d, float q);
+/*
 void foc_overide_theta(bool enable);
 void foc_overide_vdq(bool enable);
 void foc_overide_set_theta(float theta);
 void foc_overide_set_vdq(float d, float q);
+*/
 void foc_set_voltage_ramp(float final);
 void foc_set_speed_ramp(u16 rpm);
 //void foc_set_start_ramp(float v);

Application/FOC/foc_core.c

@@ -10,8 +10,8 @@
 #include "circle_limitation.h"
 #include "svpwm.h"
 
-motor_foc_t mFOC = {
-	.motor_p = {
+motor_foc_t g_foc = {
+	.motor_param = {
 		.poles = 2,
 		.ld = 0.578477f,
 		.lq = 5.78477f,
@@ -19,19 +19,19 @@ motor_foc_t mFOC = {
 		.inertia = 3.319367f,
 		.b_emf = 4.332566f,
 	},
-	.PI_id = {
+	.id_controller = {
 		.Kp_gain = 9,
 		.Ki_gain = 1071,
 		.max_output = MAX_VBUS_VOLTAGE,
 		.min_output = -MAX_VBUS_VOLTAGE,		
 	},
-	.PI_iq = {
+	.iq_controller = {
 		.Kp_gain = 10,
 		.Ki_gain = 1080,
 		.max_output = MAX_VBUS_VOLTAGE,
 		.min_output = -MAX_VBUS_VOLTAGE,
 	},
-	.PI_speed = {
+	.speed_controller = {
 		.Kp_gain = 1,
 		.Ki_gain = 200,
 		.max_output = MAX_CURRENT,
@@ -40,14 +40,8 @@ motor_foc_t mFOC = {
 };
 #if 1
 static void __inline foc_update_theta(motor_foc_t *foc) {
-	float angle = 0.0f;
-	if (foc->override.is_theta) {
-		angle = foc->override.theta;
-	}else {
-		angle = hall_sensor_get_theta();
-	}
-	foc->motor_s.angle = angle;
-	foc->motor_s.theta = degree_2_pi(foc->motor_s.angle);
+	foc->motor_stat.angle = hall_sensor_get_theta();
+	foc->motor_stat.theta = degree_2_pi(foc->motor_stat.angle);
 }
 
 #else
@@ -70,17 +64,13 @@ static void __inline foc_update_theta(motor_foc_t *foc) {
 static void __inline Foc_Calc_Voltage(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
 	//float vd = pi_control(&foc->PI_id, foc->dq_ref.Id - sampled->Id);
 	//float vq = pi_control(&foc->PI_iq, foc->dq_ref.Iq - sampled->Iq);
-	if (foc->mode == FOC_MODE_PI_CURRENT || foc->mode == FOC_MODE_PI_FULL) {
-		ref_out->Vd = pi_control(&foc->PI_id, foc->dq_ref.Id - sampled->Id);
-		ref_out->Vq = pi_control(&foc->PI_iq, foc->dq_ref.Iq - sampled->Iq);
+	if (foc->mode == FOC_MODE_CURRENT_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP) {
+		ref_out->Vd = pi_control(&foc->id_controller, foc->dq_command.Id - sampled->Id);
+		ref_out->Vq = pi_control(&foc->iq_controller, foc->dq_command.Iq - sampled->Iq);
 		//printf("vd = %f, vq = %f\n", vd, vq);
 	}else {
-		ref_out->Vd = foc->dq_ref.Vd;
-		ref_out->Vq = foc->dq_ref.Vq;
-	}
-	if (foc->override.is_vdq) {
-		ref_out->Vd = foc->override.vdq.Vd;
-		ref_out->Vq = foc->override.vdq.Vq;
+		ref_out->Vd = foc->dq_command.Vd;
+		ref_out->Vq = foc->dq_command.Vq;
 	}
 	foc->dq_v.Vd = ref_out->Vd;
 	foc->dq_v.Vq = ref_out->Vq;
@@ -160,13 +150,13 @@ void FOC_Fast_Task(motor_foc_t *foc){
 	/* ABC三相坐标到alpha-beta坐标 */
 	Clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
 	/* alpha-beta坐标系到D-Q旋转坐标系 */
-	Park(&sample_ab, foc->motor_s.theta, &sample_dq);
+	Park(&sample_ab, foc->motor_stat.theta, &sample_dq);
 	/* 电流环，输出电压给SVPWM */
 	Foc_Calc_Voltage(foc, &sample_dq, &v_dq);
 	/* 确保电压在6个扇区的内切圆中 */
 	CirCle_Limitation_Process(&v_dq, foc->vbus, 0.95f);
 	/* d-q坐标系到alpha-beta坐标系，输出给svpwm */
-	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
+	Rev_Park(&v_dq, foc->motor_stat.theta, &pwm_ab);
 	/* SVPWM，获取三相逆变器的开关时间，用的是pwm1模式，如果是pwm2模式，这个函数需要修改 */
 	SVM_Get_Phase_Time(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
 	/* 计算三相电流的采样点 */
@@ -186,29 +176,29 @@ void Foc_Calc_Current_Ref(motor_foc_t *foc) {
 	static int count = 0;
 	float speed_ref = ramp_get_target(&foc->speed_ramp);
 	float speed_feedback = foc_get_speed();
-	float vq_out = pi_control(&foc->PI_speed, speed_ref - speed_feedback);	
-	if (foc->mode == FOC_MODE_PI_SPEED || foc->mode == FOC_MODE_PI_FULL){
-		foc->dq_ref.Iq = vq_out;
-		foc->dq_ref.Id = 0.0f; //if MTPA used, d is not 0
+	float vq_out = pi_control(&foc->speed_controller, speed_ref - speed_feedback);	
+	if (foc->mode == FOC_MODE_SPEED_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP){
+		foc->dq_command.Iq = vq_out;
+		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0
 		if (((count) % 10) == 0) {
 			printf("vq_out = %f, %f, %f\n", vq_out, speed_ref, speed_feedback);
 		}
 		count++;
 		
 	}else {
-		foc->dq_ref.Iq = ramp_get_target(&foc->current_ramp);
-		foc->dq_ref.Id = 0.0f; //if MTPA used, d is not 0
+		foc->dq_command.Iq = ramp_get_target(&foc->current_ramp);
+		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0
 	}
 }
 
 void Foc_Speed_Ramp(motor_foc_t *foc){
-	if (foc->rpm_ref >= 0 && foc->mode != FOC_MODE_OPEN_LOOP){
+	if (foc->speed_command >= 0 && foc->mode != FOC_MODE_OPEN_LOOP){
 		u16 current_rpm = foc_get_speed();
-		u16 ref_rpm = foc->rpm_ref;
-		foc->rpm_ref = -1;
+		u16 ref_rpm = foc->speed_command;
+		foc->speed_command = -1;
 		
 		if (ref_rpm + RPM_FOR_CLOSE_LOOP < current_rpm){
-			ramp_set_target(&foc->voltage_ramp, foc->dq_ref.Vq, speed_to_voltage(ref_rpm), SPEED_RAMP_DURATION);
+			ramp_set_target(&foc->voltage_ramp, foc->dq_command.Vq, speed_to_voltage(ref_rpm), SPEED_RAMP_DURATION);
 			ramp_exc(&foc->current_ramp);
 			foc->mode = FOC_MODE_OPEN_LOOP;
 		}
@@ -216,11 +206,11 @@ void Foc_Speed_Ramp(motor_foc_t *foc){
 }
 
 void foc_brake_handler(void) {
-	mFOC.foc_fault = foc_brake_error;
+	g_foc.foc_fault = foc_brake_error;
 }
 
 void foc_pwm_up_handler(void){
-	phase_current_adc_triger(&mFOC.current_samp);
+	phase_current_adc_triger(&g_foc.current_samp);
 }
 
 #if defined (CCMRAM)
@@ -232,19 +222,19 @@ __attribute__( ( section ( ".ccmram" ) ) )
 #endif
 
 void current_sample_handler(void) {
-	if (mFOC.current_samp.is_calibrating) {
-		phase_current_offset(&mFOC.current_samp);
+	if (g_foc.current_samp.is_calibrating) {
+		phase_current_offset(&g_foc.current_samp);
 	}else {
-		FOC_Fast_Task(&mFOC);
+		FOC_Fast_Task(&g_foc);
 	}
 }
 
 void foc_slow_task_handler(void) {
-	FOC_Normal_Task(&mFOC);
+	FOC_Normal_Task(&g_foc);
 }
 
 void foc_pwm_start(bool start) {
-	if (start == mFOC.mosGate) {
+	if (start == g_foc.mosfec_gate) {
 		return;
 	}
 	if (start) {
@@ -252,6 +242,6 @@ void foc_pwm_start(bool start) {
 	}else {
 		PWM_Stop();
 	}
-	mFOC.mosGate = start;	
+	g_foc.mosfec_gate = start;	
 }
 

Application/FOC/foc_stm.c

@@ -3,52 +3,44 @@
 #include "foc/foc_api.h"
 #include "foc/foc_core.h"
 
-extern motor_foc_t mFOC;
+extern motor_foc_t g_foc;
 
-FOCState foc_stm_state(void){
-	return mFOC.state;
+foc_state_t foc_stm_state(void){
+	return g_foc.state;
 }
 
-foc_fault_t foc_stm_nextstate(FOCState state) {
+foc_fault_t foc_stm_nextstate(foc_state_t state) {
 	bool changed = false;
-	if (state == mFOC.state) {
+	if (state == g_foc.state) {
 		return foc_success;
 	}
 	if (state == START) {
-		if (mFOC.state == IDLE) {
+		if (g_foc.state == IDLE) {
 			changed = true;
 		}
 	}else if (state == IDLE) {
-		if (mFOC.state == ANY_STOP) {
+		if (g_foc.state == ANY_STOP) {
 			changed = true;
 		}
 	}else if (state == ANY_STOP) {
-		if (mFOC.state != IDLE) {
+		if (g_foc.state != IDLE) {
 			changed = true;
 		}
 	}else if (state == CURRENT_CALIBRATE) {
-		if (mFOC.state == START) {
+		if (g_foc.state == START) {
 			changed = true;
 		}
 	}else if (state == READY_TO_RUN) {
-		if (mFOC.state == CURRENT_CALIBRATE) {
+		if (g_foc.state == CURRENT_CALIBRATE) {
 			changed = true;
 		}
 	}else if (state == RAMPING_START) {
-		if (mFOC.state == READY_TO_RUN || mFOC.state == RUNNING) {
-			changed = true;
-		}
-	}else if (state == CLOSED_LOOP) {
-		if (mFOC.state == RAMPING_START) {
-			changed = true;
-		}		
-	}else if (state == RUNNING) {
-		if (mFOC.state == CLOSED_LOOP) {
+		if (g_foc.state == READY_TO_RUN || g_foc.state == RUNNING) {
 			changed = true;
 		}
 	}
 	if (changed) {
-		mFOC.state = state;
+		g_foc.state = state;
 		return foc_success;
 	}
 	return foc_not_allowed;
@@ -71,22 +63,21 @@ void FOC_Normal_Task(motor_foc_t *foc) {
 			foc_pwm_start(true);
 			foc_stm_nextstate(RAMPING_START);
 			ramp_exc(&foc->voltage_ramp);
-			foc_overide_vdq(true);
 			break;
 		case RAMPING_START:
-			foc_overide_set_vdq(0.0f, ramp_get_target(&foc->voltage_ramp));
+			foc_set_dq_command(0.0f, ramp_get_target(&foc->voltage_ramp));
 			printf("target %f\n", ramp_get_target(&foc->voltage_ramp));
 			if (foc_get_speed() >= RPM_FOR_CLOSE_LOOP){
 				//foc_stm_nextstate(CLOSED_LOOP);
 				//foc_overide_vdq(false);
 			}
 			break;
-		case CLOSED_LOOP:
-			Foc_Speed_Ramp(foc);
-			foc->mode = FOC_MODE_PI_FULL;
-			foc_stm_nextstate(RUNNING);
-			ramp_clear(&foc->voltage_ramp);
-			break;
+		// case CLOSED_LOOP:
+		// 	Foc_Speed_Ramp(foc);
+		// 	foc->mode = FOC_MODE_PI_FULL;
+		// 	foc_stm_nextstate(RUNNING);
+		// 	ramp_clear(&foc->voltage_ramp);
+		// 	break;
 		case RUNNING:
 			Foc_Speed_Ramp(foc);
 			Foc_Calc_Current_Ref(foc);

Application/FOC/foc_stm.h

@@ -3,7 +3,7 @@
 #include "foc/foc_api.h"
 #include "foc/foc_core.h"
 
-FOCState foc_stm_state(void);
-foc_fault_t foc_stm_nextstate(FOCState state);
+foc_state_t foc_stm_state(void);
+foc_fault_t foc_stm_nextstate(foc_state_t state);
 void FOC_Normal_Task(motor_foc_t *foc);
 #endif /* _FOC_STM_H__ */

Application/FOC/foc_type.h

@@ -52,18 +52,16 @@ typedef enum {
 	CHARGER_BOOT_CAP,
 	READY_TO_RUN,
 	RAMPING_START,
-	CLOSED_LOOP,
 	RUNNING,
 	ANY_STOP
-}FOCState;
+}foc_state_t;
 
 typedef enum {
-	FOC_MODE_OPEN_LOOP, //����
-	FOC_MODE_PI_CURRENT,      //ֻ������
-	FOC_MODE_PI_SPEED,     //ֻ�ٶȻ�
-	FOC_MODE_PI_FULL,         //�ٶȣ�����������
-	FOC_MODE_CURRENT_CALI,    //У׼������ȡ
-}foc_mode_t;
+	FOC_MODE_OPEN_LOOP, //开环模式，霍尔，电流等校准都在开环模式下，启动开始默认开环，等速度上来后切换到闭环
+	FOC_MODE_CURRENT_LOOP,      //ֻ电流环模式
+	FOC_MODE_SPEED_LOOP,     //ֻ速度环模式
+	FOC_MODE_CLOSE_LOOP,         //电流速度双闭环模式
+}control_mode_t;
 
 typedef struct current_sample {
 	u32   adc_offset_a;
@@ -78,13 +76,14 @@ typedef struct current_sample {
 	volatile bool  is_calibrating;
 }current_samp_t;
 
+/*
 typedef struct _override {
 	bool is_theta;
 	float theta;
 	bool is_vdq;
 	dq_t vdq;
 }override_param_t;
-
+*/
 typedef enum {
 	foc_success = 0,
 	foc_not_allowed = 1,
@@ -95,22 +94,22 @@ typedef enum {
 typedef struct foc_s {
 	bool is_ready;
 	alpha_beta_t alpha_beta;
-	current_samp_t current_samp; /* ����������� */
-	dq_t dq_ref; //���������ģʽ�£����ٶȻ����
-	int  rpm_ref; //�û����õ��ٶ�
+	current_samp_t current_samp; /*三相电流采集 */
+	dq_t dq_command; 
+	int  speed_command; 
 	dq_t dq_v;
-	u8   sector; //svpwm ����
-	float vbus; //ĸ�ߵ�ѹ
-	motor_param_t motor_p;
-	motor_stat_t  motor_s;
+	u8   sector; //svpwm 扇区
+	float vbus; //母线电压
+	motor_param_t motor_param;
+	motor_stat_t  motor_stat;
 	phase_time_t phase_time;
-	PI_ctrl_t    PI_id;
-	PI_ctrl_t    PI_iq;
-	PI_ctrl_t    PI_speed;
-	volatile FOCState state;
-	volatile foc_mode_t mode;
-	override_param_t override;
-	bool mosGate;
+	pi_controller_t    id_controller;
+	pi_controller_t    iq_controller;
+	pi_controller_t    speed_controller;
+	volatile foc_state_t state;
+	volatile control_mode_t mode;
+	//override_param_t override;
+	bool mosfec_gate;
 	ramp_t voltage_ramp; //开环情况下直接设置svm的voltage
 	ramp_t current_ramp; //���ٵ���б��
 	ramp_t speed_ramp; //�ı��ٶȵ�ramp���������ű仯��Ҫ����speed_ramp

Application/FOC/hall_sensor.c

@@ -85,6 +85,9 @@ static void _detect_timeout(void){
 }
 
 float hall_sensor_get_theta(void){
+	if (_hall.is_override_theta) {
+		return _hall.override_theta;
+	}
 	if (!_hall.working) {
 		read_hall(_hall.state, _hall.theta);
 		return _hall.theta;
@@ -93,15 +96,22 @@ float hall_sensor_get_theta(void){
 	float est_delta = _hall.est_theta - _hall.theta;
 	if (est_delta > 60) {
 		_hall.est_theta = _hall.theta + 60;
+		if (_hall.est_theta >= 360){
+			_hall.est_theta -= 360;
+		}
 	}else if (est_delta < -60){
 		_hall.est_theta = _hall.theta - 60;
+		if (_hall.est_theta <= -360) {
+			_hall.est_theta += 360;
+		}
 	}
-	
-	float angle = _hall.est_theta;
-	fast_norm_angle(&angle);
-	return angle;
+	return _hall.est_theta;
 }
 
+void hall_sensor_set_theta(bool override, float theta){
+	_hall.is_override_theta = override;
+	_hall.override_theta = theta;
+}
 
 float hall_sensor_get_speed(void) {
 	_detect_timeout();
@@ -114,17 +124,14 @@ float hall_sensor_avg_speed(void) {
 }
 
 
-int hall_sensor_calibrate(float current, u16 *hall_table){
-	foc_overide_set_theta(0.0f);
-	foc_overide_theta(true);
-
-	foc_overide_set_vdq(0.0f, 0.0f);
-	foc_overide_vdq(true);
-	
+int hall_sensor_calibrate(float voltage, u16 *hall_table){
+	foc_open_loop();
+	hall_sensor_set_theta(true, 0.0f);
+	foc_set_dq_command(0.0f, 0.0f);
 	foc_pwm_start(true);
 	HAL_ADC1_InJ_StartConvert();
 	for (int i = 0;i < 1000;i++) {
-		foc_overide_set_vdq((float)i * current / 1000.0f, 0.0f);
+		foc_set_dq_command((float)i * voltage / 1000.0f, 0.0f);
 		task_udelay(1000);
 	}
 	float sin_hall[8];
@@ -137,7 +144,7 @@ int hall_sensor_calibrate(float current, u16 *hall_table){
 	// Forwards
 	for (int i = 0;i < 3;i++) {
 		for (int j = 0;j < 360;j++) {
-			foc_overide_set_theta(j);
+			hall_sensor_set_theta(true, j);
 			task_udelay(10 * 1000);
 
 			int hall = get_hall_stat(7);
@@ -152,7 +159,7 @@ int hall_sensor_calibrate(float current, u16 *hall_table){
 	// Reverse
 	for (int i = 0;i < 3;i++) {
 		for (int j = 360;j >= 0;j--) {
-			foc_overide_set_theta(j);
+			hall_sensor_set_theta(true, j);
 			task_udelay(10 * 1000);
 
 			int hall = get_hall_stat(7);
@@ -164,8 +171,8 @@ int hall_sensor_calibrate(float current, u16 *hall_table){
 		}
 	}
 	foc_pwm_start(false);
-	foc_overide_theta(false);
-	foc_overide_vdq(false);
+	hall_sensor_set_theta(false, 0.0f);
+	foc_set_dq_command(0.0f, 0.0f);
 	int fails = 0;
 	for(int i = 0;i < 8;i++) {
 		if (hall_iterations[i] > 30) {

Application/FOC/hall_sensor.h

@@ -29,6 +29,8 @@ typedef struct {
 	s8    direction;
 	float degree_per_s; //当前的电角度, 单位：rad/s
 	float phase_offset;
+	bool  is_override_theta;
+	float override_theta;
 }hall_t;
 
 #define SAMPLE_MAX_COUNT 6
@@ -44,7 +46,7 @@ void hall_sensor_init(void);
 float hall_sensor_get_theta(void); //return degree
 float hall_sensor_get_speed(void); //return rpm
 float hall_sensor_avg_speed(void);
-int hall_sensor_calibrate(float current, u16 *hall_table);
-
+int hall_sensor_calibrate(float voltage, u16 *hall_table);
+void hall_sensor_set_theta(bool override, float theta);
 #endif /* _HALL_SENSOR_H__ */
 

Application/Math/fast_math.c

@@ -25,7 +25,7 @@ int main(){
         return 0;
 }
 */
-static float _sintable[] = {
+static float sin_table[] = {
         0.000000f,0.001745f,0.003491f,0.005236f,0.006981f,0.008727f,0.010472f,0.012217f,0.013962f,0.015707f,
         0.017452f,0.019197f,0.020942f,0.022687f,0.024432f,0.026177f,0.027922f,0.029666f,0.031411f,0.033155f,
         0.034899f,0.036644f,0.038388f,0.040132f,0.041876f,0.043619f,0.045363f,0.047106f,0.048850f,0.050593f,
@@ -206,7 +206,25 @@ void normal_sincosf(float angle, float *sin, float *cos) {
 	*cos = cosf(angle);
 }
 
+#define LOOKUP_I 0.1f
+#define LOOKUP_S 900
 void fast_sincosf(float angle, float *sin, float *cos){
-	
-
+	int index;
+	if (angle >=0 && angle < 90) {
+		index = (int)(angle/LOOKUP_I);
+		*sin = sin_table[index];
+		*cos = sin_table[LOOKUP_S - index];
+	}else if (angle >= 90 && angle < 180) {
+		index = (int)((angle - 90.0f)/LOOKUP_I);
+		*sin = sin_table[LOOKUP_S - index];
+		*cos = -sin_table[index];	
+	}else if (angle >= 180 && angle < 270) {
+		index = (int)((angle - 180.0f)/LOOKUP_I);
+		*sin = -sin_table[index];
+		*cos = -sin_table[LOOKUP_S - index];		
+	}else {
+		index = (int)((angle - 270.0f)/LOOKUP_I);
+		*sin = -sin_table[450 - index];
+		*cos = sin_table[index];		
+	}
 }

Project/Motor_PMSM.uvoptx

@@ -320,7 +320,7 @@
   </Group>
 
   <Group>
-    <GroupName>FOC</GroupName>
+    <GroupName>Foc</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
@@ -344,8 +344,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Application\FOC\PI_Controller.c</PathWithFileName>
-      <FilenameWithoutPath>PI_Controller.c</FilenameWithoutPath>
+      <PathWithFileName>..\Application\FOC\pi_controller.c</PathWithFileName>
+      <FilenameWithoutPath>pi_controller.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -528,7 +528,7 @@
   </Group>
 
   <Group>
-    <GroupName>BSP</GroupName>
+    <GroupName>Bsp</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
@@ -584,7 +584,7 @@
   </Group>
 
   <Group>
-    <GroupName>HAL</GroupName>
+    <GroupName>Hal</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>

Project/Motor_PMSM.uvprojx

@@ -401,7 +401,7 @@
           </Files>
         </Group>
         <Group>
-          <GroupName>FOC</GroupName>
+          <GroupName>Foc</GroupName>
           <Files>
             <File>
               <FileName>svpwm.c</FileName>
@@ -409,9 +409,9 @@
               <FilePath>..\Application\FOC\svpwm.c</FilePath>
             </File>
             <File>
-              <FileName>PI_Controller.c</FileName>
+              <FileName>pi_controller.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\Application\FOC\PI_Controller.c</FilePath>
+              <FilePath>..\Application\FOC\pi_controller.c</FilePath>
             </File>
             <File>
               <FileName>phase_current.c</FileName>
@@ -491,7 +491,7 @@
           </Files>
         </Group>
         <Group>
-          <GroupName>BSP</GroupName>
+          <GroupName>Bsp</GroupName>
           <Files>
             <File>
               <FileName>bsp.c</FileName>
@@ -516,7 +516,7 @@
           </Files>
         </Group>
         <Group>
-          <GroupName>HAL</GroupName>
+          <GroupName>Hal</GroupName>
           <Files>
             <File>
               <FileName>hal.c</FileName>