update hall,停止的时候转速平滑到0

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

Applications/foc/commands.c

@@ -197,6 +197,23 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			sys_debug("start motor %d\n", erroCode);
 			break;
 		}
+		case Foc_IF_Start:
+		{
+			if (command->len < 9) {
+				erroCode = FOC_Param_Err;
+			}else {
+				bool start = decode_u8(command->data) == 1?true:false;
+				s16  accl  = decode_s16((u8 *)command->data + 1);
+				s16  max_vel  = decode_s16((u8 *)command->data + 3);
+				s16  vd  = decode_s16((u8 *)command->data + 5);
+				s16  iq  = decode_s16((u8 *)command->data + 7);
+				sys_debug("IF:%d, %d, %d, %d, %d\n", start, accl, max_vel, vd, iq);
+				if (!mc_start_IF_mode(start, accl, max_vel, vd, iq)) {
+					erroCode = FOC_NotAllowed;
+				}
+			}
+			break;
+		}
 		case Foc_Set_DQ_Current:
 		{
 		#ifdef CONFIG_DQ_STEP_RESPONSE

Applications/foc/commands.h

@@ -47,6 +47,7 @@ typedef enum {
 	Foc_Get_Adrc_Params,
 	Foc_Get_MC_NV_Err_RT,
 	Foc_Get_MC_NV_Crit_Err,
+	Foc_IF_Start,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/controller.c

@@ -84,6 +84,39 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	return true;
 }
 
+void mot_contrl_IF_start(mot_contrl_t *ctrl, bool start, float accl, float max_vel, float iq_set) {
+	if (ctrl->if_ctl.b_ena == start) {
+		return;
+	}
+	if (start) {
+		memset(&ctrl->if_ctl, 0, sizeof(IF_t));
+		ctrl->if_ctl.elec_w_acc = accl * (1.0/30.0*M_PI*mc_conf()->m.poles);
+		ctrl->if_ctl.max_vel = max_vel * (1.0/30.0*M_PI*mc_conf()->m.poles);
+		ctrl->if_ctl.curr_angle = -M_PI/2.0f;
+		ctrl->if_ctl.angle_deg = pi_2_degree(ctrl->if_ctl.curr_angle);
+	}
+	if (start) {
+		ctrl->if_ctl.iq_set = iq_set;
+	}else {
+		ctrl->if_ctl.iq_set = 0;
+		mot_contrl_set_current(ctrl, 0);
+	}
+	ctrl->if_ctl.b_ena = start;
+}
+
+
+float mot_contrl_IF_update(mot_contrl_t *ctrl) {
+	IF_t *i_f = &ctrl->if_ctl;
+	i_f->curr_vel += i_f->elec_w_acc * ctrl->foc.ts; //加速度积分获取速度
+	if (i_f->curr_vel > ctrl->if_ctl.max_vel) {
+		i_f->curr_vel = ctrl->if_ctl.max_vel;
+	}
+	i_f->curr_angle += i_f->curr_vel * ctrl->foc.ts; //速度积分获取电角度
+	norm_angle_rad(i_f->curr_angle);
+	ctrl->if_ctl.angle_deg = pi_2_degree(ctrl->if_ctl.curr_angle);
+	return ctrl->if_ctl.angle_deg;
+}
+
 bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode) {
 	if (mode > CTRL_MODE_EBRAKE) {
 		mot_contrl_set_error(ctrl, FOC_Param_Err);
@@ -266,8 +299,12 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 		enc_angle = foc_observer_sensorless_angle();
 		enc_vel = foc_observer_sensorless_speed();
 	}
+	
 	if (!ctrl->b_mtpa_calibrate && (ctrl->force_angle != INVALID_ANGLE)) {
 		foc->in.mot_angle = ctrl->force_angle;
+	}else if (ctrl->if_ctl.b_ena) {
+		enc_angle = mot_contrl_IF_update(ctrl);
+		mot_contrl_set_current(ctrl, ctrl->if_ctl.iq_set);
 	}else {
 		foc->in.mot_angle = enc_angle;
 	}

Applications/foc/core/controller.h

@@ -83,6 +83,16 @@ typedef enum {
 }ctrl_event_r_t;
 
 
+typedef struct {
+	float elec_w_acc;
+	float max_vel; //rpm
+	float iq_set;
+	float curr_vel;
+	float curr_angle;
+	float angle_deg;
+	float b_ena;
+}IF_t;
+
 typedef struct {
 	float mot_vel;
 	float torque;
@@ -168,6 +178,7 @@ typedef struct{
 	float 			phase_b_max;
 	float 			phase_c_max;
 	u32 			phase_unbalance_cnt;
+	IF_t			if_ctl;
 }mot_contrl_t;
 
 void mot_contrl_init(mot_contrl_t *ctrl);
@@ -204,7 +215,7 @@ void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd)
 void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time);
 void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time);
 bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque);
