#include "arm_math.h"
#include "PMSM_FOC_Core.h"
#include "foc/foc_config.h"
#include "foc/motor/motor_param.h"
#include "foc/core/e_ctrl.h"
#include "math/fix_math.h"
#include "math/fast_math.h"
#include "foc/motor/current.h"
#include "foc/motor/motor.h"
#include "foc/core/svpwm.h"
#include "foc/core/thro_torque.h"
#include "foc/core/foc_observer.h"
#include "foc/samples.h"
#include "foc/limit.h"
#include "app/nv_storage.h"
#include "bsp/bsp_driver.h"
#include "libs/logger.h"
#include "math/fir.h"

#define _DEBUG(fmt, args...) sys_debug(fmt, ##args)

PMSM_FOC_Ctrl gFoc_Ctrl;
static bool g_focinit = false;

static u32 PMSM_FOC_Debug_Task(void *p);

static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
	float c,s;
#if 0
	SinCos_Lut(angle, &s, &c);
#else
	s = gFoc_Ctrl.out.sin;
	c = gFoc_Ctrl.out.cos;
#endif

	alpha_beta->a = dq->d * c - dq->q * s;
	alpha_beta->b = dq->d * s + dq->q * c;
}

static __INLINE void Clark(float A, float B, float C, AB_t *alpha_beta){
	alpha_beta->a = A;
	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
}

static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
	float c,s;
#if 0
	SinCos_Lut(angle, &s, &c);
#else
	s = gFoc_Ctrl.out.sin;
	c = gFoc_Ctrl.out.cos;
#endif
	dq->d = alpha_beta->a * c + alpha_beta->b * s;
	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
}

#if 0
#define VD_PRIO_HIGH
static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t *out) {
	float sq_vdq = vdq->d * vdq->d + vdq->q * vdq->q;
	float vDC_m = vDC * module * SQRT3_BY_2;
	float sq_vDC = vDC_m * vDC_m;
	if (sq_vdq > sq_vDC) {
#ifdef VD_PRIO_HIGH		
		out->d = vdq->d;
		out->q = sqrtf(sq_vDC - out->d*out->d);
#else
		float r = sqrtf(sq_vDC / sq_vdq);
		out->d = vdq->d * r;
		out->q = vdq->q * r;
#endif
	}else {
		out->d = vdq->d;
		out->q = vdq->q;
	}
	return sqrtf(sq_vdq/sq_vDC);
}
#endif
static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {	
	float cp = c->s_Cp;
	c->s_FinalTgt = target; 
	c->s_Step = (c->s_FinalTgt - cp) / (float)time;
}

static __INLINE float FOC_Get_DqRamp(dq_Rctrl *c) {
	if (++c->n_StepCount == c->n_CtrlCount) {
		c->s_Cp += c->s_Step;
		if (c->s_Step < 0) {
			if (c->s_Cp < c->s_FinalTgt) {
				c->s_Cp = c->s_FinalTgt;
			}
		}else {
			if (c->s_Cp > c->s_FinalTgt) {
				c->s_Cp = c->s_FinalTgt;
			}
		}
		c->n_StepCount = 0;
	}
	return c->s_Cp;
}

static __INLINE void FOC_DqRamp_init(dq_Rctrl *c, int count) {
	c->n_CtrlCount = count;
	c->n_StepCount = 0;
	c->s_Cp = 0;
	c->s_FinalTgt = 0;
	c->s_Step = 0;
}

static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, float target) {
	FOC_Set_DqRamp(c, target, (/*CONFIG_IDQ_CTRL_TS/CONFIG_SPD_CTRL_TS - 1*/CURRENT_LOOP_RAMP_COUNT));
}

static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
	FOC_Set_DqRamp(c, target, (CONFIG_FOC_VDQ_RAMP_FINAL_TIME/1000*((CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS))));
}


static void PMSM_FOC_Reset_PID(void) {
	PI_Controller_Reset(&gFoc_Ctrl.pi_id, 0);
	PI_Controller_Reset(&gFoc_Ctrl.pi_iq, 0);
	PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
	PI_Controller_Reset(&gFoc_Ctrl.pi_power, 0);
#ifdef CONFIG_SPEED_LADRC
	ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, 0);
	ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, 0);
#else
	PI_Controller_Reset(&gFoc_Ctrl.pi_speed, 0);
	PI_Controller_Reset(&gFoc_Ctrl.pi_torque, 0);
#endif
}

static void PMSM_FOC_Conf_PID(void) {
	float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
	gFoc_Ctrl.pi_id.kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
	gFoc_Ctrl.pi_id.ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
	gFoc_Ctrl.pi_id.kd = nv_get_foc_params()->pid_conf[PID_D_id].kd;
	gFoc_Ctrl.pi_id.DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);

	gFoc_Ctrl.pi_iq.kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
	gFoc_Ctrl.pi_iq.ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
	gFoc_Ctrl.pi_iq.kd = nv_get_foc_params()->pid_conf[PID_Q_id].kd;
	gFoc_Ctrl.pi_iq.DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);

	gFoc_Ctrl.pi_power.kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
	gFoc_Ctrl.pi_power.ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
	gFoc_Ctrl.pi_power.kd = nv_get_foc_params()->pid_conf[PID_Pow_id].kd;
	gFoc_Ctrl.pi_power.DT = slow_ctrl_ts;

	gFoc_Ctrl.pi_lock.kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
	gFoc_Ctrl.pi_lock.ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
	gFoc_Ctrl.pi_lock.kd = nv_get_foc_params()->pid_conf[PID_Lock_id].kd;
	gFoc_Ctrl.pi_lock.DT = slow_ctrl_ts;

