加入IF启动模式，主要用来校准hall位置传感器

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

Applications/foc/commands.c

@@ -197,6 +197,24 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			sys_debug("start motor %d\n", erroCode);
 			break;
 		}
+		case Foc_IFCTL_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_ifctl_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_IFCTL_Start,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/controller.c

@@ -84,6 +84,42 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	return true;
 }
 
+void mot_contrl_ifctl_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_ctl_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 = 0;
+		ctrl->if_ctl.angle_deg = pi_2_degree(ctrl->if_ctl.curr_angle);
+	}
+	if (start) {
+		line_ramp_init(&ctrl->ramp_target_current, 20);
+		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_ifctl_update(mot_contrl_t *ctrl) {
+	if_ctl_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);
+	i_f->curr_vel_rpm = i_f->curr_vel/M_PI*30.0f/mc_conf()->m.poles;
+	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);
@@ -257,7 +293,12 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 
 	float enc_angle = motor_encoder_get_angle();
 	float enc_vel = motor_encoder_get_speed();
-	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
+
+	if (ctrl->if_ctl.b_ena) {
+		enc_angle = mot_contrl_ifctl_update(ctrl);
+		enc_vel = ctrl->if_ctl.curr_vel_rpm;
+		mot_contrl_set_current(ctrl, ctrl->if_ctl.iq_set);
+	}else if (!foc_observer_diagnostic(enc_angle, enc_vel)){
 		/* detect encoder angle error, do something here */
 		if (!foc_observer_sensorless_stable()) {
 			foc->in.mot_velocity = 0;

Applications/foc/core/controller.h

@@ -82,6 +82,16 @@ typedef enum {
 	FOC_NV_Err,
 }ctrl_event_r_t;
 
+typedef struct {
+	float elec_w_acc;
+	float max_vel; //rpm
+	float iq_set;
+	float curr_vel;
+	float curr_vel_rpm;
+	float curr_angle;
+	float angle_deg;
+	float b_ena;
+}if_ctl_t; //IF CTRL Force running
 
 typedef struct {
 	float mot_vel;
@@ -166,6 +176,7 @@ typedef struct{
 	lineramp_t 		ramp_dc_curr_lim;
 	lineramp_t		ramp_input_torque;
 	lineramp_t		ramp_adv_angle;
+	if_ctl_t		if_ctl;
 	float 			phase_a_max;
 	float 			phase_b_max;
 	float 			phase_c_max;
@@ -206,7 +217,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_ifctl_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/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,
@@ -826,9 +829,62 @@ u16 mc_get_running_status2(void) {
 	return data;
 }
 
+static void _if_start_timer_handler(shark_timer_t *t) {
+	mot_contrl_set_vdq_immediate(&motor.controller, 0, 0);
+	task_udelay(200);
+	sys_debug("start IF\n");
+	u32 mask = cpu_enter_critical();
+	mc_internal_init(CTRL_MODE_OPEN, true);
+	mc_set_ctrl_mode(CTRL_MODE_CURRENT);
+	mot_contrl_set_angle(&motor.controller, INVALID_ANGLE);
+	mot_contrl_ifctl_start(&motor.controller, true, motor.if_acc, motor.if_max_vel, motor.if_iq_set);
+	cpu_exit_critical(mask);
+}
+
+bool mc_start_ifctl_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);
+		motor.if_acc = accl;
+		motor.if_iq_set = iq_set;
+		motor.if_max_vel = max_vel;
+		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_ifctl_start(&motor.controller, false, 0, 0, 0);
+		cpu_exit_critical(mask);
+		pwm_up_enable(true);
+	}
+	return true;
+}
+
 
-static float _force_angle = 0.0f;
-static int   _force_wait = 2000;
 /* 开环，强制给定电角度和DQ的电压 */
 void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	if (motor.b_start || motor.b_force_run) {
@@ -866,9 +922,9 @@ void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	mot_contrl_set_angle(&motor.controller, 0);
 	mot_contrl_set_vdq(&motor.controller, (float)vd, 0);
 	if (align) {
-		_force_wait = 2000 + 1;
+		motor.force_open_wait = 2000 + 1;
 	}else {
-		_force_wait = 2000;
+		motor.force_open_wait = 2000;
 	}
 	motor.b_force_run = true;
 }
@@ -1468,13 +1524,13 @@ static void mc_process_epm_move(void) {
 
 static bool mc_process_force_running(void) {
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
-		if (motor.b_force_run && _force_wait <= 2000) {
-			if (_force_wait > 0) {
-				--_force_wait;
+		if (motor.b_force_run && motor.force_open_wait <= 2000) {
+			if (motor.force_open_wait > 0) {
+				--motor.force_open_wait;
 			}else {
-				_force_angle += 1.5f;
-				norm_angle_deg(_force_angle);
-				mot_contrl_set_angle(&motor.controller, _force_angle);
+				motor.force_open_angle += 1.5f;
+				norm_angle_deg(motor.force_open_angle);
+				mot_contrl_set_angle(&motor.controller, motor.force_open_angle);
 			}
 		}
 		return true;

Applications/foc/motor/motor.h

@@ -31,7 +31,7 @@ typedef struct {
 }user_rt_set;
 
 typedef struct {
-	bool   b_start;
+	bool   	b_start;
 	float  throttle;
 	bool   b_ignor_throttle;
 	bool   b_ind_start;
@@ -44,39 +44,43 @@ typedef struct {
 	bool   b_epm;
 	bool   b_cruise;
 	u32    cruise_time;
-	float  cruise_torque;
-	epm_dir_t epm_dir;
-	bool   b_epm_cmd_move;
-	float  f_epm_vel;
-	float  f_epm_trq;
-	u32    runStall_time;
-	float  runStall_pos;
-	u8     u_throttle_ration;
-	s8     s_direction;
-	u32    n_brake_errors;
-	u32    n_CritiCalErrMask;
-	u8     mode;
-	bool   b_high_vol_mode;
-	u8     n_gear;
-	bool   b_limit_pending;
-	gear_t *gear_cfg;
-	u32    n_autohold_time;
-	bool   b_wait_brk_release;
-	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;
+	float  		cruise_torque;
+	epm_dir_t 	epm_dir;
+	bool   		b_epm_cmd_move;
+	float  		f_epm_vel;
+	float  		f_epm_trq;
+	u32    		runStall_time;
+	float  		runStall_pos;
+	u8     		u_throttle_ration;
+	s8     		s_direction;
+	u32    		n_brake_errors;
+	u32    		n_CritiCalErrMask;
+	u8     		mode;
+	bool   		b_high_vol_mode;
+	u8     		n_gear;
+	bool   		b_limit_pending;
+	gear_t 		*gear_cfg;
+	u32    		n_autohold_time;
+	bool   		b_wait_brk_release;
+	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];
-
+	user_rt_set  u_set; //用户运行时设置
+	fan_t  		fan[2];
+	s16 		if_acc;
+	s16			if_max_vel;
+	s16			if_iq_set;
+	float		force_open_angle;
+	s32			force_open_wait;
 	//for debug
 	int foc_stop_cnt;
 	int foc_start_cnt;
@@ -137,6 +141,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_ifctl_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;