MC100/Applications/foc/motor/motor.c

#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/foc_config.h"
#include "foc/mc_error.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/bsp_driver.h"
#include "libs/time_measure.h"
#include "foc/commands.h"
#include "libs/logger.h"
#include "foc/samples.h"
#include "foc/motor/motor_param.h"
#include "foc/core/foc_observer.h"
#include "foc/core/F_Calc.h"
#include "foc/core/etcs.h"
#include "app/nv_storage.h"
#include "app/factory.h"
#include "foc/motor/mot_params_ind.h"
#include "foc/motor/throttle.h"
#include "foc/limit.h"

extern u32 enc_pwm_err_ms;
extern s16 enc_delta_err1, enc_delta_err2;

static bool mc_detect_hwbrake(void);
static bool mc_is_gpio_mlock(void);
static void _pwm_brake_prot_timer_handler(shark_timer_t *);
static shark_timer_t _brake_prot_timer = TIMER_INIT(_brake_prot_timer, _pwm_brake_prot_timer_handler);
static void _fan_det_timer_handler(shark_timer_t *);
static shark_timer_t _fan_det_timer1 = TIMER_INIT(_fan_det_timer1, _fan_det_timer_handler);
static shark_timer_t _fan_det_timer2 = TIMER_INIT(_fan_det_timer2, _fan_det_timer_handler);
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);
motor_t motor = {
	.s_direction = POSITIVE,
	.n_gear = 0,
	.b_high_vol_mode = false,
	.mode = CTRL_MODE_OPEN,
	.mos_lim = 0,
	.motor_lim = 0,
	.b_ind_start = false,
	.s_target_speed = MAX_S16,
	.s_force_torque = MAX_S16,
	.u_set.idc_lim = IDC_USER_LIMIT_NONE,
	.u_set.rpm_lim = RPM_USER_LIMIT_NONE,
	.u_set.ebrk_lvl = 0,
	.u_set.n_brkShutPower = CONFIG_Settings_BrkShutPower,
	.u_set.b_tcs = 0xFF,
};
/* 无感运行的挡位，限制速度，母线电流，最大扭矩 */
static gear_t sensorless_gear = {
	.max_speed = CONFIG_SENSORLESS_MAX_SPEED,
	.max_torque = CONFIG_SENSORLESS_MAX_TORQUE,
	.max_idc = CONFIG_SENSORLESS_MAX_IDC,
	.zero_accl = 1500,
	.accl_time = 1500,
	.torque = {100, 80, 60, 50, 40, 30, 0, 0, 0, 0},
};
static runtime_node_t mc_error;

static void MC_Check_MosVbusThrottle(void) {
	int count = 1000;
	float ibus_adc = 0;
	float vref_adc = 0;
	float vref_5v_adc = 0;
	gpio_phase_u_detect(true);
	while(count-- > 0) {
		task_udelay(20);
		sample_uvw_phase();
		sample_throttle();
		sample_vbus();
		vref_adc += adc_get_vref();
		vref_5v_adc += adc_get_5v_ref();
	}

	adc_set_vref_calc(vref_adc/1000.0f);
	adc_set_5vref_calc(vref_5v_adc/1000.0f);
	sys_debug("5v ref %f\n", vref_5v_adc/1000.0f);
	count = 50;
	while(count-- >0) {
		task_udelay(300);
		ibus_adc += adc_get_ibus();
	}

	u16 offset = ((float)ibus_adc)/50.0f;
	sys_debug("ibus offset %d\n", offset);
	sample_ibus_offset(offset);

	if (factory_is_running()) {
		return;
	}

	gpio_phase_u_detect(false);
	float abc[3];
	get_phase_vols_filtered(abc);
	int vbus_vol = sample_vbus_raw();
	if (vbus_vol > mc_conf()->c.max_dc_vol) {
		mc_set_critical_error(FOC_CRIT_OV_Vol_Err);
	}
	if (vbus_vol <= mc_conf()->c.min_dc_vol) {
		mc_set_critical_error(FOC_CRIT_UN_Vol_Err);
	}

	vbus_vol = sample_acc_vol_raw();
	sys_debug("acc vol %d\n", vbus_vol);
	if (vbus_vol >= mc_conf()->c.max_dc_vol) {
		mc_set_critical_error(FOC_CRIT_ACC_OV_Err);
	}
	if (vbus_vol <= mc_conf()->c.min_dc_vol) {
		mc_set_critical_error(FOC_CRIT_ACC_Un_Err);
	}
	if (throttle_is_all_error()) {
		if (!motor.b_ignor_throttle) {
			mc_set_critical_error(FOC_CRIT_THRO_Err);
			mc_set_critical_error(FOC_CRIT_THRO2_Err);
		}
	}
	if (abc[0] > vbus_vol/2 || abc[1] > vbus_vol/2 || abc[2] > vbus_vol/2) {
		mc_set_critical_error(FOC_CRIT_H_MOS_Err);
	}else if (abc[0] < 0.001f){
		mc_set_critical_error(FOC_CRIT_L_MOS_Err);
	}else if ((abc[0] > 0.5f) && (abc[1] < 0.001f || abc[2] < 0.001f)) {
		mc_set_critical_error(FOC_CRIT_Phase_Conn_Err);
	}
	sys_debug("phase vol %f, %f, %f\n", abc[0], abc[1], abc[2]);
}

static int hw_brk_err_cnt = 0;
static int hw_brk_no_err_cnt = 0;
static u32 _self_check_task(void *p) {
	if (factory_is_running()) {
		return 1000;
	}
	if (get_tick_ms() < 500) { //启动500ms内检测刹车状态是否正常，应该是没有刹车
		if (mc_detect_hwbrake() && hw_brk_err_cnt++ >= 60) {
			mc_set_critical_error(FOC_CRIT_BRK_Err);
		}
	}else {
		if (mc_critical_err_is_set(FOC_CRIT_BRK_Err) && !mc_detect_hwbrake()) {
			if (hw_brk_no_err_cnt++ >= 100) {
				mc_clr_critical_error(FOC_CRIT_BRK_Err);
			}
		}else {
			hw_brk_no_err_cnt = 0;
		}
	}
#ifndef CONFIG_USE_ENCODER_HALL
	if (ENC_Check_error()) {
		if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
			mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, -1);
		}
	}
#endif
	if (get_tick_ms() < 5000) { //启动后5s内检测锁电机线
		if (mc_is_gpio_mlock()) {
			mc_lock_motor(true);
		}
	}
	if (motor.b_lock_motor) {
		if (!mc_is_gpio_mlock()) {
			mc_lock_motor(false);
		}
	}
	if (!motor.b_ignor_throttle) {
		if (throttle_is_all_error()) {
			mc_set_critical_error(FOC_CRIT_THRO_Err);
			mc_set_critical_error(FOC_CRIT_THRO2_Err);
		}else if (throttle1_is_error()) {
			mc_set_critical_error(FOC_CRIT_THRO_Err);
		}else if (throttle2_is_error()) {
			mc_set_critical_error(FOC_CRIT_THRO2_Err);
		}
	}
	if (fan_pwm_is_running()) {
	#ifdef GPIO_FAN1_IN_GROUP
		if ((get_delta_ms(motor.fan[0].start_ts) >= 5000) && (motor.fan[0].rpm == 0)) {
			mc_set_critical_error(FOC_CRIT_Fan_Err);
		}else if (motor.fan[0].rpm > 0) {
			mc_clr_critical_error(FOC_CRIT_Fan_Err);
		}
	#endif
	}
	return 5;
}