#ifdef CONFIG_SPEED_LADRC
	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
	ladrc_init(&gFoc_Ctrl.vel_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
#else
	gFoc_Ctrl.pi_torque.kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
	gFoc_Ctrl.pi_torque.ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
	gFoc_Ctrl.pi_torque.kd = nv_get_foc_params()->pid_conf[PID_TRQ_id].kd;
	gFoc_Ctrl.pi_torque.DT = slow_ctrl_ts;
	gFoc_Ctrl.pi_speed.kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
	gFoc_Ctrl.pi_speed.ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
	gFoc_Ctrl.pi_speed.kd = nv_get_foc_params()->pid_conf[PID_Spd_id].kd;
	gFoc_Ctrl.pi_speed.DT = slow_ctrl_ts;
#endif
}

static void PMSM_FOC_UserInit(void) {
	memset(&gFoc_Ctrl.userLim, 0, sizeof(gFoc_Ctrl.userLim));
	gFoc_Ctrl.userLim.s_iDCLim = min(nv_get_foc_params()->s_LimitiDC, gFoc_Ctrl.hwLim.s_iDCMax);
	gFoc_Ctrl.userLim.s_motRPMLim = min(nv_get_foc_params()->s_maxRPM, gFoc_Ctrl.hwLim.s_motRPMMax);
	gFoc_Ctrl.userLim.s_torqueLim = nv_get_foc_params()->s_maxTorque;//MAX_TORQUE;
	gFoc_Ctrl.userLim.s_PhaseCurrLim = min(nv_get_foc_params()->s_PhaseCurrLim, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
	gFoc_Ctrl.userLim.s_vDCMaxLim = nv_get_foc_params()->s_maxDCVol;
	gFoc_Ctrl.userLim.s_vDCMinLim = nv_get_foc_params()->s_minDCVol;
	gFoc_Ctrl.userLim.s_iDCeBrkLim = nv_get_foc_params()->s_iDCeBrkLim;
	gFoc_Ctrl.userLim.s_TorqueBrkLim = nv_get_foc_params()->s_TorqueBrkLim;
	gFoc_Ctrl.userLim.s_PhaseVoleBrkLim = gFoc_Ctrl.hwLim.s_PhaseVolMax;
}

void PMSM_FOC_RT_LimInit(void) {
	gFoc_Ctrl.protLim.s_iDCLim = HW_LIMIT_NONE;
	gFoc_Ctrl.protLim.s_TorqueLim = HW_LIMIT_NONE;

	eRamp_init_target(&gFoc_Ctrl.rtLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
	eRamp_init_target(&gFoc_Ctrl.rtLim.torqueLimRamp, gFoc_Ctrl.userLim.s_torqueLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
	eRamp_init_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
}

void PMSM_FOC_CoreInit(void) {

	PMSM_FOC_Conf_PID();
	
	memset(&gFoc_Ctrl.in, 0, sizeof(gFoc_Ctrl.in));
	memset(&gFoc_Ctrl.out, 0, sizeof(gFoc_Ctrl.out));
	
	gFoc_Ctrl.hwLim.s_iDCMax = CONFIG_MAX_VBUS_CURRENT;
	gFoc_Ctrl.hwLim.s_motRPMMax = CONFIG_MAX_MOT_RPM;
	gFoc_Ctrl.hwLim.s_PhaseCurrMax = CONFIG_MAX_PHASE_CURR;
	gFoc_Ctrl.hwLim.s_PhaseVolMax = CONFIG_MAX_PHASE_VOL;
	gFoc_Ctrl.hwLim.s_vDCMax      = CONFIG_MAX_DC_VOL;
	gFoc_Ctrl.hwLim.s_torqueMax  = CONFIG_MAX_MOTOR_TORQUE;
	gFoc_Ctrl.hwLim.s_FWDCurrMax = CONFIG_MAX_FW_D_CURR;
	if (!g_focinit) {
		PMSM_FOC_UserInit();
		PMSM_FOC_RT_LimInit();
		shark_task_create(PMSM_FOC_Debug_Task, NULL);
		g_focinit = true;
		//_DEBUG("User Limit:\n");
		//_DEBUG("dc %f, rpm %f, torque %f, phase %f, vDCmax %f, vDCmin %f, ebrk %f\n", gFoc_Ctrl.userLim.s_iDCLim, gFoc_Ctrl.userLim.s_motRPMLim, gFoc_Ctrl.userLim.s_torqueLim,
		//	gFoc_Ctrl.userLim.s_PhaseCurrLim, gFoc_Ctrl.userLim.s_vDCMaxLim, gFoc_Ctrl.userLim.s_vDCMinLim, gFoc_Ctrl.userLim.s_TorqueBrkLim);
		//_DEBUG("Hw Limit:\n");
		//_DEBUG("dc %f, rpm %f, torque %f, phase %f\n", gFoc_Ctrl.hwLim.s_iDCMax, gFoc_Ctrl.hwLim.s_motRPMMax, gFoc_Ctrl.hwLim.s_torqueMax, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
	}
	
	gFoc_Ctrl.params.n_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
	gFoc_Ctrl.params.n_PhaseFilterCeof = CONFIG_CURR_LP_CEOF;
	gFoc_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
	gFoc_Ctrl.params.lq = nv_get_motor_params()->lq;
	gFoc_Ctrl.params.ld = nv_get_motor_params()->ld;
	gFoc_Ctrl.params.flux = nv_get_motor_params()->flux_linkage;
	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxDCVol;//(CONFIG_RATED_DC_VOL);
//	gFoc_Ctrl.params.f_DCLim = get_vbus_float();
	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
	
	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
	gFoc_Ctrl.out.f_vdqRation = 0;

	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[0], 1);
	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[1], 1);

	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[0], (CONFIG_FOC_VDQ_RAMP_TS));
	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_FOC_VDQ_RAMP_TS));	
	PMSM_FOC_Reset_PID();

	foc_observer_init();

	gFoc_Ctrl.plot_type = Plot_None;
}
//#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
//#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
static __INLINE void PMSM_FOC_Update_Input(void) {
	AB_t iAB;

	float *iabc = gFoc_Ctrl.in.s_iABC;

	phase_current_get(iabc);

	Clark(iabc[0], iabc[1], iabc[2], &iAB);

	foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * 0.66667f, gFoc_Ctrl.out.s_OutVAB.b * 0.66667f, iAB.a, iAB.b);

	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
	}else {
		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
	}
	gFoc_Ctrl.in.s_motRPM = motor_encoder_get_speed();
	LowPass_Filter(gFoc_Ctrl.in.s_motRPMFilted, gFoc_Ctrl.in.s_motRPM, 0.005f);
	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motRPM / 30.0f * PI * gFoc_Ctrl.params.n_poles;
