MC100/Applications/foc/core/controller.c

#include "controller.h"
#include "foc/mc_config.h"
#include "foc/foc_config.h"
#include "foc/core/foc_observer.h"
#include "foc/motor/motor_param.h"
#include "foc/motor/motor.h"
#include "foc/samples.h"
#include "foc/core/f_calc.h"
#include "foc/motor/current.h"
#include "foc/mc_error.h"

static void mot_contrl_pid(mot_contrl_t *ctrl);
static void mot_contrl_ulimit(mot_contrl_t *ctrl);
static void mot_contrl_rtlimit(mot_contrl_t *ctrl);
static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl);

void mot_contrl_init(mot_contrl_t *ctrl) {
	memset(ctrl, 0, sizeof(mot_contrl_t));
	ctrl->foc.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
	ctrl->foc.half_period = FOC_PWM_Half_Period;
	ctrl->force_angle = INVALID_ANGLE;
	ctrl->adv_angle = INVALID_ANGLE;
	ctrl->hwlim.dc_curr = CONFIG_HW_MAX_DC_CURRENT;
	ctrl->hwlim.mot_vel = CONFIG_HW_MAX_MOTOR_RPM;
	ctrl->hwlim.phase_curr = CONFIG_HW_MAX_PHASE_CURR;
	//ctrl->hwlim.phase_vol = CONFIG_HW_MAX_PHASE_VOL;
	//ctrl->hwlim.dc_vol      = CONFIG_HW_MAX_DC_VOLTAGE;
	ctrl->hwlim.torque  = mc_conf()->m.max_torque; //电机的最大扭矩
	ctrl->hwlim.fw_id = mc_conf()->m.max_fw_id;  //电机能支持的最大弱磁电流
	ctrl->protlim.dc_curr = HW_LIMIT_NONE;
	ctrl->protlim.torque = HW_LIMIT_NONE;
	ctrl->torque_acc_time = 500; //will be set after start
	ctrl->torque_dec_time = 500; //will be set after start
	ctrl->ebrk_ramp_time = 500;  //will be set after start
	etcs_init(&ctrl->etcs);
	foc_init(&ctrl->foc);
}
bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
	if (ctrl->b_start == start) {
		return true;
	}
	if (start) {
		line_ramp_init(&ctrl->ramp_torque_lim, CONFIG_LIMIT_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_vel_lim, CONFIG_LIMIT_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_cruise_vel, CONFIG_CRUISE_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_target_vd, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
		line_ramp_init(&ctrl->ramp_target_vq, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
		line_ramp_init(&ctrl->ramp_target_vel, CONFIG_CRUISE_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_target_current, CONFIG_CURRENT_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);
		line_ramp_init(&ctrl->ramp_adv_angle, CONFIG_CURRENT_RAMP_TIME);
		mot_contrl_ulimit(ctrl);
		mot_contrl_rtlimit(ctrl);
	}
	mot_contrl_pid(ctrl);
	ctrl->b_ebrk_running = false;
	ctrl->b_AutoHold = false;
	ctrl->b_cruiseEna = false;
	ctrl->b_mtpa_calibrate = false;
	ctrl->b_hw_braker = false;

	ctrl->mode_req = CTRL_MODE_OPEN;
	ctrl->mode_running = CTRL_MODE_OPEN;

	ctrl->force_angle = INVALID_ANGLE;
	ctrl->adv_angle = INVALID_ANGLE;
	ctrl->angle_last = INVALID_ANGLE;

	ctrl->dc_curr_filted = 0;
	ctrl->dc_curr_calc = 0;
	ctrl->phase_curr_filted[0] = ctrl->phase_curr_filted[1] = 0;
	ctrl->out_idq_filterd.d = ctrl->out_idq_filterd.q = 0;
	ctrl->out_vdq_filterd.d = ctrl->out_vdq_filterd.q = 0;
	ctrl->autohold_torque = 0;
	ctrl->out_current_vec = 0;
	ctrl->target_idq.d = 0;
	ctrl->target_idq.q = 0;
	ctrl->target_torque = 0;
	ctrl->target_torque_raw = 0;
	foc_init(&ctrl->foc);
	foc_observer_init();
	ctrl->b_start = 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);
		return false;
	}
	ctrl->mode_req = mode;
	return true;
}

