Applications/bsp/gd32/can.h

@@ -27,6 +27,27 @@ typedef union {
 	};
 }can_id_t;
 
+#if 0
+typedef union {
+	uint32_t id;
+	struct {
+		uint32_t	dest		:5; /*bit 0-4  */
+		uint32_t	src 		:5; /*bit 5-9 */
+		uint32_t	idx 		:4; /*bit 10-13 */
+		uint32_t	total		:4; /*bit 14-17 */
+		uint32_t	command		:8; /*bit 18-25 */
+		uint32_t	type 		:1; /*bit 26, 1:Request(Need ack), 0:Ack(No ack needed)*/
+		uint32_t	res			:1; /*bit 27 */
+		uint32_t    lpkg		:1; /*bit 28 , 0: short package, 1: long package*/
+	};
+	struct {
+		uint32_t ext_command		:27; /*bit 0-26 */
+		uint32_t ext_type			:1;  /*bit 26, 1:Request(Need ack), 0:Ack(No ack needed)*/
+		uint32_t ext_pkg			:1;  /*bit 28 , 0: short package, 1: long package*/
+	};
+}cid_v2;
+#endif
+
 //CAN filter setting
 #ifdef CAN_COMMUNICATION_SPEC_V1P4
 //specification 1.4

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/hall.c

@@ -166,6 +166,7 @@ void hall_init(void) {
 	g_hall.samples.index = 0;
 	g_hall.samples.filled = 0;
 	g_hall.elec_angle_vel = 0;
+	g_hall.edge_ticks = 0;
 	g_hall.dir_set = g_hall.prev_dir = g_hall.dir = POSITIVE;
 	g_min_delta = 1000000 * 100;
 	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
@@ -285,17 +286,13 @@ float hall_update_elec_angle(void) {
 	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.filled > 1) && (delta_ticks / g_hall.last_delta_ticks >= 1.4f)) {
-		g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.95f;
-		if (g_hall.elec_angle_vel < 30) {
-			g_hall.elec_angle_vel = 0;
-			g_hall.samples.filled = 0;
-			g_hall.samples.index = 0;
-			g_hall.dir = g_hall.prev_dir = g_hall.dir_set;
-			hall_init_low_pos();
-		}
-		float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
-		g_hall.velocity_filted = velocity_raw;
+	if ((g_hall.elec_angle_vel > 0.000001f) && (delta_ticks >= HALL_MAX_TIME)) {
+		g_hall.elec_angle_vel = 0;
+		g_hall.samples.filled = 0;
+		g_hall.samples.index = 0;
+		g_hall.velocity_filted = 0;
+		g_hall.dir = g_hall.prev_dir = g_hall.dir_set;
+		hall_init_low_pos();
 	}
 	return hall_get_elec_angle();
 }
@@ -335,10 +332,11 @@ void HALL_IRQHandler(void) {
 	g_delta_cnt++;
 	u32 mask = cpu_enter_critical();
 	u32 delta_cnt = hall_delta_us(g_hall.edge_ticks);
-	if (delta_cnt < 200) {
+	if (delta_cnt < 200) { //1200rpm, 30 poles
 		g_hall.noise_errors++;
 		goto hall_end;
 	}
+
 	if (!hall_update_low_pos()) {
 		goto hall_end;
 	}

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,
@@ -743,6 +746,12 @@ bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
 		if (!mot_contrl_is_start(&motor.controller)) {
 			mot_contrl_start(&motor.controller, motor.mode);
 			mc_gear_mode_set(false);
+			motor_encoder_reinit();
+			if (dir == EPM_Dir_Back) {
+				motor_encoder_set_direction(NEGATIVE);
+			}else {
+				motor_encoder_set_direction(POSITIVE);
+			}
 			pwm_enable_channel();
 		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
@@ -826,9 +835,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 +928,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 +1530,17 @@ 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);
+				float positive = 1.0f;
+				if (line_ramp_get_target(&motor.controller.ramp_target_vd) < 0) {
+					positive = -1.0f;
+				}
+				motor.force_open_angle += 1.5f * positive;
+				norm_angle_deg(motor.force_open_angle);
+				mot_contrl_set_angle(&motor.controller, motor.force_open_angle);
 			}
 		}
 		return true;