#ifdef CONFIG_DQ_STEP_RESPONSE
	gFoc_Ctrl.in.s_hallAngle = 0;
	gFoc_Ctrl.in.s_motAngle = 0;
#endif

	gFoc_Ctrl.in.s_vDC = get_vbus_float();
	
	get_phase_vols(gFoc_Ctrl.in.s_vABC);

	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);

	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);

	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
#ifdef Volvec_Delay_Comp
	float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelDegreePers / PI * 180.0f * (FOC_CTRL_US - 2e-6f);
	rand_angle(next_angle);
	SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
#endif
}

#ifdef CONFIG_DQ_STEP_RESPONSE
float target_d = 0.0f;
float target_q = 0.0f;
#endif

static u32 PMSM_FOC_Debug_Task(void *p) {
	if (gFoc_Ctrl.in.b_motEnable) {
#ifdef CONFIG_DQ_STEP_RESPONSE
		if (gFoc_Ctrl.plot_type == Plot_D_Step) {
			plot_2data16(FtoS16x10(target_d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d));
		}if (gFoc_Ctrl.plot_type == Plot_Q_Step) {
			plot_2data16(FtoS16x10(target_q), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
		}
#else
		if (gFoc_Ctrl.plot_type == Plot_D_flow) {
			plot_2data16(FtoS16x10(gFoc_Ctrl.idq_ctl[0].s_Cp), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d));
		}else if (gFoc_Ctrl.plot_type == Plot_Q_flow) {
			plot_2data16(FtoS16x10(gFoc_Ctrl.idq_ctl[1].s_Cp), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
		}else if (gFoc_Ctrl.plot_type == Plot_DQ_Curr) {
			plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.out.s_FilteriDC));
		}else if (gFoc_Ctrl.plot_type == Plot_Spd_flow) {
			plot_2data16(gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
		}
#endif
	}
	return 1;
}

static __INLINE void id_feedforward(float eW) {
#ifdef CONFIG_CURRENT_LOOP_DECOUPE
	gFoc_Ctrl.in.s_targetVdq.d += -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
	gFoc_Ctrl.in.s_targetVdq.d = fclamp(gFoc_Ctrl.in.s_targetVdq.d, gFoc_Ctrl.pi_id.min, gFoc_Ctrl.pi_id.max);
#endif
}