u8 mot_contrl_mode(mot_contrl_t *ctrl) {	
	u8 preMode = ctrl->mode_running;

	if (!ctrl->b_start) {
		ctrl->mode_running = CTRL_MODE_OPEN;
	}else if (ctrl->mode_req == CTRL_MODE_OPEN) {
		ctrl->mode_running = CTRL_MODE_OPEN;
	}else if (ctrl->mode_req == CTRL_MODE_SPD || ctrl->b_cruiseEna){
		ctrl->mode_running = CTRL_MODE_SPD;
	}else if (ctrl->mode_req == CTRL_MODE_CURRENT) {
		ctrl->mode_running = CTRL_MODE_CURRENT;
	}else if (ctrl->mode_req == CTRL_MODE_EBRAKE) {
		ctrl->mode_running = CTRL_MODE_EBRAKE;
	}else {
		if (!ctrl->b_cruiseEna) {
			ctrl->mode_running = CTRL_MODE_TRQ;
		}
	}
	if (preMode != ctrl->mode_running) {
		if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_TRQ)) {
			line_ramp_set_acctime(&ctrl->ramp_input_torque, ctrl->torque_acc_time);
			line_ramp_set_dectime(&ctrl->ramp_input_torque, ctrl->torque_dec_time);
			line_ramp_update(&ctrl->ramp_input_torque);
			if (preMode == CTRL_MODE_SPD) {
				ctrl->target_torque_raw = ctrl->target_torque;
				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
			}else if (preMode == CTRL_MODE_CURRENT) {
				ctrl->target_torque_raw = line_ramp_get_interp(&ctrl->ramp_target_current);
				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
			}else if (preMode == CTRL_MODE_EBRAKE) {
				line_ramp_set_target(&ctrl->ramp_input_torque, 0);
			}
		}else if ((preMode == CTRL_MODE_TRQ) && (ctrl->mode_running == CTRL_MODE_SPD)) {
			PI_Controller_Reset(&ctrl->pi_vel, ctrl->target_torque);
		}else if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_EBRAKE)) {
			line_ramp_reset(&ctrl->ramp_input_torque, ctrl->target_torque);
			line_ramp_set_time(&ctrl->ramp_input_torque, ctrl->torque_dec_time);
			line_ramp_set_target(&ctrl->ramp_input_torque, 0);//先把扭矩快速减小到0
		}else if ((preMode == CTRL_MODE_EBRAKE) && (ctrl->mode_running == CTRL_MODE_SPD)) {
			PI_Controller_Reset(&ctrl->pi_vel, 0);
		}
	}
	if (ctrl->mode_running == CTRL_MODE_OPEN) {
		line_ramp_step(&ctrl->ramp_target_vd);
		line_ramp_step(&ctrl->ramp_target_vq);
	}
	return ctrl->mode_running;
}

static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
	static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
	static float a_max = 0, b_max = 0, c_max = 0;
	static u32 _unbalance_cnt = 0;
	static u32 _unbalance_time = 0;
	foc_t *foc = &ctrl->foc;
	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US / 2.0f;
	if (lowpass > 1.0f) {
		lowpass = 1.0f;
	}
	LowPass_Filter(ctrl->phase_curr_filted[0], foc->in.curr_abc[0], lowpass);
	LowPass_Filter(ctrl->phase_curr_filted[1], foc->in.curr_abc[1], lowpass);
	ctrl->phase_curr_filted[2] = -(ctrl->phase_curr_filted[0] + ctrl->phase_curr_filted[1]);
	if ((ctrl->angle_last == INVALID_ANGLE) || (foc->mot_vel_radusPers < 100) || ctrl->out_current_vec < 50) {
		ctrl->angle_last = foc->in.mot_angle;
		a_max = b_max = c_max = 0;
		_unbalance_cnt = 0;
		_unbalance_time = get_tick_ms();
		_cycle_cnt = 0;
		_last_mod_cnt = 0;
		return;
	}
	float delta_angle = foc->in.mot_angle - ctrl->angle_last;
	if (delta_angle > 200 || delta_angle < -200) { //one cycle
		_cycle_cnt ++;
	}
	ctrl->angle_last = foc->in.mot_angle;
	u32 mod_cnt = _cycle_cnt % CONFIG_PHASE_UNBALANCE_PEAK_CNT;
	bool trigger = false;
	if ((mod_cnt == 0) && (_last_mod_cnt != mod_cnt)) {
		trigger = true;
	}
	_last_mod_cnt = mod_cnt;

	a_max = MAX(a_max, ctrl->phase_curr_filted[0] * (2.2f));
	b_max = MAX(b_max, ctrl->phase_curr_filted[1] * (2.2f));
	c_max = MAX(c_max, ctrl->phase_curr_filted[2] * (2.2f));
	if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
		float i_min = 1000.0f, i_max = 0;
		if (a_max > i_max) {
			i_max = a_max;
		}
		if (a_max < i_min) {
			i_min = a_max;
		}
		if (b_max > i_max) {
			i_max = b_max;
		}
		if (b_max < i_min) {
			i_min = b_max;
		}
		if (c_max > i_max) {
			i_max = c_max;
		}
		if (c_max < i_min) {
			i_min = c_max;
		}
		float unbalance_r = (i_max - i_min - CONFIG_PHASE_UNBALANCE_THROLD)/(i_max + 1e-8f);
		if (unbalance_r >= CONFIG_PHASE_UNBALANCE_R) {
			if ((_unbalance_cnt++ >= 500) || (get_delta_ms(_unbalance_time) >= 1000*10)) {
				if (mc_set_critical_error(FOC_CRIT_PHASE_UNBalance_Err)) {
					mc_crit_err_add(FOC_CRIT_PHASE_UNBalance_Err, (s16)i_max, (s16)i_min);
				}
			}
		}else {
			_unbalance_cnt = 0;
			_unbalance_time = get_tick_ms();
		}
		a_max = b_max = c_max = 0;
	}
}

