fix foc_clear

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

Applications/app/app.c

@@ -26,7 +26,7 @@ static void _can_report_info(void) {
 	current_samp_t *s = foc_get_current_sample();
 	can_report_phase_current(0x45, F2I(s->Ia * 1000), F2I(s->Ib * 1000), F2I(s->Ic * 1000));
 	//sys_debug("phase current %f %f %f %f\n", s->Ia, s->Ib, s->Ic, s->max_Ia);
-	//sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
+	sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
 	s->max_Ia = 0.0f;
 }
 

Applications/foc/foc_api.c

@@ -49,7 +49,7 @@ static void foc_defulat_value(void){
 	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);	
+	pi_clear(&g_foc.speed_controller);
 }
 
 current_samp_t *foc_get_current_sample(void) {
@@ -75,7 +75,24 @@ void foc_stop(void) {
 }
 
 void foc_clear(void) {
-	foc_defulat_value();
+	g_foc.alpha_beta.alpha = 0;
+	g_foc.alpha_beta.beta = 0;
+	g_foc.dq_command.Id = 0;
+	g_foc.dq_command.Iq = 0;
+	g_foc.foc_fault = foc_success;
+	g_foc.speed_command.speed = -1;
+	g_foc.speed_command.delta_ts = 0;
+	memset(&g_foc.phase_time, 0, sizeof(g_foc.phase_time));
+	g_foc.sector = 0;
+	g_foc.dq_last.Id = 0;
+	g_foc.dq_last.Iq = 0;
+	phase_current_init(&g_foc.current_samp);
+	ramp_ctrl_init(&g_foc.voltage_ramp);
+	ramp_ctrl_init(&g_foc.current_ramp);
+	ramp_ctrl_init(&g_foc.speed_ramp);
+	pi_clear(&g_foc.id_controller);
+	pi_clear(&g_foc.iq_controller);
+	pi_clear(&g_foc.speed_controller);
 }
 
 u32 foc_get_speed(void) {

Applications/foc/foc_cmd.c

@@ -71,6 +71,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}else if (start == Foc_Stop) {
 				f = foc_stop_motor();
 			}
+			sys_debug("start motor %d\n", f);
 			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)f);
 			break;
 		}

Applications/foc/hall_sensor.c

@@ -202,9 +202,11 @@ int hall_sensor_calibrate(float voltage){
 	int hall_iterations[8];
 	memset(sin_hall, 0, sizeof(sin_hall));
 	memset(cos_hall, 0, sizeof(cos_hall));
+	memset(_sensor_hander.angle_table, 0, sizeof(_sensor_hander.angle_table));
 	memset(hall_iterations, 0, sizeof(hall_iterations));
 	co_task_delay(2 * 1000);
 	// Forwards
+#if 1	
 	for (int i = 0;i < 5;i++) {
 		for (int j = 0;j < 360;j++) {
 			hall_sensor_set_theta(true, j);
@@ -218,6 +220,8 @@ int hall_sensor_calibrate(float voltage){
 			hall_iterations[hall]++;
 		}
 	}
+#endif	
+#if 0
 	//hall_sensor_set_theta(true, 360);
 	//co_task_delay(2 * 1000);
 	sys_debug("Revers\n");
@@ -235,16 +239,23 @@ int hall_sensor_calibrate(float voltage){
 			hall_iterations[hall]++;
 		}
 	}
+#endif	
 	foc_pwm_start(false);
 	hall_sensor_set_theta(false, 0.0f);
 	foc_set_dq_command(0.0f, 0.0f);
 	int fails = 0;
 	for(int i = 0;i < 8;i++) {
 		if (hall_iterations[i] > 30) {
+			float ang = pi_2_degree(atan2f(sin_hall[i], cos_hall[i]));
+			fast_norm_angle(&ang);
+			_sensor_hander.angle_table[i] = (u16)ang;
+			sys_debug("%d: %d\n", i, _sensor_hander.angle_table[i]);
 		} else {
 			fails++;
+			_sensor_hander.angle_table[i] = 0xFFFF;
 		}
 	}
+	
 	return fails == 2;	
 }
 
@@ -350,7 +361,7 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 
 void hall_sensor_handler(void) {
 	if (_sensor_hander.is_override_angle) {
-		sys_debug("%d:%d\n", (int)get_hall_stat(HALL_READ_TIMES), (int)_sensor_hander.override_el_angle);
+		sys_debug("irq: %d:%d\n", (int)get_hall_stat(HALL_READ_TIMES), (int)_sensor_hander.override_el_angle);
 		return;
 	}
 	time_measure_start(&g_meas_hall);

Applications/foc/hall_sensor.h

@@ -49,6 +49,7 @@ typedef struct {
 	float override_el_angle;
 	hall_sample_t samples[8];
 	u32  sensor_error;
+	u16  angle_table[8];
 }hall_sensor_t;
 
 void hall_sensor_init(void);