static __INLINE void iq_feedforward(float eW) {
#ifdef CONFIG_CURRENT_LOOP_DECOUPE
	gFoc_Ctrl.in.s_targetVdq.q += (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
	gFoc_Ctrl.in.s_targetVdq.q = fclamp(gFoc_Ctrl.in.s_targetVdq.q, gFoc_Ctrl.pi_iq.min, gFoc_Ctrl.pi_iq.max);
#endif
}

void PMSM_FOC_Schedule(void) {

	gFoc_Ctrl.ctrl_count++;

	PMSM_FOC_Update_Input();
	
	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {

		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
		float max_vd = max_Vdc * SQRT3_BY_2;

		/* limiter Vd output for PI controller */
		gFoc_Ctrl.pi_id.max = max_vd;
		gFoc_Ctrl.pi_id.min = -max_vd;
	#ifndef CONFIG_DQ_STEP_RESPONSE
		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
	#endif
		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(&gFoc_Ctrl.pi_id, err);
		id_feedforward(gFoc_Ctrl.in.s_motVelDegreePers);
#ifdef UPDATE_Lq_By_iq
		/* update kp&ki from lq for iq PI controller */
		float lq = motor_get_lq_from_iq((s16)gFoc_Ctrl.out.s_FilterIdq.q);
		LowPass_Filter(gFoc_Ctrl.params.lq, lq, 0.01f);
		gFoc_Ctrl.pi_iq.kp = ((float)nv_get_foc_params()->n_currentBand * gFoc_Ctrl.params.lq);
		gFoc_Ctrl.pi_iq.ki = (nv_get_motor_params()->r/gFoc_Ctrl.params.lq);
#endif
		/* limiter Vq output for PI controller */
		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
		gFoc_Ctrl.pi_iq.max = max_vq;
		gFoc_Ctrl.pi_iq.min = -max_vq;
	#ifndef CONFIG_DQ_STEP_RESPONSE
		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
	#endif
		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(&gFoc_Ctrl.pi_iq, err);
		iq_feedforward(gFoc_Ctrl.in.s_motVelDegreePers);
	}else {
		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
		float max_vd = max_Vdc * SQRT3_BY_2;
		float vd_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[0]);
		gFoc_Ctrl.in.s_targetVdq.d = fclamp(vd_ref, -max_vd, max_vd);
	
		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
		float vq_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[1]);
		gFoc_Ctrl.in.s_targetVdq.q = fclamp(vq_ref, -max_vq, max_vq);
	}

#if 0
	gFoc_Ctrl.out.f_vdqRation = Circle_Limitation(&gFoc_Ctrl.in.s_targetVdq, gFoc_Ctrl.in.s_vDC, gFoc_Ctrl.params.n_modulation, &gFoc_Ctrl.out.s_OutVdq);
	gFoc_Ctrl.out.s_OutVdq.d *= SQRT3_BY_2;
	gFoc_Ctrl.out.s_OutVdq.q *= SQRT3_BY_2;
#else
	gFoc_Ctrl.out.s_OutVdq.d = gFoc_Ctrl.in.s_targetVdq.d;
	gFoc_Ctrl.out.s_OutVdq.q = gFoc_Ctrl.in.s_targetVdq.q;
#endif
	RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVAB);
	
	SVM_Duty_Fix(&gFoc_Ctrl.out.s_OutVAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);

	phase_current_point(&gFoc_Ctrl.out);
	
	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);

	if (gFoc_Ctrl.plot_type != Plot_None) {
		if (gFoc_Ctrl.ctrl_count % 5 == 0) {
			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
				plot_3data16(FtoS16(gFoc_Ctrl.in.s_iABC[0]), FtoS16(gFoc_Ctrl.in.s_iABC[1]), FtoS16(gFoc_Ctrl.in.s_iABC[2]));
			}else if (gFoc_Ctrl.plot_type == Plot_Phase_vol) {
				plot_3data16(FtoS16(gFoc_Ctrl.in.s_vABC[0]), FtoS16(gFoc_Ctrl.in.s_vABC[1]), FtoS16(gFoc_Ctrl.in.s_vABC[2]));
			}else if (gFoc_Ctrl.plot_type == Plot_SMO_OBS) {
#ifdef CONFIG_SMO_OBSERVER
				float smo_angle = foc_observer_sensorless_angle();
				float delta = smo_angle - gFoc_Ctrl.in.s_hallAngle;
				if (delta > 180) {
					delta -= 360;
				}else if (delta < -180) {
					delta += 360;
				}
				plot_3data16(gFoc_Ctrl.in.s_hallAngle, smo_angle, delta);
#endif
			}
		}
	}
}