static __INLINE void mot_contrl_update_phase_vol(mot_contrl_t *ctrl) {
	float phase_vol[3];
	get_uvw_phases_raw(phase_vol);
	/* 三相端电压转到alpha-beta轴的相电压 */
	ctrl->phase_v_ab.a = (2 * phase_vol[0] - phase_vol[1] - phase_vol[2])/3.0f;
	ctrl->phase_v_ab.b = (phase_vol[1] - phase_vol[2]) * ONE_BY_SQRT3;
	/* 当前电气频率   除于相电压低通滤波器截止频率 */
	float We_hz = (ctrl->foc.in.mot_velocity / 60.0f * mc_conf()->m.poles);
	float w_r_wc = We_hz / PHASE_VOL_LPF_BAND;
	if (ctrl->mot_param_ind_freq > 10.0f) {//dq轴电感识别
		w_r_wc = (ctrl->mot_param_ind_freq / (2.0f * M_PI)) / PHASE_VOL_LPF_BAND;
	}
	/* 计算低通滤波器的幅度补偿系数*/
	float mag_mul = sqrtf(1 + SQ(w_r_wc));
	ctrl->phase_v_ab.a *= mag_mul;
	ctrl->phase_v_ab.b *= mag_mul;
	/* 计算低通相位延时 */
	float angle_rad = fast_atan2(We_hz, PHASE_VOL_LPF_BAND);
	/* 延时半周期*/
	angle_rad += (We_hz * 2 * M_PI * ctrl->foc.ts / 2);
	norm_angle_rad(angle_rad);
	/* 补偿 alpha-beta的相位 */
	float sin,cos;
	arm_sin_cos(-pi_2_degree(angle_rad), &sin, &cos);
	float alpha = ctrl->phase_v_ab.a * cos + ctrl->phase_v_ab.b * sin;
	float beta  = ctrl->phase_v_ab.b * cos - ctrl->phase_v_ab.a * sin;

	ctrl->phase_v_ab.a = alpha;
	ctrl->phase_v_ab.b = beta;
}

bool mot_contrl_update(mot_contrl_t *ctrl) {
	foc_t *foc = &ctrl->foc;

	mot_contrl_update_phase_vol(ctrl);

	phase_current_get(foc->in.curr_abc);
	clark(foc->in.curr_abc[0], foc->in.curr_abc[1], foc->in.curr_abc[2], &foc->in.curr_ab);

	foc_observer_update(foc->out.vol_albeta.a * TWO_BY_THREE, foc->out.vol_albeta.b * TWO_BY_THREE, foc->in.curr_ab.a, foc->in.curr_ab.b);

	float enc_angle = motor_encoder_get_angle();
	float enc_vel = motor_encoder_get_speed();

	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;
			return false;
		}
		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 {
		foc->in.mot_angle = enc_angle;
	}
#ifdef CONFIG_DQ_STEP_RESPONSE
	foc->in.mot_angle = 0;
#endif
	foc->in.mot_velocity = enc_vel;
	foc->in.dc_vol = get_vbus_float();
	foc->in.b_openloop = ctrl->mode_running == CTRL_MODE_OPEN;
	phase_curr_unbal_check(ctrl);
	if (foc->in.b_openloop) {
		foc->in.target_vol_dq.d = line_ramp_get_interp(&ctrl->ramp_target_vd);
		foc->in.target_vol_dq.q = line_ramp_get_interp(&ctrl->ramp_target_vq);
	}

	foc_update(foc);

	park(foc, &ctrl->phase_v_ab, &ctrl->phase_v_dq);

	ctrl->duty_raw = NORM2_f(foc->out.vol_dq.d, foc->out.vol_dq.q)/(foc->in.dc_vol * CONFIG_SVM_MODULATION * SQRT3_BY_2);
	LowPass_Filter(ctrl->duty_filterd, ctrl->duty_raw, 0.01f);
	float lowpass = 0.001f;
	LowPass_Filter(ctrl->out_idq_filterd.d, foc->out.curr_dq.d ,lowpass);
	LowPass_Filter(ctrl->out_idq_filterd.q, foc->out.curr_dq.q ,lowpass);
	LowPass_Filter(ctrl->out_vdq_filterd.d, foc->out.vol_dq.d ,lowpass/1.5f);
	LowPass_Filter(ctrl->out_vdq_filterd.q, foc->out.vol_dq.q ,lowpass/1.5f);

	/* 计算母线电流 */
#if (CONFIG_IBUS_CALC_SAMPLED_V)
	float v_alpha = ctrl->phase_v_ab.a;
	float v_beta = ctrl->phase_v_ab.b;
	float i_alpha = foc->in.curr_ab.a;
	float i_beta = foc->in.curr_ab.b;
#else
	float vd = ctrl->out_vdq_filterd.d;
	float vq = ctrl->out_vdq_filterd.q;
	float id = ctrl->out_idq_filterd.d;
	float iq = ctrl->out_idq_filterd.q;
