update hall function, not ready to run

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

Applications/app/app.c

@@ -159,6 +159,18 @@ static u32 app_report_task(void *p) {
 }
 int plot_type = 0;
 static void plot_smo_angle(void) {
+	u32 mask = cpu_enter_critical();
+	float smo_angle = foc_observer_sensorless_angle();
+	float mot_angle = hall_get_elec_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(mot_angle, smo_angle, delta);
+}
+
+static void plot_hall_angle(void) {
 	u32 mask = cpu_enter_critical();
 	float smo_angle = hall_get_elec_angle();
 	float mot_angle = foc()->in.mot_angle;
@@ -169,6 +181,7 @@ static void plot_smo_angle(void) {
 	delta = fast_atan2(s, c)/PI*180.0f;
 	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();
@@ -187,7 +200,8 @@ static u32 app_plot_task(void * args) {
 	}else if (plot_type == 3) {
 		plot_smo_angle();
 	}else if (plot_type == 4) {
-		can_plot2((s16)foc()->out.curr_dq.d, (s16)foc()->out.curr_dq.q);
+		//can_plot2((s16)foc()->out.curr_dq.d, (s16)foc()->out.curr_dq.q);
+		plot_hall_angle();
 	}else if (plot_type == 5) {
 		can_plot2((s16)foc()->in.target_id.interpolation , (s16)foc()->in.target_iq.interpolation);
 	}else if (plot_type == 6) {
@@ -206,7 +220,7 @@ static u32 app_plot_task(void * args) {
 		s16 duty_filterd = (s16)(motor.controller.duty_filterd * 100);
 		can_plot2(duty, duty_filterd);
 	}
-	return 20;
+	return 10;
 }
 static u32 app_low_task(void *args) {
 	wdog_reload();

Applications/bsp/gd32/adc.c

@@ -335,7 +335,7 @@ void adc_5vref_filter(void) {
 	LowPass_Filter(vref_5v_compestion_filter, value, VREF_COMP_LFP_CEOF);
 }
 #else
-#define VREF_5V_COMPESTION() VREF_3V3_COMPESTION()
+#define VREF_5V_COMPESTION() 1.0f
 void adc_5vref_filter(void) {
 
 }

Applications/bsp/gd32/board_mc100_v1.h

@@ -21,7 +21,8 @@
 
 #define CONFIG_HIGH_VOL_MODE_MIN_VOL (60.0F)
 
-#define CONFIG_SMO_OBSERVER 1
+//#define CONFIG_SMO_OBSERVER 1
+#define CONFIG_LADRC_OBSERVER 1
 //#define CONFIG_SPEED_LADRC  1
 #define CONFIG_SENSORLESS_TS FOC_CTRL_US
 
@@ -116,7 +117,7 @@
 #define W_PHASE_ADC_MODE 	GPIO_MODE_AIN
 
 #define ADC_TO_CURR_ceof1  (CONFIG_CURRENT_SENSOR_CEOF)
-#define ADC_TO_CURR_ceof2  (CONFIG_CURRENT_SENSOR_CEOF)
+#define ADC_TO_CURR_ceof2  (CONFIG_CURRENT_SENSOR_CEOF*1.16f)
 
 #define CONFIG_PWM_UV_SWAP 1
 

Applications/bsp/gd32/bsp.h

@@ -35,7 +35,7 @@
 #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
+#define CONFIG_BOARD_MIN_VOLTAGE 30.0f
 #include "bsp/gd32/board_mc100_v1.h"
 #define CONFIG_BOARD_MCXXX
 #define CONFIG_BOARD_NAME "MC100"

Applications/foc/foc_config.h

@@ -66,7 +66,7 @@
 
 #define CONFIG_ENABLE_IAB_REC 1   // for phase current/voltage debug
 
-#define CONFIG_MOT_IND_USE_PHASE_SAMPLE 1 //电机参数离线识别使用采集的相电压
+#define CONFIG_MOT_IND_USE_PHASE_SAMPLE 0 //电机参数离线识别使用采集的相电压
 
 #ifdef CONFIG_SPEED_LADRC
 	#define CONFIG_LADRC_Wo  200.0F

Applications/foc/motor/encoder.c

@@ -254,7 +254,7 @@ static void encoder_detect_error(s16 cnt) {
 #endif
 }
 
-float encoder_get_theta(bool detect_err) {
+float encoder_update_elec_angle(bool detect_err) {
 	if (!g_encoder.b_index_found) {
 		return g_encoder.pwm_angle;
 	}

Applications/foc/motor/encoder.h

@@ -48,7 +48,7 @@ typedef struct {
 
 void encoder_init(void);
 void encoder_init_clear(void);
-float encoder_get_theta(bool detect_err);
+float encoder_update_elec_angle(bool detect_err);
 float encoder_get_speed(void);
 void encoder_set_direction(s8 direction);
 float encoder_get_vel_count(void);

Applications/foc/motor/hall.c

@@ -13,13 +13,19 @@
 //#define USE_DETECTED_ANGLE 1
 
 #define HALL_READ_TIMES 5
-#define SMOOTH_COUNT 10.0F
+#define SMOOTH_COUNT 50.0F
 
 /* 
 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,0,4,5,2,1,3,7};
+static s8 hall_2_pos[] = {7,2,0,1,4,3,5,7};
 static hall_t g_hall;
 
 measure_time_t g_meas_hall = {.exec_max_time = 6,};
@@ -32,25 +38,19 @@ measure_time_t g_meas_hall = {.exec_max_time = 6,};
 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++) {
-		if (gpio_hall_a_value()) {
-			hall_a++;
-		}
-		if (gpio_hall_b_value()) {
-			hall_b++;
-		}
-		if (gpio_hall_c_value()) {
-			hall_c++;
-		}
+		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<<2;
+		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;
+		state = state | (1 << 2);
 	}
 	return state;
 }
@@ -64,7 +64,7 @@ static void hall_init_low_pos(void) {
 	}
 	g_hall.state = state;
 	g_hall.prev_dir = g_hall.dir = POSITIVE;
-	g_hall.low_res_pos = pos + 0.5f - g_hall.phase_offset/PHASE_60_DEGREE;
+	g_hall.low_res_pos = pos + 0.5f;
 }
 
 
@@ -190,7 +190,7 @@ float hall_update_elec_angle(void) {
 	float delta_angle = delta_pos * PHASE_60_DEGREE;
 	float elec_smooth_angle;
 	if (g_hall.angle_smooth_cnt < (SMOOTH_COUNT + 1)) {
-		elec_smooth_angle = g_hall.delta_angle_edge + g_hall.angle_smooth_step * g_hall.angle_smooth_cnt + delta_angle;
+		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);
@@ -225,7 +225,7 @@ static void hall_calc_mot_velocity(u32 prev_ticks) {
 	float elec_vel;
 	if (g_hall.b_trns_det) {
 		elec_vel = PHASE_60_DEGREE/(us_2_s(delta_cnt));
-		LowPass_Filter(g_hall.elec_angle_vel, elec_vel, 1.0f);
+		LowPass_Filter(g_hall.elec_angle_vel, elec_vel, 0.5f);
 	}else {
 		g_hall.elec_angle_vel = hall_elec_angle_vel();
 	}

Applications/foc/motor/motor.c

@@ -825,7 +825,12 @@ static int   _force_invert = false;
 void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	if (motor.b_start || motor.b_force_run) {
 		if (vd == 0 && vq == 0) {
-			mot_contrl_set_vdq(&motor.controller, 0, 0);
+			if (align) {
+				mot_contrl_set_vdq(&motor.controller, 0, 0);
+			}else {
+				mot_contrl_set_vdq(&motor.controller, 0, 0);
+				mot_contrl_set_current(&motor.controller, 0);
+			}
 			delay_ms(500);
 			wdog_reload();
 			adc_stop_convert();
@@ -845,22 +850,30 @@ void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	if (mc_unsafe_critical_error()) {
 		mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 	}
-	
+	_force_angle = 0.0f;
 	pwm_up_enable(false);
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
+	if (align) {
+		mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
+	}else {
+		mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
+		//mot_contrl_start(&motor.controller, CTRL_MODE_CURRENT);
+	}
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
 	mot_contrl_set_angle(&motor.controller, 0);
-	mot_contrl_set_vdq(&motor.controller, (float)vd, 0);
 	if (align) {
 		_force_wait = 2000 + 1;
+		mot_contrl_set_vdq(&motor.controller, (float)vd/10.0f, 0);
 	}else {
 		_force_wait = 2000;
+		mot_contrl_set_vdq(&motor.controller, (float)vd/10.0f, 0);
+		//mot_contrl_set_curr_mode_id(&motor.controller, true);
+		//mot_contrl_set_current(&motor.controller, (float)vd);
 	}
 	if (vd < 0) {
 		_force_invert = true;
@@ -1478,6 +1491,9 @@ static bool mc_process_force_running(void) {
 				mot_contrl_set_angle(&motor.controller, _force_angle);
 			}
 		}
+		if (motor.controller.mode_running == CTRL_MODE_CURRENT) {
+			mot_contrl_slow_task(&motor.controller);
+		}
 		return true;
 	}
 	return false;

Applications/foc/motor/motor.h

@@ -150,7 +150,7 @@ static __INLINE float motor_encoder_get_angle(void) {
 #ifdef CONFIG_USE_ENCODER_HALL
 	return hall_update_elec_angle();
 #elif defined (USE_ENCODER_ABI)
-	return encoder_get_theta(true);
+	return encoder_update_elec_angle(true);
 #else
 	#error "Postion sensor ERROR"
 #endif
@@ -170,7 +170,7 @@ static __INLINE void motor_encoder_update(bool detect_err) {
 #ifdef CONFIG_USE_ENCODER_HALL
 	hall_update_elec_angle();
 #elif defined (USE_ENCODER_ABI)
-	encoder_get_theta(detect_err);
+	encoder_update_elec_angle(detect_err);
 #else
 	#error "Postion sensor ERROR"
 #endif

Applications/math/fast_math.h

@@ -140,6 +140,20 @@ static __INLINE float fast_atan2(float y, float x) {
     return r;
 }
 
+static __INLINE void saturate_vector_2d(float *x, float *y, float max) {
+	float mag = NORM2_f(*x, *y);
+	max = fabsf(max);
+
+	if (mag < 1e-4f) {
+		mag = 1e-4f;
+	}
+	if (mag > max) {
+		const float f = max / mag;
+		*x *= f;
+		*y *= f;
+	}
+}
+
 static void normal_sincosf(float angle, float *sin, float *cos) {
 	*sin = arm_sin_f32(angle);
 	*cos = arm_cos_f32(angle);