void PMSM_FOC_LogDebug(void) {
	sys_debug("DC curr %f, Lq %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.params.lq);
}

/*called in media task */
u8 PMSM_FOC_CtrlMode(void) {
	u8 preMode = gFoc_Ctrl.out.n_RunMode;

	if (!gFoc_Ctrl.in.b_motEnable) {
		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_SPD || gFoc_Ctrl.in.b_cruiseEna){
		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_EBRAKE) {
		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_EBRAKE;
	}else {
		if (!gFoc_Ctrl.in.b_cruiseEna) {
			gFoc_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
		}
	}
	if (preMode != gFoc_Ctrl.out.n_RunMode) {
		if ((preMode == CTRL_MODE_SPD) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
#ifdef CONFIG_SPEED_LADRC
			//ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque);
			ladrc_copy(&gFoc_Ctrl.vel_lim_adrc, &gFoc_Ctrl.vel_adrc);
#else
			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
#endif
		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
#ifdef CONFIG_SPEED_LADRC
			//ladrc_reset(&gFoc_Ctrl.vel_adrc, gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque);
			ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
#else
			float target_troque = gFoc_Ctrl.in.s_targetTorque;
			if (gFoc_Ctrl.pi_id->is_sat || gFoc_Ctrl.pi_iq->is_sat) {
				target_troque = PMSM_FOC_Get_Real_dqVector();
			}
			PI_Controller_Reset(&gFoc_Ctrl.pi_speed, target_troque);
#endif
		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
#ifdef CONFIG_SPEED_LADRC
			ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque);
#else
			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
#endif
		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
#if 0
			float real_trq = PMSM_FOC_Get_Real_dqVector() * 0.9f;
			eCtrl_reset_Current(min(real_trq, gFoc_Ctrl.in.s_targetTorque));
			eCtrl_set_TgtCurrent(-PMSM_FOC_GetEbrkTorque());
#else
			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
			eCtrl_set_TgtTorque(-PMSM_FOC_GetEbrkTorque());
#endif
		}	
	}
	
	return gFoc_Ctrl.out.n_RunMode;
}

static void crosszero_step_towards(float *value, float target) {
	float v_now = *value;
	bool cross_zero = false;
	if (target > 0) {
		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET*1.5f) {
			step_towards(value, target, 0.05f);
			cross_zero = true;
		}
	}else if (target == 0) {
		if (v_now >= 0 && v_now <= CONFIG_RAMP_SECOND_TARGET) {
			step_towards(value, target, 0.05f);
			cross_zero = true;
		}
	}else {
		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET*1.5f) {
			step_towards(value, target, 0.02f);
			cross_zero = true;
		}
	}
	if (!cross_zero) {
		*value = target;
	}
}


/* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
#if 1
	gFoc_Ctrl.pi_power.max = maxTrq;
	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.DCCurrLimRamp) - (gFoc_Ctrl.out.s_FilteriDC);
	return PI_Controller_Run(&gFoc_Ctrl.pi_power, errRef);
#else
	return maxTrq;
#endif
}

static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
#ifdef CONFIG_SPEED_LADRC
	float lim = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp);
	ladrc_set_range(&gFoc_Ctrl.vel_lim_adrc, 0, maxTrq);
	return ladrc_run(&gFoc_Ctrl.vel_lim_adrc, lim, gFoc_Ctrl.in.s_motRPM);
#else
#if 1
	gFoc_Ctrl.pi_torque->max = maxTrq;
	gFoc_Ctrl.pi_torque->min = 0;

	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
	return PI_Controller_RunLimit(&gFoc_Ctrl.pi_torque, err);
#else
	return maxTrq;
#endif
#endif
}

static __INLINE void PMSM_FOC_idq_Assign(void) {
	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
		if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
			float s, c;
			normal_sincosf(degree_2_pi(gFoc_Ctrl.in.s_manualAngle + 90.0f), &s, &c);
			gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.in.s_targetCurrent * c;
			
			if (gFoc_Ctrl.in.s_targetIdq.d > gFoc_Ctrl.hwLim.s_FWDCurrMax) {
				gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.hwLim.s_FWDCurrMax;
			}else if (gFoc_Ctrl.in.s_targetIdq.d < -gFoc_Ctrl.hwLim.s_FWDCurrMax) {
				gFoc_Ctrl.in.s_targetIdq.d = -gFoc_Ctrl.hwLim.s_FWDCurrMax;
			}
			gFoc_Ctrl.in.s_targetIdq.q = sqrtf(SQ(gFoc_Ctrl.in.s_targetCurrent) - SQ(gFoc_Ctrl.in.s_targetIdq.d));
		}else {
			gFoc_Ctrl.in.s_targetIdq.d = 0;
			gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent;
		}
	}else if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) ||
				(gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
		motor_mpta_fw_lookup(gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
	}
	u32 mask = cpu_enter_critical();
	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);
	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[1], gFoc_Ctrl.in.s_targetIdq.q);
	cpu_exit_critical(mask);
}

/*called in media task */
void PMSM_FOC_idqCalc(void) {
	if (gFoc_Ctrl.in.b_AutoHold) {
		gFoc_Ctrl.pi_lock.max = CONFIG_MAX_LOCK_TORQUE;
		gFoc_Ctrl.pi_lock.min = -CONFIG_MAX_LOCK_TORQUE;
		float vel_count = motor_encoder_get_vel_count();
		float errRef = 0 - vel_count;
		gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(&gFoc_Ctrl.pi_lock ,errRef);
		PMSM_FOC_idq_Assign();
		return;
	}
	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
		gFoc_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE) {
		float maxTrq = eCtrl_get_RefTorque();
		if (eCtrl_get_FinalTorque() < 0.0001f && gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE) {
			maxTrq = 0;
		}
		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
		float refTorque = min(eCtrl_get_RefTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp));
		float maxTrq = PMSM_FOC_Limit_Speed(refTorque);
		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
		float maxSpeed = eCtrl_get_FinalSpeed();
		float refSpeed = eCtrl_get_RefSpeed();
		if (gFoc_Ctrl.in.b_cruiseEna) {
			maxSpeed = eRamp_get_target(&gFoc_Ctrl.in.cruiseRpmRamp);
			refSpeed = eRamp_get_intepolation(&gFoc_Ctrl.in.cruiseRpmRamp);//gFoc_Ctrl.in.s_cruiseRPM;
		}