#endif
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 3/2(iq x vq + id x vd) = 3/2(i_alpha * v_alpha + i_beta * v_beta);
	*/
#if (CONFIG_IBUS_CALC_SAMPLED_V)
	float m_pow = (v_alpha * i_alpha + v_beta * i_beta);
#else
	float m_pow = (vd * id + vq * iq);
#endif
	float raw_idc = 0.0f;
	float v_dc = get_vbus_float();
	if (v_dc != 0.0f) {
		raw_idc = m_pow / v_dc;
	}
	ctrl->dc_curr_calc = raw_idc;
	return true;
}

#ifndef CONFIG_TORQUE_LUT
static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
	float dc_lim = line_ramp_get_interp(&ctrl->ramp_dc_curr_lim);
	/* 计算iq的前馈,(母线功率 - d轴功率)/vq
	 * 加上0.0001f 避免除0
	*/

	float r = ctrl->dc_curr_filted / (ctrl->dc_curr_calc + 0.0001f) + 0.0001f;
	r = fclamp(ABS(r), 0.5f, 1.5f);
	float q_axis_power = dc_lim * get_vbus_float() / r - ctrl->out_idq_filterd.d * ctrl->out_vdq_filterd.d;
	float vq = ctrl->out_vdq_filterd.q;
	if (vq < 1.0f) {
		vq = 1.0f;
	}
	float iq_ff = q_axis_power / vq;
	iq_ff = fabs(iq_ff);
	if (iq_ff > maxTrq) {
		ctrl->dc_lim_t_ff = maxTrq;
	}else {
		ctrl->dc_lim_t_ff = iq_ff;
	}
	/** 使用min(maxTrq, ctrl->dc_lim_t_ff * 1.1f)带入max的原因是为了防止积分器饱和后有过冲现象
	 *  1.1f的系数是为了保证ctrl->dc_lim_t_ff 计算偏小的情况下功率能达到上限，这个系数太大不能
	 *  防止过冲
	*/
	ctrl->pi_power.max = min(maxTrq, ctrl->dc_lim_t_ff * 1.1f);
	float errRef = dc_lim - ctrl->dc_curr_filted;
	return PI_Controller_Parallel(&ctrl->pi_power, errRef, ctrl->dc_lim_t_ff);
}
#else
static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
	float dc_lim = line_ramp_get_interp(&ctrl->ramp_dc_curr_lim);
	/* 计算扭矩的前馈,(母线功率 * 9.55 * 电机效率)/(电机速度)
	 * 加上0.0001f 避免除0
	*/
	float torque_ff = dc_lim * get_vbus_float() * 9.55f * 0.8f/(ctrl->foc.in.mot_velocity + 0.0001f);
	torque_ff = ABS(torque_ff);
	if (torque_ff > maxTrq) {
		ctrl->dc_lim_t_ff = maxTrq;
	}else if (torque_ff < 0) {
		ctrl->dc_lim_t_ff = 0;
	}else {
		ctrl->dc_lim_t_ff = torque_ff;
	}
	/** 使用min(maxTrq, ctrl->dc_lim_t_ff * 1.1f)带入max的原因是为了防止积分器饱和后有过冲现象
	 *  1.1f的系数是为了保证ctrl->dc_lim_t_ff 计算偏小的情况下功率能达到上限，这个系数太大不能
	 *  防止过冲
	*/
	ctrl->pi_power.max = min(maxTrq, ctrl->dc_lim_t_ff * 1.1f);
	float errRef = dc_lim - ctrl->dc_curr_filted;
	return PI_Controller_Parallel(&ctrl->pi_power, errRef, ctrl->dc_lim_t_ff);
}
#endif
static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
	ctrl->pi_vel_lim.max = maxTrq;
	ctrl->pi_vel_lim.min = 0;
	float err = line_ramp_get_interp(&ctrl->ramp_vel_lim) - ctrl->foc.in.mot_velocity;
	return PI_Controller_Parallel(&ctrl->pi_vel_lim, err, 0);
}

/* current vector or torque to dq axis current */
static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
		float target_current = line_ramp_get_interp(&ctrl->ramp_target_current);
		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
			float s, c;
			float angle_step = line_ramp_step(&ctrl->ramp_adv_angle);
			arm_sin_cos(angle_step + 90.0f, &s, &c);
			ctrl->target_idq.d = target_current * c;
			if (ctrl->target_idq.d > ctrl->hwlim.fw_id) {
				ctrl->target_idq.d = ctrl->hwlim.fw_id;
			}else if (ctrl->target_idq.d < -ctrl->hwlim.fw_id) {
				ctrl->target_idq.d = -ctrl->hwlim.fw_id;
			}
			ctrl->target_idq.q = sqrtsub2_f(target_current, ctrl->target_idq.d);
			if (s < 0) {
				ctrl->target_idq.q = -ctrl->target_idq.q;
			}
		}else {
			ctrl->target_idq.d = 0;
			ctrl->target_idq.q = target_current;
		}
	}else if (ctrl->mode_running != CTRL_MODE_OPEN) {
		motor_mpta_fw_lookup(ctrl->foc.in.mot_velocity, ctrl->target_torque, &ctrl->target_idq);
	}
	u32 mask = cpu_enter_critical();
	foc_set_target_idq(&ctrl->foc, &ctrl->target_idq);
	cpu_exit_critical(mask);
}