-
+void mot_contrl_IF_start(mot_contrl_t *ctrl, bool start, float accl, float max_vel, float iq_set);
 
 static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
 	mot_contrl_enable(ctrl, true);

Applications/foc/motor/hall.c

@@ -12,8 +12,8 @@
 
 //#define USE_DETECTED_ANGLE 1
 
-#define HALL_READ_TIMES 5
-#define SMOOTH_COUNT 50.0F
+#define HALL_READ_TIMES 9
+#define SMOOTH_COUNT 0
 
 /* 
 1,5,4,6,2,3
@@ -30,11 +30,9 @@ static hall_t g_hall;
 
 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
-#define HALL_TIMEOUT_US (1000000L/4)
-
 
+u32 stop_cnt = 0;
 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++) {
@@ -70,6 +68,7 @@ static void hall_init_low_pos(void) {
 
 static void __INLINE hall_put_sample(u32 ticks, float angle) {
 	hsample_t *s = &g_hall.samples;
+	g_hall.last_delta_ticks = ticks;
 	s->ticks_sum -= s->ticks[s->index];
 	s->angles_sum -= s->angles[s->index];
 	s->ticks[s->index] = ticks;
@@ -93,19 +92,9 @@ static float __INLINE hall_elec_angle_vel(void){
 }
 
 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("angle dir %d, stat %d, lowres %f, err %d,%d, sp %d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.sig_errors, g_hall.noise_errors, stop_cnt);
 }
 
-static u32 hall_timeout_task(void *args) {
-	hall_t *phall = (hall_t *)args;
-	if (phall->velocity_raw != 0) {
-		if (time_delta_us(phall->edge_ticks, NULL) >= HALL_TIMEOUT_US) {
-			phall->elec_angle_vel = 0;
-			phall->velocity_raw = phall->velocity_filted = 0;
-		}
-	}
-	return 0;
-}
 
 void hall_init(void) {
 	g_hall.phase_offset = mc_conf()->m.encoder_offset;
@@ -126,11 +115,12 @@ void hall_init(void) {
 	if (!g_hall.inited) {
 		g_hall.inited = true;
 		gpio_hall_init();
-		shark_task_create(hall_timeout_task, &g_hall);
 	}
 	hall_init_low_pos();
+	stop_cnt = 0;
 }
 
+#if SMOOTH_COUNT > 0
 static float get_angle_diff(float a1, float a2) {
 	float diff = a1 - a2;
 	float abs_diff = ABS(diff);
@@ -140,6 +130,7 @@ static float get_angle_diff(float a1, float a2) {
 		return diff;
 	}
 }
+#endif
 
 static bool hall_update_low_pos(void) {
 	u8 state = hall_read_state();
@@ -148,20 +139,24 @@ static bool hall_update_low_pos(void) {
 		g_hall.sig_errors ++;
 		return false;
 	}
+	s16 pos_prev = hall_2_pos[g_hall.state];
 	g_hall.state = state;
-	s16 delta_pos = pos - g_hall.low_res_pos;
+	s16 delta_pos = pos - pos_prev;
 	g_hall.low_res_pos = pos;
 	if (delta_pos != 0) {
 		s8  prev_dir = g_hall.dir;
-		if (g_hall.samples.full || g_hall.samples.index != 0) {
-			if (delta_pos == 1 || delta_pos == -5) {
-				g_hall.dir = POSITIVE;
-			}else{
-				g_hall.dir = NEGATIVE;
-			}
+		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 {
+			//keep prev dir value
 		}
 		g_hall.edge_ticks = task_ticks_abs();
-		g_hall.prev_dir = prev_dir;
+	}else {
+		g_hall.sig_errors ++;
 	}
 	return true;
 }
@@ -187,8 +182,9 @@ float hall_update_elec_angle(void) {
 	}
 	float high_res_pos = delta_pos  + low_res;
 	float elec_angle = high_res_pos * PHASE_60_DEGREE;
-	float delta_angle = delta_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++;
@@ -200,9 +196,22 @@ float hall_update_elec_angle(void) {
 	}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.full && (delta_ticks / g_hall.last_delta_ticks >= 1.3f)) {
+		stop_cnt ++;
+		g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.99f;
+		if (g_hall.elec_angle_vel < 10) {
+			g_hall.elec_angle_vel = 0;
+		}
+		float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
+		g_hall.velocity_raw = velocity_raw;
+		LowPass_Filter(g_hall.velocity_filted, velocity_raw, 1.0f);
+	}
 	return hall_get_elec_angle();
 }
 
@@ -246,17 +255,18 @@ float hall_offset_detect(float *off) {
 }
 
 void HALL_IRQHandler(void) {
+	u32 mask = cpu_enter_critical();
 	u32 prev_ticks = g_hall.edge_ticks;
 	if (!hall_update_low_pos()) {
-		return;
+		goto hall_end;
 	}
 	if (time_delta_us(prev_ticks, NULL) == 0) {
 		g_hall.noise_errors++;
-		return;
+		goto hall_end;
 	}
 	g_hall.elec_angle_edge = g_hall.elec_angle;
-	float low_res = g_hall.low_res_pos;
-	g_hall.delta_angle_edge = get_angle_diff(low_res * PHASE_60_DEGREE, g_hall.elec_angle_edge);
+#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 = 0;//g_hall.delta_angle_edge/SMOOTH_COUNT;
 		g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
@@ -264,7 +274,11 @@ void HALL_IRQHandler(void) {
 		g_hall.angle_smooth_step = 0;
 		g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
 	}
+#endif
 	hall_calc_mot_velocity(prev_ticks);
+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
@@ -35,6 +36,7 @@ typedef struct {
 	s8				dir;
 	s8				prev_dir;
 	u32				edge_ticks;
+	u32				last_delta_ticks;
 	float			elec_angle; //经过插值的高分辨率角度
 	float			elec_angle_vel;
 	float			elec_angle_edge; //霍尔中断的时候，上面插值高分辨率的角度所存

Applications/foc/motor/motor.c

@@ -33,6 +33,9 @@ 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);
+static void _if_start_timer_handler(shark_timer_t *);
+static shark_timer_t _if_start_timer = TIMER_INIT(_if_start_timer, _if_start_timer_handler);
+
 motor_t motor = {
 	.s_direction = POSITIVE,
 	.n_gear = 0,
@@ -817,6 +820,58 @@ u16 mc_get_running_status2(void) {
 	return data;
 }
 
+static s16 if_acc, if_max_vel, if_iq_set;
+static void _if_start_timer_handler(shark_timer_t *t) {
+	mot_contrl_set_vdq_immediate(&motor.controller, 0, 0);
+	task_udelay(200);
+	u32 mask = cpu_enter_critical();
+	mc_set_ctrl_mode(CTRL_MODE_CURRENT);
+	mot_contrl_set_angle(&motor.controller, INVALID_ANGLE);
+	mot_contrl_IF_start(&motor.controller, true, if_acc, if_max_vel, if_iq_set);
+	cpu_exit_critical(mask);
+}
+
+bool mc_start_IF_mode(bool start, s16 accl, s16 max_vel, s16 align_vd, s16 iq_set) {
+	if ((motor.controller.if_ctl.b_ena == start) || shark_timer_started(&_if_start_timer)) {
+		return true;
+	}
+	if (start) {
+		motor.b_ignor_throttle = true;
+		MC_Check_MosVbusThrottle();
+		if (mc_unsafe_critical_error()) {
+			mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
+			return false;
+		}
+		pwm_up_enable(false);
+		pwm_turn_on_low_side();
+		task_udelay(500);
+		mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
+		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)align_vd/10.0f, 0);
+		if_acc = accl;
+		if_max_vel = max_vel;
+		if_iq_set = iq_set;
+		shark_timer_post(&_if_start_timer, 1500);
+	}else {
+		mot_contrl_set_vdq_immediate(&motor.controller, 0, 0);
+		task_udelay(500);
+		u32 mask = cpu_enter_critical();
+		shark_timer_cancel(&_if_start_timer);
+		pwm_disable_channel();
+		mot_contrl_stop(&motor.controller);
+		adc_stop_convert();
+		pwm_stop();
+		mc_internal_init(CTRL_MODE_OPEN, false);
+		mot_contrl_IF_start(&motor.controller, false, 0, 0, 0);
+		cpu_exit_critical(mask);
+	}
+	return true;
+}
 
 static float _force_angle = 0.0f;
 static int   _force_wait = 2000;
@@ -854,27 +909,18 @@ void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	pwm_up_enable(false);
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	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);
-	}
+	mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
 	mot_contrl_set_angle(&motor.controller, 0);
+	_force_wait = 2000;
 	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);
+		_force_wait += 1;
 	}
+	mot_contrl_set_vdq(&motor.controller, (float)vd/10.0f, 0);
 	if (vd < 0) {
 		_force_invert = true;
 	}else {
@@ -1601,6 +1647,9 @@ static void mc_motor_runstop(void) {
 		mc_gear_mode_set();
 		throttle_torque_reset();
 		pwm_enable_channel();
+#ifdef CONFIG_USE_ENCODER_HALL
+		motor_encoder_start(true);
+#endif
 		g_meas_foc.first = true;
 		motor.foc_start_cnt ++;
 		cpu_exit_critical(mask);

Applications/foc/motor/motor.h

@@ -137,6 +137,7 @@ 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);
+bool mc_start_IF_mode(bool start, s16 accl, s16 max_vel, s16 align_vd, s16 iq_set);
 
 static __INLINE mot_contrl_t *mot_contrl(void) {
 	return &motor.controller;