static bool mc_detect_vbus_mode(void) {
#ifdef CONFIG_FORCE_HIGH_VOL_MODE
	motor.b_high_vol_mode = true;
	return false;
#else
	bool is_96mode = motor.b_high_vol_mode;
	motor.b_high_vol_mode = get_vbus_int() >= CONFIG_HIGH_VOL_MODE_MIN_VOL;
	return (is_96mode != motor.b_high_vol_mode);
#endif
}

static void mc_internal_init(u8 mode, bool start) {
	motor.mode = mode;
	motor.throttle = 0;
	motor.b_start = start;
	motor.b_runStall = false;
	motor.runStall_time = 0;
	motor.b_epm = false;
	motor.b_epm_cmd_move = false;
	motor.epm_dir = EPM_Dir_None;
	motor.n_autohold_time = 0;
	motor.b_auto_hold = 0;
	motor.b_break = false;
	motor.b_wait_brk_release = false;
	motor.b_force_run = false;
	motor.b_cruise = false;
	motor.b_limit_pending = false;
	motor.f_epm_trq = 0;
	motor.f_epm_vel = 0;
	motor.vbus_le_cnt = motor.vbus_he_cnt = 0;
	motor.s_vbus_hw_min = mc_conf()->c.min_dc_vol;
	motor.s_vbus_hw_max = mc_conf()->c.max_dc_vol;
}

static void _led_off_timer_handler(shark_timer_t *t) {
	gpio_led_enable(false);
}


static void mc_gear_mode_set(void) {
	gear_t *gears = mc_gear_conf();
	if (gears != &sensorless_gear) {
		sensorless_gear.max_torque = gears->max_torque;
	}else { //slowly changed
		line_ramp_set_time(&motor.controller.ramp_vel_lim, CONFIG_SENSORLESS_RAMP_TIME);
		line_ramp_set_time(&motor.controller.ramp_dc_curr_lim, CONFIG_SENSORLESS_RAMP_TIME);
	}
	mot_contrl_set_vel_limit(&motor.controller, (float)min(gears->max_speed, motor.u_set.rpm_lim));
	mot_contrl_set_dccurr_limit(&motor.controller, (float)min(gears->max_idc, motor.u_set.idc_lim));
	mot_contrl_set_torque_limit(&motor.controller, (float)gears->max_torque);
}

void mc_init(void) {
	mc_internal_init(CTRL_MODE_OPEN, false);
	fan_pwm_init();
	adc_init(false);
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	throttle_init();
	mc_detect_vbus_mode();
	mot_contrl_init(&motor.controller);
	mc_gpio_init();
	MC_Check_MosVbusThrottle();
	sched_timer_enable(CONFIG_SPD_CTRL_US);
	shark_task_create(_self_check_task, NULL);
	pwm_up_enable(true);
	gpio_led_enable(true);
	shark_timer_post(&_led_off_timer, 5000);
}

motor_t * get_motor(void) {
	return &motor;
}

gear_t *mc_gear_conf_by_gear(u8 n_gear) {
	gear_t *gears;
	if (!foc_observer_is_encoder()) { //无感模式，受限运行
		return &sensorless_gear;
	}
	if (motor.b_high_vol_mode) {
		gears = &mc_conf()->g_n[0];
	}else {
		gears = &mc_conf()->g_l[0];
	}
	return &gears[n_gear];
}

gear_t *mc_gear_conf(void) {
	return mc_gear_conf_by_gear(motor.n_gear);
}

static __INLINE float gear_rpm_2_torque(u8 torque, s16 max) {
	return (float)torque/100.0f * max;
}

float mc_gear_max_torque(s16 vel, u8 gear_n) {
	gear_t *gear = mc_gear_conf_by_gear(gear_n);
	if (gear == NULL) {
		return 0;
	}

	vel = ABS(vel);
	if (vel <= 1000) {
		return gear_rpm_2_torque(gear->torque[0], gear->max_torque);
	}
	int vel_idx = vel / 1000;
	if (vel_idx >= CONFIG_GEAR_SPEED_TRQ_NUM -1 ) {
		return gear_rpm_2_torque(gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], gear->max_torque);
	}
	float torque_1 = gear_rpm_2_torque(gear->torque[vel_idx-1], gear->max_torque);
	float min_rpm = vel_idx * 1000;
	float torque_2 = gear_rpm_2_torque(gear->torque[vel_idx], gear->max_torque);
	float max_rpm = min_rpm + 1000;
	return f_map((float)vel, min_rpm, max_rpm, torque_1, torque_2);
}

float mc_get_max_torque_now(void) {
	return mc_gear_max_torque(motor.controller.foc.mot_velocity_filterd, motor.n_gear);
}
/* 必须立即停机 */
bool mc_critical_need_stop(void) {
	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Phase_Err) |
				FOC_Cri_Err_Mask(FOC_CRIT_Vol_HW_Err) | FOC_Cri_Err_Mask(FOC_CRIT_OV_Vol_Err);
	u32 err = motor.n_CritiCalErrMask & mask;
	return (err != 0);
}

bool mc_critical_can_not_run(void) {
	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Lim) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Lim);
	u32 err = motor.n_CritiCalErrMask & mask;
	bool crit_err = (err != 0) || mc_critical_need_stop();
	if (!motor.b_ignor_throttle) {
		crit_err = crit_err || throttle_is_all_error();
	}
	return crit_err;
}

bool mc_unsafe_critical_error(void) {
	u32 err = motor.n_CritiCalErrMask & (~(FOC_Cri_Err_Mask(FOC_CRIT_Fan_Err)));
#ifdef CONFIG_DQ_STEP_RESPONSE
	sys_debug("err=0x%x\n", err);
	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_Encoder_Err)));
	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
	sys_debug("err=0x%x\n", err);
#else
	if (!throttle_is_all_error()) {//不是两个转把都有问题，忽略出问题的转把错误
		if (throttle1_is_error()) {
			err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
		}else if (throttle2_is_error()) {
			err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
		}
	}
#endif
	if (motor.b_ignor_throttle) {
		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
	}
	return (err != 0);
}

bool mc_start(u8 mode) {
	if (motor.b_start) {
		return true;
	}
#ifdef CONFIG_DQ_STEP_RESPONSE
	mode = CTRL_MODE_CURRENT;
#endif
	motor.s_force_torque = MAX_S16;
	mc_detect_vbus_mode();
	etcs_enable(&motor.controller.etcs, mc_tcs_is_enabled());
	if (motor.b_lock_motor) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}
	MC_Check_MosVbusThrottle();

	if (mc_unsafe_critical_error()) {
		mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
		return false;
	}
	if (mode > CTRL_MODE_CURRENT) {
		mot_contrl_set_error(&motor.controller, FOC_Param_Err);
		return false;
	}
	if (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
		return false;
	}
	
	pwm_up_enable(false);

	u32 mask = cpu_enter_critical();

	mc_internal_init(mode, true);
	throttle_torque_reset();
	motor_encoder_start(true);
	mot_contrl_start(&motor.controller, mode);
	mot_contrl_set_torque_ramp_time(&motor.controller, mc_gear_conf()->zero_accl, mc_conf()->c.thro_dec_time);
	mc_set_ebrk_level(motor.u_set.ebrk_lvl);
	mc_gear_mode_set();
	cpu_exit_critical(mask);

	pwm_turn_on_low_side();
	delay_ms(10);
	phase_current_offset_calibrate();
	pwm_start();
	delay_us(10); //wait for ebrake error
	if (mc_unsafe_critical_error()) {
		mc_stop();
		return false;
	}
	adc_start_convert();
	phase_current_calibrate_wait();
	if (phase_curr_offset_check()) {
		mot_contrl_set_error(&motor.controller, FOC_CRIT_CURR_OFF_Err);
		mc_stop();
		return false;
	}
	if (mc_detect_hwbrake()) {
		mot_contrl_set_hw_brake(&motor.controller, true);
	}

	return true;
}