static void crosszero_step_towards(float *value, float target) {
	static float no_cro_step = CONFIG_CrossZero_NorStep;
	float v_now = *value;
	bool cross_zero = false;
	float nor_step = mc_conf()->cz.normal_step;
	float min_step = mc_conf()->cz.min_step;
	float min_ramp_torque = mc_conf()->cz.low;
	float high_ramp_torque = mc_conf()->cz.high;
	if (target > 0) {
		if (v_now < -min_ramp_torque) {
			step_towards(value, -min_ramp_torque + 0.001f, nor_step);
			cross_zero = true;
		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
			step_towards(value, target, min_step);
			cross_zero = true;
		}
	}else if (target == 0) {
		if (v_now > high_ramp_torque) {
			step_towards(value, high_ramp_torque - 0.001f, nor_step);
			cross_zero = true;
		}else if (v_now >= min_ramp_torque && v_now <= high_ramp_torque) {
			step_towards(value, target, min_step);
			cross_zero = true;
		}
	}else {
		if (v_now > high_ramp_torque) {
			step_towards(value, high_ramp_torque - 0.001f, nor_step);
			cross_zero = true;
		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
			step_towards(value, target, min_step);
			cross_zero = true;
		}
	}
	if (!cross_zero) {
		step_towards(&no_cro_step, nor_step, 0.1f);
		step_towards(value, target, no_cro_step);
	}else {
		no_cro_step = 0.5f;
	}
}

/*called in media task */
void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
	foc_t *foc = &ctrl->foc;
	float etcs_out = etcs_process(&ctrl->etcs);
	if (ctrl->b_AutoHold) {
		float hold_torque = min(ctrl->protlim.torque, mc_conf()->c.max_autohold_torque);
		ctrl->pi_lock.max = hold_torque;
		ctrl->pi_lock.min = -hold_torque;
		float vel_count = motor_encoder_get_vel_count();
		float errRef = 0 - vel_count;
		ctrl->target_torque = PI_Controller_Parallel(&ctrl->pi_lock ,errRef, 0);
		mot_contrl_dq_assign(ctrl);
		return;
	}
	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
		line_ramp_step(&ctrl->ramp_target_current);
	}else if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
		float maxTrq = line_ramp_step(&ctrl->ramp_input_torque);
		if (line_ramp_get_target(&ctrl->ramp_input_torque) < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
			maxTrq = 0;
		}
		crosszero_step_towards(&ctrl->target_torque, maxTrq);
	}else if (ctrl->mode_running == CTRL_MODE_TRQ) {
		float refTorque = line_ramp_step(&ctrl->ramp_input_torque);
		refTorque = min(refTorque, line_ramp_get_interp(&ctrl->ramp_torque_lim)) * etcs_out;
		float maxTrq = mot_contrl_vel_limiter(ctrl, refTorque);
		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
	}else if (ctrl->mode_running == CTRL_MODE_SPD){
		float refSpeed;
		float maxSpeed;
		if (ctrl->b_cruiseEna) {
			refSpeed = line_ramp_step(&ctrl->ramp_cruise_vel);
			maxSpeed = line_ramp_get_target(&ctrl->ramp_cruise_vel);
		}else {
			refSpeed = line_ramp_step(&ctrl->ramp_target_vel);
			maxSpeed = line_ramp_get_target(&ctrl->ramp_target_vel);
		}
		float max_input = line_ramp_get_interp(&ctrl->ramp_torque_lim) * etcs_out;
		if (maxSpeed >= 0) {
			ctrl->pi_vel.max = max_input;
#ifdef CONFIG_SERVO_MOTOR
			ctrl->pi_vel.min = -max_input;
#else
			ctrl->pi_vel.min = -CONFIG_MAX_NEG_TORQUE;
#endif
		}else if (maxSpeed < 0) {
			ctrl->pi_vel.min = -max_input;
#ifdef CONFIG_SERVO_MOTOR
			ctrl->pi_vel.max = max_input;
#else
			ctrl->pi_vel.max = CONFIG_MAX_NEG_TORQUE;
#endif
		}

		if ((maxSpeed == 0) && (ctrl->foc.in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
			ctrl->pi_vel.max = 0;
			ctrl->pi_vel.min = 0; //防止倒转
		}
		float errRef = refSpeed - ctrl->foc.in.mot_velocity;
		float maxTrq = PI_Controller_Parallel(&ctrl->pi_vel, errRef, 0);
		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
	}

	mot_contrl_dq_assign(ctrl);
}