#ifdef CONFIG_SPEED_LADRC
		if (maxSpeed >= 0) {
			ladrc_set_range(&gFoc_Ctrl.vel_adrc, -CONFIG_MAX_NEG_TORQUE, eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp));
		}else if (maxSpeed < 0) {
			ladrc_set_range(&gFoc_Ctrl.vel_adrc, -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp), CONFIG_MAX_NEG_TORQUE);
		}

		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
			ladrc_set_range(&gFoc_Ctrl.vel_adrc, 0, 0);
		}
		gFoc_Ctrl.in.s_targetRPM = refSpeed;
		float maxTrq = ladrc_run(&gFoc_Ctrl.vel_adrc, refSpeed, gFoc_Ctrl.in.s_motRPM);
#else
		if (maxSpeed >= 0) {
			gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
			gFoc_Ctrl.pi_speed->min = -CONFIG_MAX_NEG_TORQUE;
		}else if (maxSpeed < 0) {
			gFoc_Ctrl.pi_speed->min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
			gFoc_Ctrl.pi_speed->max = CONFIG_MAX_NEG_TORQUE;
		}

		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
			gFoc_Ctrl.pi_speed->max = 0;
			gFoc_Ctrl.pi_speed->min = 0; //防止倒转
		}
		gFoc_Ctrl.in.s_targetRPM = refSpeed;
		float errRef = refSpeed - gFoc_Ctrl.in.s_motRPM;
		float maxTrq = PI_Controller_Run(&gFoc_Ctrl.pi_speed, errRef);
#endif
		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_iDC(maxTrq);
	}

	PMSM_FOC_idq_Assign();
}

bool PMSM_FOC_RunTime_Limit(void) {
	bool changed = false;
	float dc_lim = (float)vbus_current_vol_lower_limit();
	float torque_lim = (float)torque_temp_high_limit();

	if (gFoc_Ctrl.protLim.s_iDCLim != dc_lim || gFoc_Ctrl.protLim.s_TorqueLim != torque_lim) {
		gFoc_Ctrl.protLim.s_iDCLim = dc_lim;
		gFoc_Ctrl.protLim.s_TorqueLim = torque_lim;
		changed = true;
	}
	return changed;
}


bool PMSM_FOC_iDC_is_Limited(void) {
	return (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE);
}

bool PMSM_FOC_Torque_is_Limited(void) {
	return (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE);
}

void PMSM_FOC_Slow_Task(void) {
	eRamp_running(&gFoc_Ctrl.rtLim.torqueLimRamp);
	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
	eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
	PMSM_FOC_idqCalc();
}

float PMSM_FOC_Get_Real_dqVector(void) {
	if (gFoc_Ctrl.out.s_RealCurrentFiltered == 0) {
		gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
	}
	return gFoc_Ctrl.out.s_RealCurrentFiltered;
}

PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
	return &gFoc_Ctrl;
}

void PMSM_FOC_Start(u8 nCtrlMode) {
	if (gFoc_Ctrl.in.b_motEnable) {
		return;
	}
	PMSM_FOC_CoreInit();
	eCtrl_Reset();
	gFoc_Ctrl.in.n_ctlMode = nCtrlMode;
	gFoc_Ctrl.in.b_motEnable = true;
}

void PMSM_FOC_Stop(void) {
	if (!gFoc_Ctrl.in.b_motEnable) {
		return;
	}
	PMSM_FOC_CoreInit();
	gFoc_Ctrl.in.b_motEnable = false;
}

bool PMSM_FOC_Is_Start(void) {
	return gFoc_Ctrl.in.b_motEnable;
}

void PMSM_FOC_DCCurrLimit(float ibusLimit) {
	if (ibusLimit > gFoc_Ctrl.hwLim.s_iDCMax) {
		ibusLimit = gFoc_Ctrl.hwLim.s_iDCMax;
	}
	if (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE) {
		ibusLimit = min(ibusLimit, gFoc_Ctrl.protLim.s_iDCLim);
	}
	gFoc_Ctrl.userLim.s_iDCLim = ibusLimit;

	if (ABS(gFoc_Ctrl.in.s_motRPM) <= CONFIG_ZERO_SPEED_RPM){
		eRamp_reset_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, ibusLimit);
	}else {
		eRamp_set_step_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, ibusLimit, CONFIG_eCTRL_STEP_TS);
	}
}

float PMSM_FOC_GetDCCurrLimit(void) {
	return gFoc_Ctrl.userLim.s_iDCLim;
}