bool mc_stop(void) {
	if (!motor.b_start) {
		return true;
	}

	if (motor.b_lock_motor) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}

	if (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
		return false;
	}

	u32 mask = cpu_enter_critical();
	mc_internal_init(CTRL_MODE_OPEN, false);
	adc_stop_convert();
	pwm_stop();
	mot_contrl_stop(&motor.controller);
	motor_encoder_start(false);
	pwm_up_enable(true);
	cpu_exit_critical(mask);

	return true;
}

void mc_set_mos_lim_level(u8 l) {
	if (motor.mos_lim != l) {
		mc_crit_err_add(FOC_EV_MOS_Limit_L, l, get_mos_temp_raw());
	}
	motor.mos_lim = l;
}

void mc_set_motor_lim_level(u8 l) {
	if (motor.motor_lim != l) {
		mc_crit_err_add(FOC_EV_MOT_Limit_L, l, get_motor_temp_raw());
	}
	motor.motor_lim = l;
}

bool mc_set_gear(u8 gear) {
	if (gear >= CONFIG_MAX_GEARS) {
		mot_contrl_set_error(&motor.controller, FOC_Param_Err);
		return false;
	}
	if (motor.n_gear != gear) {
#if TURBO_GEAR_AUTO_EXIT_TIME>0
		if (gear == TURBO_GEAR) {
			if (motor.mos_lim != 0 || motor.motor_lim != 0) {
				mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
				return false;
			}
			if (get_motor_temp_raw() >= TURBO_MIN_MOTOR_TEMP || get_mos_temp_raw() >= TURBO_MIN_MOS_TEMP) {
				mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
				return false;
			}
			motor.gear_last = motor.n_gear;
			motor.turbo_time = get_tick_ms();
		}
#endif
		u32 mask = cpu_enter_critical();
		motor.n_gear = gear;
		mc_gear_mode_set();
		cpu_exit_critical(mask);
	}
	return true;
}


static void mc_turbo_auto_exit(void) {
#if TURBO_GEAR_AUTO_EXIT_TIME>0
	if (motor.n_gear != TURBO_GEAR) {
		return;
	}
	if (motor.mos_lim != 0 || motor.motor_lim != 0) {
		mc_set_gear(motor.gear_last);
		return;
	}
	u32 delta_time = get_delta_ms(motor.turbo_time);
	if (delta_time >= TURBO_GEAR_AUTO_EXIT_TIME) {
		motor.turbo_remain_sec = 0;
		mc_set_gear(motor.gear_last);
		return;
	}else {
		motor.turbo_remain_sec = (TURBO_GEAR_AUTO_EXIT_TIME - delta_time) / 1000;
	}
#endif
}

u8 mc_get_gear(void) {
	if (motor.n_gear == 3){
		return 0;
	}
	return motor.n_gear + 1;
}

u8 mc_get_internal_gear(void) {
	return motor.n_gear;
}

bool mc_tcs_is_enabled(void) {
	bool tcs_enabled = mc_conf()->s.tcs_enable;
	if (motor.u_set.b_tcs == 0) {
		tcs_enabled = false;
	}else if (motor.u_set.b_tcs == 1) {
		tcs_enabled = true;
	}
	return tcs_enabled;
}

bool mc_hwbrk_can_shutpower(void) {
	if (motor.u_set.n_brkShutPower != MAX_U8) {
		return (motor.u_set.n_brkShutPower != 0);
	}
	return (mc_conf()->s.brk_shut_power != 0);
}

bool mc_enable_cruise(bool enable) {
	if (enable == motor.b_cruise) {
		return true;
	}
	if (mot_contrl_set_cruise(&motor.controller, enable)) {
		motor.b_cruise = enable;
		motor.cruise_time = enable?shark_get_seconds():0;
		motor.cruise_torque = 0.0f;
		return true;
	}
	return false;
}

bool mc_set_target_vel(s16 vel) {
	if (motor.mode != CTRL_MODE_SPD || mc_is_epm()) {
		return false;
	}
	motor.s_target_speed = vel;
	return true;
}

bool mc_set_force_torque(s16 torque) {
	if (motor.mode != CTRL_MODE_TRQ) {
		return false;
	}
	if (torque == MAX_S16) {
		motor.s_force_torque = MAX_S16;
	}else {
		motor.s_force_torque = fclamp(torque, -mc_gear_conf()->max_torque, mc_gear_conf()->max_torque);
	}
	return true;
}

bool mc_is_cruise_enabled(void) {
	return motor.b_cruise;
}

bool mc_set_cruise_speed(bool rpm_abs, float target_rpm) {
	bool ret;
	if (rpm_abs) {
		ret = mot_contrl_set_cruise_speed(&motor.controller, target_rpm);
	}else {
		ret = mot_contrl_set_cruise_speed(&motor.controller, mot_contrl_get_speed(&motor.controller) + target_rpm);
	}
	if (ret) {
		motor.cruise_time = shark_get_seconds();
		motor.cruise_torque = 0.0f;
	}
	return ret;
}

void mc_set_idc_limit(s16 limit) {
	if ((limit == motor.u_set.idc_lim) || (limit < 0)) {
		return;
	}
	motor.u_set.idc_lim = limit;
	mc_gear_mode_set();
}

void mc_set_rpm_limit(s16 limit) {
	if ((limit == motor.u_set.rpm_lim) || (limit < 0)) {
		return;
	}
	motor.u_set.rpm_lim = limit;
	mc_gear_mode_set();
}


bool mc_set_ebrk_level(u8 level) {
	if (level < CONFIG_EBRK_LVL_NUM) {
		motor.u_set.ebrk_lvl = level;
		mot_contrl_set_ebrk_time(&motor.controller, mc_get_ebrk_time());
		mot_contrl_set_ebrk_torquer(&motor.controller, mc_get_ebrk_torque());
		return true;
	}
	return false;
}

void mc_enable_brkshutpower(u8 shut) {
	motor.u_set.n_brkShutPower = shut;
}

void mc_enable_tcs(bool enable) {
	motor.u_set.b_tcs = enable?1:0;
	etcs_enable(&motor.controller.etcs, enable);
}

s16 mc_get_ebrk_torque(void) {
	if (motor.u_set.ebrk_lvl < CONFIG_EBRK_LVL_NUM) {
		return min(mc_conf()->e_r[motor.u_set.ebrk_lvl].torque, mc_conf()->c.max_ebrk_torque);
	}
	return 0;
}

u16 mc_get_ebrk_time(void) {
	if (motor.u_set.ebrk_lvl < CONFIG_EBRK_LVL_NUM) {
		return (u16)mc_conf()->e_r[motor.u_set.ebrk_lvl].time;
	}
	return 0xFFFF;
}