static void mot_contrl_pid(mot_contrl_t *ctrl) {
	float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
	PI_Controller_Reset(&ctrl->pi_power, 0);
	ctrl->pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
	ctrl->pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
	ctrl->pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
	ctrl->pi_power.ts = slow_ctrl_ts;

	PI_Controller_Reset(&ctrl->pi_lock, 0);
	ctrl->pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
	ctrl->pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
	ctrl->pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
	ctrl->pi_lock.ts = slow_ctrl_ts;

	PI_Controller_Reset(&ctrl->pi_vel_lim, 0);
	ctrl->pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
	ctrl->pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
	ctrl->pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
	ctrl->pi_vel_lim.ts = slow_ctrl_ts;

	PI_Controller_Reset(&ctrl->pi_vel, 0);
	ctrl->pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
	ctrl->pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
	ctrl->pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
	ctrl->pi_vel.ts = slow_ctrl_ts;
}

static void mot_contrl_ulimit(mot_contrl_t *ctrl) {
	ctrl->userlim.dc_curr = min(mc_conf()->c.max_idc, ctrl->hwlim.dc_curr);
	ctrl->userlim.mot_vel = min(mc_conf()->c.max_rpm, ctrl->hwlim.mot_vel);
	ctrl->userlim.torque = mc_conf()->c.max_torque;//MAX_TORQUE;
	ctrl->userlim.phase_curr = min(mc_conf()->c.max_phase_curr, ctrl->hwlim.phase_curr);
	ctrl->userlim.dc_vol_min = mc_conf()->c.max_dc_vol;
	ctrl->userlim.dc_vol_max = mc_conf()->c.min_dc_vol;
	ctrl->userlim.ebrk_dc_curr = 0xFF;
	ctrl->userlim.ebrk_torque = mc_get_ebrk_torque();
}

static void mot_contrl_rtlimit(mot_contrl_t *ctrl) {
	line_ramp_reset(&ctrl->ramp_torque_lim, ctrl->userlim.torque);
	line_ramp_reset(&ctrl->ramp_dc_curr_lim, ctrl->userlim.dc_curr);
	line_ramp_reset(&ctrl->ramp_vel_lim, ctrl->userlim.mot_vel);
}

void mot_contrl_slow_task(mot_contrl_t *ctrl) {
	line_ramp_step(&ctrl->ramp_torque_lim);
	line_ramp_step(&ctrl->ramp_dc_curr_lim);
	line_ramp_step(&ctrl->ramp_vel_lim);
	mot_contrl_dq_calc(ctrl);
}


u8 mot_contrl_protect(mot_contrl_t *ctrl) {
	float torque_lim;
	u8 changed = FOC_LIM_NO_CHANGE;
	float dc_lim = (float)vbus_voltage_low_limit();
	float mot_lim_r = motor_temp_high_limit();
	float mos_lim_r = mos_temp_high_limit();
	float mos_torque = HW_LIMIT_NONE;
	float mot_torque = HW_LIMIT_NONE;
	float mot_idc = HW_LIMIT_NONE;
	if (mot_lim_r < 1.0f) {
		mot_torque = mot_lim_r * mc_gear_conf()->max_torque;
		mot_idc    = mot_lim_r * mc_gear_conf()->max_idc;
	}
	if (mos_lim_r < 1.0f) {
		mos_torque = mos_lim_r * mc_gear_conf()->max_torque;
	}

	dc_lim = min(dc_lim, mot_idc);
	torque_lim = min(mos_torque, mot_torque);

	if (ctrl->protlim.dc_curr != dc_lim || ctrl->protlim.torque != torque_lim) {
		if ((dc_lim > ctrl->protlim.dc_curr) || (torque_lim > ctrl->protlim.torque)) {
			changed = FOC_LIM_CHANGE_H;
		}else {
			changed = FOC_LIM_CHANGE_L;
		}
		ctrl->protlim.dc_curr = dc_lim;
		ctrl->protlim.torque = torque_lim;
	}
	return changed;
}

float mot_contrl_get_speed(mot_contrl_t *ctrl) {
	float speed = ctrl->foc.in.mot_velocity;
	if (!ctrl->b_start || foc_observer_is_encoder()) {
		speed = motor_encoder_get_speed();
	}else {
		if (foc_observer_sensorless_stable()) {
			speed = foc_observer_sensorless_speed();
		}else {
			speed = 0;
		}
	}
	return speed;
}

void mot_contrl_velloop_params(mot_contrl_t *ctrl, float wcv, float b0) {
#ifdef CONFIG_SPEED_LADRC
	ladrc_change_b0(&gFoc_Ctrl.vel_adrc, b0);
	ladrc_change_K(&gFoc_Ctrl.vel_adrc, wcv);
#else
	PI_Controller_Change_Kpi(&ctrl->pi_vel, wcv, b0);
#endif
}

void mot_contrl_trqloop_params(mot_contrl_t *ctrl, float wcv, float b0) {
#ifdef CONFIG_SPEED_LADRC
	ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, b0);
	ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, wcv);
#else
	PI_Controller_Change_Kpi(&ctrl->pi_vel_lim, wcv, b0);
#endif
}

