MC100/Applications/foc/core/e_ctrl.c

#include "e_ctrl.h"
#include "foc/foc_config.h"
#include "math/fix_math.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "app/nv_storage.h"
#include "libs/logger.h"

e_Ctrl g_eCtrl;
static void _eCtrl_set_TgtCurrent(float c);
static void _eCtrl_set_TgtSpeed(float s);
static void _eCtrl_set_TgtTorque(float t);
static bool _eCtrl_isHwBrk_shutPower(void);

void eCtrl_init(u16 accl_time, u16 dec_time){
	g_eCtrl.dec_time = dec_time;
	g_eCtrl.accl_time = accl_time;
	if (g_eCtrl.accl_time == 0) {
		g_eCtrl.accl_time = nv_get_foc_params()->n_acc_time;
	}
	if (g_eCtrl.dec_time == 0) {
		g_eCtrl.dec_time = nv_get_foc_params()->n_dec_time;
	}
	g_eCtrl.ebrk_time = nv_get_foc_params()->n_ebrk_time;
	
	g_eCtrl.ebrk_time_shadow = g_eCtrl.ebrk_time;
	g_eCtrl.accl_time_shadow = g_eCtrl.accl_time;
	g_eCtrl.dec_time_shadow = g_eCtrl.dec_time;
	g_eCtrl.hw_brake = false;
	g_eCtrl.is_ebrake_shadow = g_eCtrl.is_ebrake = false;
	eRamp_init(&g_eCtrl.current, g_eCtrl.ebrk_time, g_eCtrl.ebrk_time);
	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
}

void eCtrl_set_ebrk_time(u16 ebrk_time) {
	g_eCtrl.ebrk_time_shadow = ebrk_time;
}


void eCtrl_set_TgtCurrent(float c) {
	g_eCtrl.current_shadow = c;
}

void eCtrl_set_TgtTorque(float t) {
	g_eCtrl.torque_shadow = t;
}

void eCtrl_reset_Torque(float init_trq) {
	g_eCtrl.torque_shadow = init_trq;
	eRamp_reset_target(&g_eCtrl.torque, init_trq);
}

void eCtrl_reset_Current(float init_curr) {
	g_eCtrl.current_shadow = init_curr;
	eRamp_reset_target(&g_eCtrl.current, init_curr);
}

void eCtrl_set_TgtSpeed(float s) {
	g_eCtrl.speed_shadow = s;
}

bool eCtrl_enable_eBrake(bool enable) {
	if (PMSM_FOC_GeteBrkPhaseCurrent() == 0.0f) {
		g_eCtrl.is_ebrake_shadow = false;
		return g_eCtrl.is_ebrake_shadow;
	}
	if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE))) {
		g_eCtrl.is_ebrake_shadow = true;
	}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
		g_eCtrl.is_ebrake_shadow = false;
	}
	return g_eCtrl.is_ebrake_shadow;
}

static void _eCtrl_clear_ramp(void) {
	eRamp_init(&g_eCtrl.current, nv_get_foc_params()->n_ebrk_time, nv_get_foc_params()->n_ebrk_time);
	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
	g_eCtrl.current_shadow = 0.0f;
	g_eCtrl.torque_shadow = 0.0f;
	g_eCtrl.speed_shadow = 0.0f;
}

void _eCtrl_process_eBrake(void) {
	if (g_eCtrl.is_ebrake) {
		PMSM_FOC_SetCtrlMode(CTRL_MODE_CURRENT_BRK);
		eRamp_init(&g_eCtrl.current, nv_get_foc_params()->n_ebrk_time, nv_get_foc_params()->n_ebrk_time);
		g_eCtrl.current_shadow = 0.0f;
	}else {
		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
	}
}

bool eCtrl_is_eBrk_enabled(void) {
	return (g_eCtrl.is_ebrake || g_eCtrl.is_ebrake_shadow);
}

void eCtrl_Running(void) {
	if (g_eCtrl.accl_time_shadow != g_eCtrl.accl_time || g_eCtrl.dec_time_shadow != g_eCtrl.dec_time) {
		g_eCtrl.dec_time = g_eCtrl.dec_time_shadow;
		g_eCtrl.accl_time = g_eCtrl.accl_time_shadow;
		/* 重新计算ramp */
		eRamp_set_time(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
		eRamp_set_time(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
	}
	if (g_eCtrl.ebrk_time_shadow != g_eCtrl.ebrk_time) {
		g_eCtrl.ebrk_time = g_eCtrl.ebrk_time_shadow;
		eRamp_set_time(&g_eCtrl.current, g_eCtrl.ebrk_time, g_eCtrl.ebrk_time);
		_eCtrl_set_TgtCurrent(g_eCtrl.current_shadow);
	}
	if (g_eCtrl.current_shadow != g_eCtrl.current.target) {
		_eCtrl_set_TgtCurrent(g_eCtrl.current_shadow);
	}
	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target) {
		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
	}
	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target) {
		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
	}
	if (g_eCtrl.is_ebrake_shadow != g_eCtrl.is_ebrake) {
		g_eCtrl.is_ebrake = g_eCtrl.is_ebrake_shadow;
		_eCtrl_process_eBrake();
	}
	eRamp_running(&g_eCtrl.current);
	eRamp_X2_running(&g_eCtrl.speed);
	eRamp_X2_running(&g_eCtrl.torque);
}

void eCtrl_Reset(void) {
	_eCtrl_clear_ramp();
	g_eCtrl.is_ebrake_shadow = g_eCtrl.is_ebrake = false;
}

static bool _eCtrl_isHwBrk_shutPower(void) {
	return (g_eCtrl.hw_brake && nv_get_foc_params()->n_brkShutPower);
}

static void _eCtrl_set_TgtCurrent(float c) {
	eRamp_set_step_target(&g_eCtrl.current, c, CONFIG_eCTRL_STEP_TS);
}

static void _eCtrl_set_TgtSpeed(float s) {
	if (_eCtrl_isHwBrk_shutPower()){
		return;
	}
	eRamp_set_X2_target(&g_eCtrl.speed, s);
}

static void _eCtrl_set_TgtTorque(float t) {
	if (_eCtrl_isHwBrk_shutPower()){
		return;
	}
	eRamp_set_X2_target(&g_eCtrl.torque, t);
}

float eCtrl_get_RefSpeed(void) {
	return eRamp_get_intepolation(&g_eCtrl.speed);
}

float eCtrl_get_RefCurrent(void) {
	return eRamp_get_intepolation(&g_eCtrl.current);
}

float eCtrl_get_RefTorque(void) {
	return eRamp_get_intepolation(&g_eCtrl.torque);
}

float eCtrl_get_FinalSpeed(void) {
	return eRamp_get_target(&g_eCtrl.speed);
}

float eCtrl_get_FinalCurrent(void) {
	return eRamp_get_target(&g_eCtrl.current);
}

float eCtrl_get_FinalTorque(void) {
	return eRamp_get_target(&g_eCtrl.torque);
}

void eCtrl_brake_signal(bool hw_brake) {
	u32 mask = cpu_enter_critical();
	if (hw_brake != g_eCtrl.hw_brake) {
		g_eCtrl.hw_brake = hw_brake;
		if (hw_brake) {
			g_eCtrl.brake_ts = get_tick_ms();
		}
	}
	if (_eCtrl_isHwBrk_shutPower()) {
		if (!eCtrl_is_eBrk_enabled() && !eCtrl_enable_eBrake(true)) {
			_eCtrl_clear_ramp();
		}
	}
	cpu_exit_critical(mask);
}