bool mc_set_ctrl_mode(u8 mode) {
	if (mode == motor.mode) {
		return true;
	}
	if (!motor.b_start && !motor.b_ind_start) {
		return false;
	}
	if (mc_critical_can_not_run()) {
		return false;
	}
	if ((mode == CTRL_MODE_OPEN) && (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM)) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}

	u32 mask = cpu_enter_critical();
	bool ret = false;
	if (mot_contrl_request_mode(&motor.controller, mode)) {
		motor.mode = mode;
		if ((mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) && !mot_contrl_is_start(&motor.controller)) {
			mot_contrl_start(&motor.controller, motor.mode);
			pwm_enable_channel();
		}
		ret = true;
	}
	cpu_exit_critical(mask);
	return ret;
}

bool mc_start_epm(bool epm) {
	if (motor.b_epm == epm) {
		return true;
	}
	if (!motor.b_start) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}
	if (motor_encoder_get_speed() > CONFIG_ZERO_SPEED_RPM) {
		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
		return false;
	}
	u32 mask = cpu_enter_critical();
	motor.b_epm = epm;
	motor.f_epm_vel = 0.0f;
	motor.f_epm_trq = 0.0f;
	motor_encoder_band_epm(epm);
	if (epm) {
		mot_contrl_set_target_vel(&motor.controller, 0);
		motor.mode = CTRL_MODE_SPD;
		motor.epm_dir = EPM_Dir_None;
		mot_contrl_set_torque_limit(&motor.controller, mc_conf()->c.max_epm_torque);
		mot_contrl_request_mode(&motor.controller, CTRL_MODE_SPD);
	}else {
		motor.epm_dir = EPM_Dir_None;
		motor.mode = CTRL_MODE_TRQ;
		motor.b_epm_cmd_move = false;
		mot_contrl_request_mode(&motor.controller, CTRL_MODE_TRQ);
		mot_contrl_set_torque_limit_rttime(&motor.controller, CONFIG_LIMIT_RAMP_TIME);
		mot_contrl_set_vel_rttime(&motor.controller, CONFIG_CRUISE_RAMP_TIME);
		mc_gear_mode_set();
	}
	cpu_exit_critical(mask);
	
	return false;
}

bool mc_is_epm(void) {
	return motor.b_epm;
}

bool mc_is_start(void) {
	return (motor.b_start || mot_contrl_is_start(&motor.controller));
}

bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
	if (!motor.b_epm || !motor.b_start) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}
	if ((dir == motor.epm_dir) && (is_command == motor.b_epm_cmd_move)) {
		return true;
	}
	u32 mask = cpu_enter_critical();
	if (motor.epm_dir != dir) {
		motor.f_epm_vel = 0.0f;
		motor.f_epm_trq = 0.0f;
	}
	motor.epm_dir = dir;
	if (dir != EPM_Dir_None) {
		motor.b_epm_cmd_move = is_command;
		
		if (!mot_contrl_is_start(&motor.controller)) {
			mot_contrl_start(&motor.controller, motor.mode);
			mc_gear_mode_set();
			pwm_enable_channel();
		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
			mc_auto_hold(false);
		}
		mot_contrl_velloop_params(&motor.controller, mc_conf()->c.epm_pid.kp, mc_conf()->c.epm_pid.ki);
		mot_contrl_set_torque_limit_rttime(&motor.controller, 2.0);
		mot_contrl_set_torque_limit(&motor.controller, motor.f_epm_trq);
		mot_contrl_set_vel_rttime(&motor.controller, 1000.0f);
		mot_contrl_set_target_vel(&motor.controller, motor.f_epm_vel);
	}else {
		motor.b_epm_cmd_move = false;
		mot_contrl_velloop_params(&motor.controller, mc_conf()->c.pid[PID_Vel_ID].kp, mc_conf()->c.pid[PID_Vel_ID].ki);
		mot_contrl_set_target_vel(&motor.controller, 0);
	}
	cpu_exit_critical(mask);
	return true;
}

void mc_set_fan_duty(u8 duty) {
	sys_debug("fan duty %d\n", duty);
	if (!fan_pwm_is_running() && duty > 0) {
		motor.fan[0].start_ts = get_tick_ms();
		motor.fan[1].start_ts = get_tick_ms();
		shark_timer_post(&_fan_det_timer1, 5000);
		shark_timer_post(&_fan_det_timer2, 5000);
	}else if (duty == 0) {
		shark_timer_cancel(&_fan_det_timer1);
		shark_timer_cancel(&_fan_det_timer2);
	}
	fan_set_duty(duty);
}

bool mc_command_epm_move(epm_dir_t dir) {
	return mc_start_epm_move(dir, true);
}

bool mc_throttle_epm_move(epm_dir_t dir) {
	return mc_start_epm_move(dir, false);
}

void mc_set_throttle_r(bool use, u8 r) {
	motor.u_throttle_ration = r;
	motor.b_ignor_throttle = use;
	if (motor.b_ignor_throttle) {
		mc_clr_critical_error(FOC_CRIT_THRO_Err);
		mc_clr_critical_error(FOC_CRIT_THRO2_Err);
	}
}

void mc_use_throttle(void) {
	motor.b_ignor_throttle = false;
}

void mc_get_running_status(u8 *data) {
	data[0] = motor.mode;
	data[0] |= (mot_contrl_is_auto_holdding(&motor.controller)?1:0) << 2;
	data[0] |= (motor.b_break?1:0) << 3;
	data[0] |= (motor.b_cruise?1:0) << 4;
	data[0] |= (motor.b_start?1:0) << 5;
	data[0] |= (mc_is_epm()?1:0) << 6;
	data[0] |= (motor.b_lock_motor) << 7; //motor locked
}

u16 mc_get_running_status2(void) {
	u16 data = 0;
	data = motor.b_start?1:0;
	data |= (motor.n_gear & 0x7) << 1;
	data |= (mot_contrl_is_auto_holdding(&motor.controller)?1:0) << 3;
	data |= (motor.b_break?1:0) << 4;
	data |= (motor.b_cruise?1:0) << 5;
	data |= (mc_is_epm()?1:0) << 6;
	data |= (motor.b_lock_motor) << 7; //motor locked
	data |= (mot_contrl_ebrk_is_running(&motor.controller)?1:0) << 8; //能量回收运行标志
	data |= ((motor.n_CritiCalErrMask != 0)?1:0) << 9;
	data |= (fan_pwm_is_running()?1:0) << 10; //风扇是否运行
	data |= (motor.b_runStall?1:0) << 11; //是否堵转
	data |= (mot_contrl_dccurr_is_protected(&motor.controller)?1:0) << 12; //是否欠压限制母线电流
	data |= (mot_contrl_torque_is_protected(&motor.controller)?1:0) << 13; //是否高温限扭矩
	data |= (etcs_is_running(&motor.controller.etcs)?1:0) << 14; //电子tcs是否正在工作
	data |= (throttle_not_released_err()?1:0) << 15;
	return data;
}


static float _force_angle = 0.0f;
static int   _force_wait = 2000;
static int   _force_invert = false;
/* 开环，强制给定电角度和DQ的电压 */
void mc_force_run_open(s16 vd, s16 vq, bool align) {
	if (motor.b_start || motor.b_force_run) {
		if (vd == 0 && vq == 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();
			pwm_stop();
			mot_contrl_stop(&motor.controller);
			pwm_up_enable(true);
			motor.b_force_run = false;
			motor.b_ignor_throttle = false;
		}
		return;
	}
	if (vd == 0 && vq == 0) {
		return;
	}
	motor.b_ignor_throttle = true;
	MC_Check_MosVbusThrottle();
	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);
	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);
	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;
	}else {
		_force_invert = false;
	}
	motor.b_force_run = true;
}