void PMSM_FOC_SpeedRampLimit(float speedLimit, float speed) {
	if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
	}
	gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
	if (ABS(speed) <= CONFIG_ZERO_SPEED_RPM) {
		eRamp_reset_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit);
	}else {
		eRamp_set_step_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
	}
}

void PMSM_FOC_SpeedLimit(float speedLimit) {
	PMSM_FOC_SpeedRampLimit(speedLimit, gFoc_Ctrl.in.s_motRPM);
}

void PMSM_FOC_SpeedDirectLimit(float limit) {
	PMSM_FOC_SpeedRampLimit(limit, 0);
}


float PMSM_FOC_GetSpeedLimit(void) {
	return gFoc_Ctrl.userLim.s_motRPMLim;
}

void PMSM_FOC_TorqueLimit(float torqueLimit) {
	if (torqueLimit > gFoc_Ctrl.hwLim.s_torqueMax) {
		torqueLimit = gFoc_Ctrl.hwLim.s_torqueMax;
	}

	if (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE) {
		torqueLimit = min(torqueLimit, gFoc_Ctrl.protLim.s_TorqueLim);
	}

	gFoc_Ctrl.userLim.s_torqueLim = torqueLimit;

	if (ABS(gFoc_Ctrl.in.s_motRPM) <= CONFIG_ZERO_SPEED_RPM){
		eRamp_reset_target(&gFoc_Ctrl.rtLim.torqueLimRamp, torqueLimit);
	}else {
		eRamp_set_step_target(&gFoc_Ctrl.rtLim.torqueLimRamp, torqueLimit, CONFIG_eCTRL_STEP_TS);
	}
}
float PMSM_FOC_GetTorqueLimit(void) {
	return gFoc_Ctrl.userLim.s_torqueLim;
}

void PMSM_FOC_SetEbrkTorque(float phase_curr, float dc_curr) {
	gFoc_Ctrl.userLim.s_TorqueBrkLim = fclamp(phase_curr, 0, nv_get_foc_params()->s_TorqueBrkLim);
	gFoc_Ctrl.userLim.s_iDCeBrkLim = fclamp(dc_curr, 0, nv_get_foc_params()->s_iDCeBrkLim);
}

float PMSM_FOC_GetEbrkTorque(void) {
	return gFoc_Ctrl.userLim.s_TorqueBrkLim;
}

float PMSM_FOC_GetVbusVoltage(void) {
	return gFoc_Ctrl.in.s_vDC;
}

float PMSM_FOC_GetVbusCurrent(void) {
	return gFoc_Ctrl.out.s_FilteriDC;
}

DQ_t* PMSM_FOC_GetDQCurrent(void) {
	return &gFoc_Ctrl.out.s_RealIdq;
}

bool PMSM_FOC_SetCtrlMode(u8 mode) {
	if (mode > CTRL_MODE_EBRAKE) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	gFoc_Ctrl.in.n_ctlMode = mode;
	return true;
}


u8 PMSM_FOC_GetCtrlMode(void) {
	return gFoc_Ctrl.in.n_ctlMode;
}

void PMSM_FOC_PhaseCurrLim(float lim) {
	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
	}
	gFoc_Ctrl.userLim.s_PhaseCurrLim = lim;
}

void PMSM_FOC_RT_PhaseCurrLim(float lim) {
	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
	}
	eRamp_init_target(&gFoc_Ctrl.rtLim.torqueLimRamp, lim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
}

float PMSM_FOC_GetPhaseCurrLim(void) {
	return gFoc_Ctrl.userLim.s_PhaseCurrLim;
}

void PMSM_FOC_SetOpenVdq(float vd, float vq) {
	FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[0], vd);
	FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[1], vq);
}


bool PMSM_FOC_EnableCruise(bool enable) {
	if (enable != gFoc_Ctrl.in.b_cruiseEna) {
		float motSpd = PMSM_FOC_GetSpeed();
		if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
		gFoc_Ctrl.in.s_cruiseRPM = motSpd;
		gFoc_Ctrl.in.b_cruiseEna = enable;
	}
	
	return true;
}

bool PMSM_FOC_PauseCruise(void) {
	gFoc_Ctrl.in.b_cruiseEna = false;
	return true;
}

bool PMSM_FOC_ResumeCruise(void) {
	gFoc_Ctrl.in.b_cruiseEna = true;
	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, PMSM_FOC_GetSpeed(), CONFIG_ACC_TIME, CONFIG_DEC_TIME);
	eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
	return true;
}

bool PMSM_FOC_Is_CruiseEnabled(void) {
	return (gFoc_Ctrl.in.b_cruiseEna && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD));
}

bool PMSM_FOC_Set_Speed(float rpm) {
	if (gFoc_Ctrl.in.b_cruiseEna) {
		return false;
	}
	eCtrl_set_TgtSpeed(min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm));
	return true;
}


bool PMSM_FOC_Set_Current(float is) {
	if (is > gFoc_Ctrl.userLim.s_PhaseCurrLim) {
		is = gFoc_Ctrl.userLim.s_PhaseCurrLim;
	}else if (is < -gFoc_Ctrl.userLim.s_PhaseCurrLim) {
		is = -gFoc_Ctrl.userLim.s_PhaseCurrLim;
	}
	eCtrl_set_TgtCurrent(is);
	return true;
}