void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit) {
	if (ibusLimit > ctrl->hwlim.dc_curr) {
		ibusLimit = ctrl->hwlim.dc_curr;
	}
	if (ctrl->protlim.dc_curr != HW_LIMIT_NONE) {
		ibusLimit = min(ibusLimit, ctrl->protlim.dc_curr);
	}
	ctrl->userlim.dc_curr = ibusLimit;
	/*
	 * 这个地方不能像限制扭矩和速度那样从当前实际母线电流开始做ramp
     * 因为用了功率环前馈，会导致扭矩突然下降
	*/
	line_ramp_set_target(&ctrl->ramp_dc_curr_lim, ctrl->userlim.dc_curr);
}

void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel) {
	if (vel > ctrl->hwlim.mot_vel) {
		vel = ctrl->hwlim.mot_vel;
	}
	ctrl->userlim.mot_vel = vel;
	line_ramp_reset(&ctrl->ramp_vel_lim, ctrl->foc.in.mot_velocity);//从当前实际转速做ramp
	line_ramp_set_target(&ctrl->ramp_vel_lim, ctrl->userlim.mot_vel);
}

void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time) {
	line_ramp_set_time(&ctrl->ramp_vel_lim, (float)time);
	line_ramp_update(&ctrl->ramp_vel_lim);
}


void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque) {
	if (torque > ctrl->hwlim.torque) {
		torque = ctrl->hwlim.torque;
	}
	if (ctrl->protlim.torque != HW_LIMIT_NONE) {
		torque = min(torque, ctrl->protlim.torque);
	}
	ctrl->userlim.torque = torque;
	line_ramp_reset(&ctrl->ramp_torque_lim, ctrl->target_torque_raw);//从当前给定的torque做ramp
	line_ramp_set_target(&ctrl->ramp_torque_lim, ctrl->userlim.torque);
}

void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time) {
	line_ramp_set_time(&ctrl->ramp_torque_lim, (float)time);
	line_ramp_update(&ctrl->ramp_torque_lim);
}

float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl) {
	if (!foc_observer_is_encoder()) {
		return 0; //无感运行关闭能量回收
	}
	return ctrl->userlim.ebrk_torque;
}

void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time) {
	ctrl->ebrk_ramp_time = time;
	if ((ctrl->mode_running == CTRL_MODE_EBRAKE) && (time != ctrl->ramp_input_torque.time_dec)) {
		line_ramp_set_time(&ctrl->ramp_input_torque, time);
		line_ramp_update(&ctrl->ramp_input_torque);
	}
}

void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq) {
	line_ramp_set_target(&ctrl->ramp_target_vd, vd);
	line_ramp_set_target(&ctrl->ramp_target_vq, vq);
}

void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq) {
	line_ramp_reset(&ctrl->ramp_target_vd, vd);
	line_ramp_reset(&ctrl->ramp_target_vq, vq);
}

bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable) {
	if (enable != ctrl->b_cruiseEna) {
		float motSpd = mot_contrl_get_speed(ctrl);
		if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
			mot_contrl_set_error(ctrl, FOC_NowAllowed_With_Speed);
			return false;
		}
		line_ramp_reset(&ctrl->ramp_cruise_vel, motSpd);
		ctrl->b_cruiseEna = enable;
	}
	return true;
}


bool mot_contrl_resume_cruise(mot_contrl_t *ctrl) {
	ctrl->b_cruiseEna = true;
	line_ramp_set_time(&ctrl->ramp_cruise_vel, CONFIG_CRUISE_RAMP_TIME);
	return true;
}

bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm) {
	if (ctrl->b_cruiseEna) {
		if (rpm < CONFIG_MIN_CRUISE_RPM) {
			rpm = CONFIG_MIN_CRUISE_RPM;
		}
		float vel = min(ABS(rpm),ctrl->userlim.mot_vel)*SIGN(rpm);
		line_ramp_set_target(&ctrl->ramp_cruise_vel, vel);
		return true;
	}
	mot_contrl_set_error(ctrl, FOC_NotCruiseMode);
	return false;
}

bool mot_contrl_set_current(mot_contrl_t *ctrl, float is) {
	is = fclamp(is, -ctrl->userlim.phase_curr, ctrl->userlim.phase_curr);
	line_ramp_set_target(&ctrl->ramp_target_current, is);
	return true;
}

void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec) {
	ctrl->torque_acc_time = acc;
	ctrl->torque_dec_time = dec;
	if (ctrl->mode_running == CTRL_MODE_TRQ) {
		line_ramp_set_acctime(&ctrl->ramp_input_torque, acc);
		line_ramp_set_dectime(&ctrl->ramp_input_torque, dec);
		line_ramp_update(&ctrl->ramp_input_torque);
	}
}

void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc) {
	ctrl->torque_acc_time = acc;
	if (ctrl->mode_running == CTRL_MODE_TRQ) {
		line_ramp_set_acctime(&ctrl->ramp_input_torque, acc);
		line_ramp_update(&ctrl->ramp_input_torque);
	}
}

bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque) {
	if (is_hw_brake_shutting_power(ctrl) && !ctrl->b_ebrk_running){
		return false;
	}
	float torque_min = 0;
	float torque_max = ctrl->userlim.torque;
	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
		torque_min = -ctrl->userlim.ebrk_torque;
		torque_max = 0;
	}
	torque = fclamp(torque, torque_min, torque_max);
	line_ramp_set_target(&ctrl->ramp_input_torque, torque);
	return true;
}