bool mc_ind_motor_start(bool start) {
	if (start == motor.b_ind_start) {
		return true;
	}
	if (start) {
		motor.b_ignor_throttle = true;
		MC_Check_MosVbusThrottle();
		if (mc_unsafe_critical_error() || mot_params_flux_pending()) {
			mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
			return false;
		}
		adc_init(true);
		task_udelay(500);
		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_vdq_immediate(&motor.controller, 0, 0);
		motor.b_ind_start = start;
	}else {
		u32 mask = cpu_enter_critical();
		motor.b_ind_start = start;
		mot_contrl_set_vdq(&motor.controller, 0, 0);
		cpu_exit_critical(mask);
		delay_us(500);
		wdog_reload();
		adc_stop_convert();
		pwm_stop();
		adc_init(false);
		mot_contrl_stop(&motor.controller);
		motor.mode = CTRL_MODE_OPEN;
		pwm_up_enable(true);
		motor.b_ignor_throttle = false;
	}
	
	return true;
}

static void _encoder_zero_off_timer_handler(shark_timer_t *t){
	if (!motor.b_calibrate) {
		return;
	}
	float enc_off = 0.0f;
	float phase = motor_encoder_zero_phase_detect(&enc_off);
	mot_contrl_set_vdq(&motor.controller, 0, 0);
	delay_ms(50);
	adc_stop_convert();
	pwm_stop();
	mot_contrl_stop(&motor.controller);
	mc_internal_init(CTRL_MODE_OPEN, false);
	motor.b_calibrate = false;
}

bool mc_encoder_zero_calibrate(s16 vd) {
	if (motor.b_calibrate) {
		if (vd == 0) {
			motor_encoder_clear_detect();
			shark_timer_cancel(&_encoder_zero_off_timer);
			mot_contrl_set_vdq(&motor.controller, 0, 0);
			delay_ms(500);
			adc_stop_convert();
			pwm_stop();
			mot_contrl_stop(&motor.controller);
			mc_internal_init(CTRL_MODE_OPEN, false);
			motor.b_calibrate = false;
			motor.b_ignor_throttle = false;
		}
		return true;
	}
	motor_encoder_clear_detect();
	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;
	}
	mc_internal_init(CTRL_MODE_OPEN, true);
	motor.b_calibrate = true;
	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, vd, 0);

	shark_timer_post(&_encoder_zero_off_timer, 6*1000);

	return true;
}


bool mc_current_sensor_calibrate(float current) {
	if (!mc_start(CTRL_MODE_OPEN)) {
		return false;
	}
	phase_current_sensor_start_calibrate(current);
	phase_current_calibrate_wait();
	return true;
}

bool mc_lock_motor(bool lock) {
	if (motor.b_lock_motor == lock) {
		return true;
	}
	int ret = true;
	u32 mask = cpu_enter_critical();
	if (motor.b_start) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		ret = false;
		goto ml_ex_cri;
	}
	if (lock && (motor_encoder_get_speed() >= CONFIG_LOCK_MOTOR_MIN_RPM)) {
		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
		ret = false;
		goto ml_ex_cri;
	}
	motor.b_lock_motor = lock;
	if (lock) {
		pwm_start();
		pwm_update_duty(0, 0, 0);
		pwm_enable_channel();
	}else {
		pwm_stop();
	}
ml_ex_cri:
	cpu_exit_critical(mask);
	return ret;
}

bool mc_auto_hold(bool hold) {
	if (motor.b_auto_hold == hold) {
		return true;
	}
	if (!mc_conf()->s.auto_hold) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}
	if (!motor.b_start) {
		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
		return false;
	}
	if (hold && !mc_throttle_released()) {
		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
		return false;
	}
	u32 mask = cpu_enter_critical();
	motor.b_auto_hold = hold;
	if (!mot_contrl_is_start(&motor.controller)) {
		mot_contrl_start(&motor.controller, motor.mode);
		mot_contrl_set_autohold(&motor.controller, hold);
		pwm_enable_channel();
	}else {
		mot_contrl_set_autohold(&motor.controller, hold);
	}
	cpu_exit_critical(mask);
	return true;
}


bool mc_set_critical_error(u8 err) {
	if (mc_critical_err_is_set(err)) {
		return false;
	}
	motor.n_CritiCalErrMask |= (1u << err);

	return true;
}

void mc_clr_critical_error(u8 err) {
	motor.n_CritiCalErrMask &= ~(1u << err);
}

bool mc_critical_err_is_set(u8 err) {
	u32 mask = (1u << err);
	return (motor.n_CritiCalErrMask & mask) != 0;
}

u32 mc_get_critical_error(void) {
	return motor.n_CritiCalErrMask;
}

bool mc_throttle_released(void) {
	if (motor.s_force_torque != MAX_S16) {
		return motor.s_force_torque == 0;
	}
	if (motor.b_ignor_throttle) {
		return motor.u_throttle_ration == 0;
	}
	return throttle_is_released();
}

static bool mc_is_gpio_mlock(void) {
	int count = 50;
	int settimes = 0;
	while(count-- > 0) {
		bool b1 = gpio_motor_locked();
		if (b1) {
			settimes ++;
		}
		delay_us(1);
	}
	if (settimes == 0) {
		return false;
	}else if (settimes == 50) {
		return true;
	}	
	//有干扰，do nothing
	return false;
}

static bool _mc_is_hwbrake(void) {
	int count = 50;
	int settimes = 0;
	while(count-- > 0) {
		bool b1 = mc_get_gpio_brake() || mc_get_gpio_brake1();
		if (b1) {
			settimes ++;
		}
		delay_us(1);
	}
	if (settimes == 0) {
#if GPIO_BREAK_MODE==GPIO_LOW_BRK_MODE
		return true;
#else
		return false;
#endif
	}else if (settimes == 50) {
#if GPIO_BREAK_MODE==GPIO_LOW_BRK_MODE
		return false;
#else
		return true;
#endif
	}
	//有干扰，do nothing
	motor.n_brake_errors++;
	return false;
}

static bool mc_detect_hwbrake(void) {
	motor.b_break = _mc_is_hwbrake();
	return motor.b_break;
}

static void _fan_det_timer_handler(shark_timer_t *t) {
	if (t == &_fan_det_timer1) {
		motor.fan[0].rpm = 0;
		motor.fan[0].det_ts = 0;
	}else {
		motor.fan[1].rpm = 0;
		motor.fan[1].det_ts = 0;
	}
}

void Fan_IRQHandler(int idx) {
	fan_t *fan = motor.fan + idx;
	u32 pre_ts = fan->det_ts;
	u32 delta_ts = get_delta_ms(pre_ts);
	fan->det_ts = get_tick_ms();
	float rpm = 60.0f * 1000 / (float)delta_ts;
	
	LowPass_Filter(fan->rpm, rpm, 0.1f);
	if (idx == 0) {
		shark_timer_post(&_fan_det_timer1, 100);
	}else {
		shark_timer_post(&_fan_det_timer2, 100);
	}
}