@@ -1583,6 +1649,14 @@ static void mc_motor_runstop(void) {
 		mot_contrl_start(&motor.controller, motor.mode);
 		mc_gear_mode_set(true);
 		throttle_torque_reset();
+		motor_encoder_reinit();
+		if (motor.b_epm) {
+			if (motor.epm_dir == EPM_Dir_Back) {
+				motor_encoder_set_direction(NEGATIVE);
+			}else {
+				motor_encoder_set_direction(POSITIVE);
+			}
+		}
 		pwm_enable_channel();
 		g_meas_foc.first = true;
 		motor.foc_start_cnt ++;

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;
@@ -148,7 +153,7 @@ static __INLINE foc_t *foc(void) {
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL
-	return hall_sensor_get_theta(true);
+	return hall_update_elec_angle();
 #elif defined (USE_ENCODER_ABI)
 	return encoder_get_theta(true);
 #else
@@ -168,7 +173,7 @@ static __INLINE u8 motor_encoder_may_error(void) {
 
 static __INLINE void motor_encoder_update(bool detect_err) {
 #ifdef USE_ENCODER_HALL
-	hall_sensor_get_theta(detect_err);
+	hall_update_elec_angle();
 #elif defined (USE_ENCODER_ABI)
 	encoder_get_theta(detect_err);
 #else
@@ -178,7 +183,7 @@ static __INLINE void motor_encoder_update(bool detect_err) {
 
 static __INLINE float motor_encoder_get_speed(void) {
 #ifdef USE_ENCODER_HALL
-	return hall_sensor_get_speed();
+	return hall_get_velocity();
 #elif defined (USE_ENCODER_ABI)
 	return encoder_get_speed();
 #else
@@ -188,7 +193,7 @@ static __INLINE float motor_encoder_get_speed(void) {
 
 static __INLINE float motor_encoder_get_vel_count(void) {
 #ifdef USE_ENCODER_HALL
-	return 0;
+	return hall_get_vel_counts();
 #elif defined (USE_ENCODER_ABI)
 	return encoder_get_vel_count();
 #else
@@ -198,7 +203,7 @@ static __INLINE float motor_encoder_get_vel_count(void) {
 
 static __INLINE float motor_encoder_get_position(void) {
 #ifdef USE_ENCODER_HALL
-	return 0;
+	return hall_get_position();
 #elif defined (USE_ENCODER_ABI)
 	return encoder_get_position();
 #else
@@ -209,7 +214,7 @@ static __INLINE float motor_encoder_get_position(void) {
 
 static __INLINE void motor_encoder_init(void) {
 #ifdef USE_ENCODER_HALL
-	hall_sensor_init();
+	hall_init();
 #elif defined (USE_ENCODER_ABI)
 	encoder_init();
 #else
@@ -219,7 +224,7 @@ static __INLINE void motor_encoder_init(void) {
 
 static __INLINE void motor_encoder_start(bool start) {
 #ifdef USE_ENCODER_HALL
-	hall_sensor_clear(direction);
+	hall_init();
 #elif defined (USE_ENCODER_ABI)
 	encoder_init_clear();
 #else
@@ -227,6 +232,17 @@ static __INLINE void motor_encoder_start(bool start) {
 #endif
 }
 
+static __INLINE void motor_encoder_reinit(void) {
+#ifdef USE_ENCODER_HALL
+	hall_init();
+#elif defined (USE_ENCODER_ABI)
+	//do nothing
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+
 static __INLINE float motor_encoder_zero_phase_detect(float *enc_off){
 #ifdef USE_ENCODER_HALL
 	return 0.0f;
@@ -241,7 +257,7 @@ static __INLINE void motor_encoder_set_direction(s8 dir) {
 #ifdef USE_ENCODER_HALL
 	hall_set_direction(dir);
 #elif defined (USE_ENCODER_ABI)
-	encoder_set_direction(dir);
+	//encoder_set_direction(dir);
 #else
 	#error "Postion sensor ERROR"
 #endif