/* 这个接口只在上位机直接设置扭矩的时候调试，其他情况一律不能使用，扭矩请求可以未负 */
bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque) {
	if (is_hw_brake_shutting_power(ctrl) && !ctrl->b_ebrk_running){
		return false;
	}
	float torque_min = -ctrl->userlim.torque;
	float torque_max = ctrl->userlim.torque;
	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
		torque_min = -ctrl->userlim.torque;
		torque_max = 0;
	}
	torque = fclamp(torque, torque_min, torque_max);
	line_ramp_set_target(&ctrl->ramp_input_torque, torque);
	return true;
}


void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable) {
	if (enable) {
		line_ramp_reset(&ctrl->ramp_adv_angle, 0);
		ctrl->b_mtpa_calibrate = true;
		ctrl->adv_angle = 0;
	}else {
		ctrl->adv_angle = INVALID_ANGLE;
		ctrl->b_mtpa_calibrate = false;
	}
}

void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock) {
	if (ctrl->b_AutoHold != lock) {
		motor_encoder_lock_pos(lock);
		PI_Controller_Reset(&ctrl->pi_lock, 0);
		if (!lock) {
			float hold_torque = ctrl->target_torque * 1.1f;
			if (ctrl->mode_running == CTRL_MODE_TRQ) {
				PI_Controller_Reset(&ctrl->pi_vel_lim, hold_torque);
			}else if (ctrl->mode_running == CTRL_MODE_SPD) {
				PI_Controller_Reset(&ctrl->pi_vel, hold_torque);
			}
			line_ramp_reset(&ctrl->ramp_input_torque, hold_torque);
			ctrl->autohold_torque = hold_torque;
		}else {
			ctrl->autohold_torque = 0;
		}
		ctrl->b_AutoHold = lock;
	}
}

static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl) {
	return (ctrl->b_hw_braker && mc_hwbrk_can_shutpower());
}

bool mot_contrl_set_ebreak(mot_contrl_t *ctrl, bool start) {
	bool enable = ctrl->b_ebrk_running;
	if (mot_contrl_get_ebrk_torque(ctrl) == 0) {
		enable = false;
	}else if (start && ctrl->foc.in.mot_velocity >= CONFIG_MIN_RPM_FOR_EBRAKE){
		enable = true;
	}else if (!start && !is_hw_brake_shutting_power(ctrl)) {
		enable = false;
	}
	if (enable != ctrl->b_ebrk_running) {
		ctrl->b_ebrk_running = enable;
		if (enable) {
			ctrl->mode_req = CTRL_MODE_EBRAKE;
		}else {
			ctrl->mode_req = CTRL_MODE_TRQ;
		}
	}
	return enable;
}

void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake) {
	u32 mask = cpu_enter_critical();
	if (hw_brake != ctrl->b_hw_braker) {
		ctrl->b_hw_braker = hw_brake;
	}
	if (is_hw_brake_shutting_power(ctrl)) {
		if (!ctrl->b_ebrk_running && !mot_contrl_set_ebreak(ctrl, true)) {
			line_ramp_reset(&ctrl->ramp_input_torque, 0);
		}
	}
	cpu_exit_critical(mask);
}


static PI_Controller *_pid(mot_contrl_t *ctrl, u8 id) {
	PI_Controller *pi = NULL;
	if (id == PID_ID_ID) {
		pi = &ctrl->foc.daxis;
	}else if (id == PID_IQ_ID) {
		pi = &ctrl->foc.qaxis;
	}else if (id == PID_VelLim_ID) {
		pi = &ctrl->pi_vel_lim;
	}else if (id == PID_Vel_ID) {
		pi = &ctrl->pi_vel;
	}else if (id == PID_AutoHold_ID) {
		pi = &ctrl->pi_lock;
	}
	return pi;
}

void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd) {
	if (id > PID_Max_ID) {
		return;
	}
	PI_Controller *pi = _pid(ctrl, id);
	if (pi != NULL) {
		u32 mask = cpu_enter_critical();
		pi->kp = kp;
		pi->ki = ki;
		pi->kd = kd;
		cpu_exit_critical(mask);
	}
}

void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd) {
	if (id > PID_Max_ID) {
		return;
	}
	PI_Controller *pi = _pid(ctrl, id);
	if (pi != NULL) {
		*kp = pi->kp;
		*ki = pi->ki;
		*kd = pi->kd;
	}
}

void mot_contrl_calc_current(mot_contrl_t *ctrl) {
	float id = ctrl->out_idq_filterd.d;
	float iq = ctrl->out_idq_filterd.q;

	float raw_idc = get_vbus_current();
	if (raw_idc != NO_VALID_CURRENT) {
		LowPass_Filter(ctrl->dc_curr_filted, raw_idc, 0.04f);
	}else {
		ctrl->dc_curr_filted = ctrl->dc_curr_calc;
	}
	ctrl->out_current_vec = NORM2_f(id, iq);
}