void MC_Brake_IRQHandler(void) {
	mc_detect_hwbrake();

	if (!motor.b_start) {
		return;
	}
	if (motor.b_break) {
		mc_enable_cruise(false);
		mot_contrl_set_hw_brake(&motor.controller, true);
	}else {
		mot_contrl_set_hw_brake(&motor.controller, false);
	}
}

static void _pwm_brake_prot_timer_handler(shark_timer_t *t){
	pwm_brake_enable(true);
	sys_debug("MC protect error\n");
}

static void mc_save_err_runtime(void) {
	mc_error.vbus_x10 = (s16)(sample_vbus_raw() * 10.0f); 
	mc_error.ibus_x10 = (s16)(sample_ibus_raw() * 10.0f);
	mc_error.vacc_x10 = (s16) (sample_acc_vol_raw() * 10.0f);
	mc_error.id_ref_x10 = (s16)(motor.controller.foc.in.target_id.interpolation * 10.0f);
	mc_error.iq_ref_x10 = (s16)(motor.controller.foc.in.target_iq.interpolation * 10.0f);
	mc_error.id_x10 = (s16)(motor.controller.foc.out.curr_dq.d * 10.0f);
	mc_error.iq_x10 = (s16)(motor.controller.foc.out.curr_dq.q * 10.0f);
	mc_error.vd_x10 = (s16)(motor.controller.foc.out.vol_dq.d * 10.0f);
	mc_error.vq_x10 = (s16)(motor.controller.foc.out.vol_dq.q * 10.0f);
	mc_error.torque_ref_x10 = (s16)(motor.controller.target_torque * 10.0f);
	mc_error.run_mode = motor.controller.mode_running;
	mc_error.rpm = (s16)motor_encoder_get_speed();
	mc_error.b_sensorless = !foc_observer_is_encoder();
	mc_error.b_sensorless_stable = foc_observer_sensorless_stable();
	mc_error.mos_temp = get_mos_temp_raw();
	mc_error.mot_temp = get_motor_temp_raw();
	mc_error.enc_error = motor_encoder_may_error();
	mc_error.sensorless_rpm = (s16)foc_observer_sensorless_speed();

	mc_err_runtime_add(&mc_error);
}

void MC_Protect_IRQHandler(void){
	pwm_brake_enable(false);
	shark_timer_post(&_brake_prot_timer, 1000);
	if (!motor.b_start) {
		return;
	}
	mc_set_critical_error(FOC_CRIT_Phase_Err);
	mc_save_err_runtime();
	mc_internal_init(CTRL_MODE_OPEN, false);
	adc_stop_convert();
	pwm_stop();
	mot_contrl_stop(&motor.controller);
	pwm_up_enable(true);
}

void motor_debug(void) {
	if (!mc_unsafe_critical_error()) {
		return;
	}
	sys_debug("err1: %f, %f, %f, %d\n", (float)mc_error.vbus_x10/10.0f, (float)mc_error.id_ref_x10/10.0f, (float)mc_error.iq_ref_x10/10.0f, mc_error.run_mode);
	sys_debug("err2: %f, %f, %f, %f\n", (float)mc_error.id_x10/10.0f, (float)mc_error.iq_x10/10.0f, (float)mc_error.vd_x10/10.0f, (float)mc_error.vq_x10/10.0f);
	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.sensorless_rpm, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
}

static void motor_vbus_crit_check(s16 curr_vbus) {
	if (curr_vbus <= motor.s_vbus_hw_min) {
		motor.vbus_le_cnt ++;
		if (motor.vbus_le_cnt >= 2) {
			if (mot_contrl_is_start(&motor.controller)) {
				pwm_disable_channel();
				mc_save_err_runtime();
				mot_contrl_stop(&motor.controller);
			}
			if (mc_set_critical_error(FOC_CRIT_Vol_HW_Err)) {
				if (mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
					mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
				}
			}
		}
	}else {
		motor.vbus_le_cnt = 0;
	}
	if (curr_vbus >= motor.s_vbus_hw_max) {
		motor.vbus_he_cnt ++;
		if (motor.vbus_he_cnt >= 1) {
			if (mot_contrl_is_start(&motor.controller)) {
				pwm_disable_channel();
				mc_save_err_runtime();
				mot_contrl_stop(&motor.controller);
			}
			if (mc_set_critical_error(FOC_CRIT_OV_Vol_Err)) {
				mc_crit_err_add_s16(FOC_CRIT_OV_Vol_Err, curr_vbus);
			}
		}
	}else {
		motor.vbus_he_cnt = 0;
	}
}

void TIMER_UP_IRQHandler(void){
	if (!motor.b_start && !mot_contrl_is_start(&motor.controller)) {
		motor_encoder_update(false);
		motor_vbus_crit_check((s16)sample_vbus_raw());
	}
}

measure_time_t g_meas_foc = {.exec_max_time = 25, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
#if (CONFIG_ENABLE_IAB_REC==1)
#define CONFIG_IAB_REC_COUNT 1000
static s16 ia[CONFIG_IAB_REC_COUNT], ib[CONFIG_IAB_REC_COUNT];
static int iab_w_count = 0, iab_r_count = 0;
static bool b_iab_rec = false;
static bool b_vab_rec = false;
extern void can_plot2(s16 v1, s16 v2);
#endif
/*ADC 电流采集中断，调用FOC的核心处理函数*/
void ADC_IRQHandler(void) {
	if (phase_current_offset()) {//check if is adc offset checked
		return;
	}
	if (phase_current_sensor_do_calibrate()){
		pwm_update_duty(100, FOC_PWM_Half_Period-100, 100);
		pwm_update_sample(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1, PHASE_BC);
		return;
	}
	TIME_MEATURE_START();

#if (CONFIG_ENABLE_IAB_REC==1)
	if (b_iab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
		ia[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[0];
		ib[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[1];
		iab_w_count ++;
	}else if (b_vab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
		ia[iab_w_count] = (s16)motor.controller.phase_v_ab.a;
		ib[iab_w_count] = (s16)motor.controller.foc.out.vol_albeta.a*TWO_BY_THREE;
		iab_w_count ++;
	}
#endif
	motor_vbus_crit_check((s16)sample_vbus_raw()); //need fast detect vbus very low, to stop the motor
	float vd, vq;
	if (motor.b_ind_start) {
		mot_params_high_freq_inject();
#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==0
		vd = motor.controller.foc.out.vol_dq.d * TWO_BY_THREE;
		vq = motor.controller.foc.out.vol_dq.q * TWO_BY_THREE;
#endif
	}
	if (!mot_contrl_update(&motor.controller)) {/* FOC 角度错误，立即停机 */
		if (mot_contrl_is_start(&motor.controller)) {
			pwm_disable_channel();
			/* 记录错误 */
			if (!foc_observer_is_force_sensorless()) {
				mc_save_err_runtime();
			}
			mot_contrl_stop(&motor.controller);
			g_meas_foc.first = true;
			if (!foc_observer_is_force_sensorless()) {
				if (mc_set_critical_error(FOC_CRIT_Angle_Err)) {
					mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
				}
				if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
					if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
						mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
					}
				}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
					if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
						mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
					}
				}
			}
		}
	}
	if (motor.b_ind_start) {
		float id = motor.controller.foc.out.curr_dq.d;
		float iq = motor.controller.foc.out.curr_dq.q;
#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==1
		vd = motor.controller.phase_v_dq.d;
		vq = motor.controller.phase_v_dq.q;
#endif
		mot_params_hj_sample_vi(vd, vq, id, iq);
	}
	TIME_MEATURE_END();
}