bool PMSM_FOC_Set_Torque(float trq) {
	if (trq > gFoc_Ctrl.userLim.s_torqueLim) {
		trq = gFoc_Ctrl.userLim.s_torqueLim;
	}else if (trq < -gFoc_Ctrl.userLim.s_torqueLim) {
		trq = -gFoc_Ctrl.userLim.s_torqueLim;
	}
	eCtrl_set_TgtTorque(trq);
	return true;
}

void PMSM_FOC_Reset_Torque(void) {
	float real_trq = PMSM_FOC_Get_Real_dqVector();
	eCtrl_reset_Torque(real_trq);
}

bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
	if (PMSM_FOC_Is_CruiseEnabled()) {
		if (rpm < CONFIG_MIN_CRUISE_RPM) {
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		gFoc_Ctrl.in.s_cruiseRPM = min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm);
		eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
		return true;
	}
	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
	return false;
}

void PMSM_FOC_MTPA_Calibrate(bool enable) {
	if (enable) {
		gFoc_Ctrl.in.b_MTPA_calibrate = true;
		gFoc_Ctrl.in.s_manualAngle = 0;
	}else {
		gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
		gFoc_Ctrl.in.b_MTPA_calibrate = false;
	}
}

void PMSM_FOC_Set_Angle(float angle) {
	gFoc_Ctrl.in.s_manualAngle = (angle);
}

void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
	dq->d = gFoc_Ctrl.in.s_targetIdq.d;
	dq->q = gFoc_Ctrl.in.s_targetIdq.q;
}

float PMSM_FOC_GetSpeed(void) {
	return gFoc_Ctrl.in.s_motRPM;
}


void PMSM_FOC_AutoHold(bool lock) {
	if (gFoc_Ctrl.in.b_AutoHold != lock) {
		motor_encoder_lock_pos(lock);
		PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
		if (!lock) {
			//解锁后为了防止倒溜，需要把当前
			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
#ifdef CONFIG_SPEED_LADRC
				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, gFoc_Ctrl.in.s_targetTorque * 1.1f);
#else
				PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque * 1.1f);
#endif
			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
#ifdef CONFIG_SPEED_LADRC
				ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, gFoc_Ctrl.in.s_targetTorque * 1.1f);
#else
				PI_Controller_Reset(&gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque * 1.1f);
#endif
			}
			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
			gFoc_Ctrl.out.f_autohold_trq = gFoc_Ctrl.in.s_targetTorque;
		}else {
			gFoc_Ctrl.out.f_autohold_trq = 0;
		}
		gFoc_Ctrl.in.b_AutoHold = lock;
	}
}


bool PMSM_FOC_AutoHoldding(void) {
	return gFoc_Ctrl.in.b_AutoHold;
}

static PI_Controller *_pid(u8 id) {
	PI_Controller *pi = NULL;
	if (id == PID_D_id) {
		pi = &gFoc_Ctrl.pi_id;
	}else if (id == PID_Q_id) {
		pi = &gFoc_Ctrl.pi_iq;
	}else if (id == PID_TRQ_id) {
#ifndef CONFIG_SPEED_LADRC
		pi = &gFoc_Ctrl.pi_torque;
#endif
	}else if (id == PID_Spd_id) {
#ifndef CONFIG_SPEED_LADRC
		pi = &gFoc_Ctrl.pi_speed;
#endif
	}
	return pi;
}

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

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

void PMSM_FOC_SetErrCode(u8 error) {
	if (gFoc_Ctrl.out.n_Error != error) {
		gFoc_Ctrl.out.n_Error = error;
	}
}

u8 PMSM_FOC_GetErrCode(void) {
	return gFoc_Ctrl.out.n_Error;
}


void PMSM_FOC_Set_PlotType(Plot_t t) {
	gFoc_Ctrl.plot_type = t;
}
//获取母线电流和实际输出电流矢量大小
void PMSM_FOC_Calc_Current(void) {
	float vd = gFoc_Ctrl.out.s_OutVdq.d;
	float vq = gFoc_Ctrl.out.s_OutVdq.q;

	float id = gFoc_Ctrl.out.s_FilterIdq.d;
	float iq = gFoc_Ctrl.out.s_FilterIdq.q;
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 2/3(iq x vq + id x vd);
	*/
	float m_pow = (vd * id + vq * iq); //s32q10
	float raw_idc = m_pow / get_vbus_float();// * 1.5f * 0.66f; //s16q5
	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC, raw_idc, 0.1f);

	raw_idc = get_vbus_current();
	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.1f);

	gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));

}

void PMSM_FOC_Brake(bool brake) {
	gFoc_Ctrl.in.b_eBrake = brake;
	if (gFoc_Ctrl.in.b_eBrake & gFoc_Ctrl.in.b_cruiseEna) {
		gFoc_Ctrl.in.b_cruiseEna = false;
	}
	eCtrl_brake_signal(brake);
}