#if (CONFIG_ENABLE_IAB_REC==1)
static void _iab_plot_timer_handler(shark_timer_t *t) {
	if (!b_iab_rec && !b_vab_rec) {
		return;
	}
	if (iab_r_count < iab_w_count) {
		can_plot2(ia[iab_r_count], ib[iab_r_count]);
		iab_r_count ++;
		shark_timer_post(t, 10);
	}
}
static shark_timer_t _iab_plot_timer = TIMER_INIT(_iab_plot_timer, _iab_plot_timer_handler);
void mc_start_current_rec(bool is_i, bool rec) {

	if (!rec) {
		b_iab_rec = false;
		b_vab_rec = false;
		shark_timer_cancel(&_iab_plot_timer);
		return;
	}

	if (b_iab_rec == true || b_vab_rec == true) {
		return;
	}

	iab_w_count = 0;
	iab_r_count = 0;
	if (is_i) {
		b_iab_rec = true;
	}else {
		b_vab_rec = true;
	}
	shark_timer_post(&_iab_plot_timer, 100);
}
#endif
static bool mc_run_stall_process(u8 run_mode) {
	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !mot_contrl_is_auto_holdding(&motor.controller)) {
		//堵转判断
		if (motor.b_runStall) {
			if (!mc_throttle_released()) {
				return true;
			}
			motor.runStall_time = 0;
			motor.b_runStall = false; //转把释放，清除堵转标志
		}else if (mot_contrl_get_current_vector(&motor.controller) >= CONFIG_STALL_MAX_CURRENT){
			if (mot_contrl_get_speed_abs(&motor.controller) < 1 && (motor.runStall_time == 0)) {
				motor.runStall_time = get_tick_ms();
				motor.runStall_pos = motor_encoder_get_position();
			}
			if (motor.runStall_time > 0) {
				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
					motor.b_runStall = true;
					motor.runStall_time = 0;
					mot_contrl_set_torque(&motor.controller, 0);
					throttle_torque_reset();
					return true;
				}
				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
					motor.runStall_time = 0;
				}
			}
		}else {
			motor.runStall_time = 0;
		}
	}
	return false;
}
static void mc_autohold_process(void) __attribute__((unused));
static void mc_autohold_process(void) {
	if (!mc_conf()->s.auto_hold) {
		if (mot_contrl_is_auto_holdding(&motor.controller)) {
			mc_auto_hold(false);
		}
		return;
	}
	if (mot_contrl_is_auto_holdding(&motor.controller)) {
		if (!mc_throttle_released()) {
			mc_auto_hold(false);
			motor.b_wait_brk_release = false;
		}else if (!motor.b_break && motor.b_wait_brk_release) {
			motor.b_wait_brk_release = false;
		}else if (motor.b_break && !motor.b_wait_brk_release) {
			mc_auto_hold(false);
		}
	}
	if (!mot_contrl_is_auto_holdding(&motor.controller) && motor.b_break && (motor_encoder_get_speed() == 0)) {
		if (motor.n_autohold_time == 0) {
			motor.n_autohold_time = get_tick_ms();
		}else {
			if (get_delta_ms(motor.n_autohold_time) >= CONFIG_AUTOHOLD_DETECT_TIME) {
				if (mc_auto_hold(true)) {
					motor.b_wait_brk_release = true;
				}
			}
		}
	}else {
		motor.n_autohold_time = 0;
	}
}
static void mc_process_throttle_epm(void) __attribute__((unused));
static void mc_process_throttle_epm(void) {
	if (!motor.b_epm_cmd_move) {//通过命令前进后退，不处理转把
		if (mc_throttle_released()) {
			mc_throttle_epm_move(EPM_Dir_None);
		}else {
			mc_throttle_epm_move(EPM_Dir_Forward);
		}
	}
}
static void mc_process_epm_move(void) __attribute__((unused));
static void mc_process_epm_move(void) {
	if (!motor.b_epm || (motor.epm_dir == EPM_Dir_None)){
		return;
	}
	float target_vel = mc_conf()->c.max_epm_rpm;
	float target_trq = mc_conf()->c.max_epm_torque;
	if (motor.epm_dir == EPM_Dir_Back) {
		target_vel = -mc_conf()->c.max_epm_back_rpm;
		target_trq = mc_conf()->c.max_epm_back_torque;
	}else if (!motor.b_epm_cmd_move) {
		target_vel = throttle_vol_to_opening(throttle_get_signal()) * 2.0f * (float)target_vel;
	}
	motor.f_epm_trq = target_trq;
	motor.f_epm_vel = target_vel;
	mot_contrl_set_torque_limit(&motor.controller, motor.f_epm_trq);
	mot_contrl_set_target_vel(&motor.controller, motor.f_epm_vel);
}

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;
			}else {
				float step = 2.5f;
				if (_force_invert) {
					step = -step;
				}
				_force_angle += step;
				norm_angle_deg(_force_angle);
				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;
}

static void mc_process_brake_light(void) {
	bool can_lighting = false;
	if (motor.b_break || motor.b_auto_hold || mot_contrl_ebrk_is_running(&motor.controller) || ((!mc_critical_can_not_run()) && motor_encoder_get_speed() > 2000)) {
		can_lighting = true;
	}
	gpio_brk_light_enable(can_lighting);
}

#ifdef CONFIG_CRUISE_ENABLE_ACCL
static void mc_process_curise(void) {
	static bool can_pause_resume = false;
	if (motor.b_cruise) {
		if (mot_contrl_get_speed_abs(&motor.controller) < CONFIG_CRUISE_EXIT_RPM) {
			mot_contrl_set_cruise(&motor.controller, false);
			return;
		}
		/* 定速巡航模式下，必须转把归位后才能开始通过拧动转把加速 */
		if (mc_throttle_released() && !can_pause_resume) {
			can_pause_resume = true;
		}
		if (!can_pause_resume) {
			return;
		}
		if (mot_contrl_is_cruise_enabled(&motor.controller)) {
			u32 cruise_time = shark_get_seconds() - motor.cruise_time;
			if ((cruise_time >= 3) && motor.cruise_torque == 0.0f) {
				motor.cruise_torque = motor.controller.target_torque;
			}else if ((cruise_time >= 3) && (motor.cruise_torque > 0.0f)){
				float trq_req = get_user_request_torque();
				if (trq_req > motor.cruise_torque * 1.2f) {
					mot_contrl_pause_cruise(&motor.controller); //需要加速,暂停定速巡航
				}
			}
		}else {
			float trq_req = get_user_request_torque();
			if (trq_req <= motor.cruise_torque * 1.1f) {
				mot_contrl_resume_cruise(&motor.controller); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
			}
		}
	}else {
		mot_contrl_set_cruise(&motor.controller, false);
		can_pause_resume = false;
	}
}
#endif

static bool mc_can_stop_foc(void) {
	if (mc_critical_need_stop()) {
		return true;
	}
	if (motor.mode == CTRL_MODE_CURRENT) {
		return false;
	}
	if (!motor.b_cruise && !motor.b_epm && motor.mode == CTRL_MODE_SPD) {
		if (motor.s_target_speed != MAX_S16 && motor.s_target_speed != 0) {
			return false;
		}else {
			return true;
		}
	}
	if (mc_throttle_released() && ((mot_contrl_get_speed_abs(&motor.controller)) == 0)) {
		if (!mot_contrl_is_auto_holdding(&motor.controller) && motor.epm_dir == EPM_Dir_None) {
			return true;
		}
	}
	/* 启用无感观测器，但是观测器未稳定，关闭输出，滑行 */
	if (!foc_observer_is_encoder() && !foc_observer_sensorless_stable()) {
		return true;
	}
	return false;
}

static bool mc_can_restart_foc(void) {
	if (mc_critical_can_not_run()) {
		return false;
	}
	bool can_start = (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None));
	if (!foc_observer_is_encoder() && !foc_observer_sensorless_stable()){
		return false;
	}
	if ((motor.s_target_speed != MAX_S16 && motor.s_target_speed != 0) && motor.mode == CTRL_MODE_SPD) {
		return true;
	}
	return can_start;
}
static void mc_motor_runstop(void) __attribute__((unused));
static void mc_motor_runstop(void) {
	u32 mask;
	if (mot_contrl_is_start(&motor.controller) && mc_can_stop_foc()) {
		mask = cpu_enter_critical();
		mot_contrl_stop(&motor.controller);
		pwm_disable_channel();
		g_meas_foc.first = true;
		motor.foc_stop_cnt ++;
		cpu_exit_critical(mask);
	}
	if (!mot_contrl_is_start(&motor.controller) && mc_can_restart_foc()) {
		mask = cpu_enter_critical();
		mot_contrl_start(&motor.controller, motor.mode);
		mc_gear_mode_set();
		throttle_torque_reset();
		pwm_enable_channel();
		g_meas_foc.first = true;
		motor.foc_start_cnt ++;
		cpu_exit_critical(mask);
	}
}

static void mc_process_throttle_torque(float vol) __attribute__((unused));
static void mc_process_throttle_torque(float vol) {
	float torque = throttle_get_torque(&motor.controller, vol);

	if (mc_throttle_released()) {
#ifdef CONFIG_CRUISE_ENABLE_ACCL
		if (mc_is_cruise_enabled()) {
			return;
		}
#endif
		if (!mot_contrl_ebrk_is_running(&motor.controller) && mot_contrl_set_ebreak(&motor.controller, true)) {
			return;
		}
	}

	if (motor.controller.mode_running == CTRL_MODE_TRQ) {
		throttle_set_torque(&motor.controller, torque);
	}else if (motor.controller.mode_running == CTRL_MODE_SPD) {
		if (!mc_is_cruise_enabled()) {
			float vel_ref = motor.controller.userlim.mot_vel * throttle_get_open_ration_filted();
			mot_contrl_set_target_vel(&motor.controller, vel_ref);
		}
	}else if (motor.controller.mode_running == CTRL_MODE_EBRAKE){
		/* 等待输入扭矩先减小到0 */
		if (line_ramp_get_interp(&motor.controller.ramp_input_torque) >= 0.001f) {
			return;
		}
		mot_contrl_set_ebrk_time(&motor.controller, motor.controller.ebrk_ramp_time);
		float vel = mot_contrl_get_speed(&motor.controller);
		float ebrk_trq = motor_get_ebreak_toruqe(vel);
		mot_contrl_set_torque(&motor.controller, ebrk_trq);
		if ((vel < CONFIG_MIN_RPM_EXIT_EBRAKE) || (!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f)))) {
			mot_contrl_set_ebreak(&motor.controller, false);
			throttle_torque_reset();
		}
	}
}
/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
measure_time_t g_meas_MCTask = {.exec_max_time = 100, .intval_max_time = 1000,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
#define mc_TaskStart time_measure_start(&g_meas_MCTask)
#define mc_TaskEnd time_measure_end(&g_meas_MCTask)
void Sched_MC_mTask(void) {
	static int vbus_err_cnt = 0;
	static bool _sensorless_run = false;
	mc_TaskStart;

	adc_vref_filter();

	throttle_detect(motor.b_start);

	F_all_Calc();

#ifdef CONFIG_CRUISE_ENABLE_ACCL
	mc_process_curise();
#endif
	u8 runMode = mot_contrl_mode(&motor.controller);
	/*保护功能*/
	u8 limted = mot_contrl_protect(&motor.controller);
	/* 母线电流，实际采集的相电流矢量大小的计算 */
	mot_contrl_calc_current(&motor.controller);

	if ((mot_contrl_get_dc_current(&motor.controller) > (CONFIG_HW_MAX_DC_CURRENT * 1.1f)) || (mot_contrl_get_dc_current(&motor.controller) < CONFIG_HW_MAX_CHRG_CURRENT)) {
		vbus_err_cnt ++;
		if (vbus_err_cnt >= 5) {
			if (mc_set_critical_error(FOC_CRIT_IDC_OV)) {
				mc_crit_err_add(FOC_CRIT_IDC_OV, (s16)sample_vbus_raw(), (s16)sample_ibus_raw());
				mc_save_err_runtime();
			}
		}
	}else {
		vbus_err_cnt = 0;
	}

	if (mc_process_force_running()) {
		mc_TaskEnd;
		return;
	}
	bool sensor_less = !foc_observer_is_encoder();
	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L) || (_sensorless_run != sensor_less)) {
		mc_gear_mode_set();
		if (sensor_less && foc_observer_sensorless_stable()) {//unstable 记录在ADC中断处理中
			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
				if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
					mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
				}
			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
				if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
					mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
				}
			}
		}
		motor.b_limit_pending = false;
	}else if (limted == FOC_LIM_CHANGE_H) {
		motor.b_limit_pending = true;
	}
	_sensorless_run = sensor_less;

	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
	if (motor.b_limit_pending && mc_throttle_released()) {
		motor.b_limit_pending = false;
		mc_gear_mode_set();
	}
	/* 堵转处理 */
	if (mc_run_stall_process(runMode) || (motor.mode == CTRL_MODE_CURRENT)) {
#ifndef CONFIG_DQ_STEP_RESPONSE
		mot_contrl_slow_task(&motor.controller);
		mc_motor_runstop();
#endif
		if (motor.b_ind_start) {
			mot_params_flux_stop();
		}
		mc_TaskEnd;
		return;
	}

	mc_process_brake_light();

	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
#ifndef CONFIG_DQ_STEP_RESPONSE
		mc_autohold_process();
		mc_turbo_auto_exit();
		if (motor.mode != CTRL_MODE_OPEN) {
			mc_motor_runstop();
		}
		if (runMode != CTRL_MODE_OPEN) {
			if (runMode == CTRL_MODE_SPD) {
				if (mc_is_epm()) {
					mc_process_throttle_epm();
					mc_process_epm_move();
				}else if (motor.s_target_speed != MAX_S16) {
					mot_contrl_set_target_vel(&motor.controller, motor.s_target_speed);
				}
			}else {
				if (motor.s_force_torque != MAX_S16) {
					mot_contrl_set_force_torque(&motor.controller, motor.s_force_torque);
				}else {
					float thro = throttle_get_signal();
					if (motor.b_ignor_throttle) {
						float r = (float)motor.u_throttle_ration/100.0f;
						thro = throttle_opening_to_vol(r);
					}
					mc_process_throttle_torque(thro);
				}
			}
			mot_contrl_slow_task(&motor.controller);
		}
#endif
	}
	mc_TaskEnd;
}