MC100/Applications/foc/core/thro_torque.c

#include "foc/core/thro_torque.h"
#include "foc/foc_config.h"
#include "foc/motor/motor.h"
#include "foc/motor/motor_param.h"
#include "foc/core/e_ctrl.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "app/nv_storage.h"
#include "libs/logger.h"
#include "prot/can_foc_msg.h"

static thro_torque_t _torque;

void thro_torque_reset(void) {
	_torque.accl = false;
	_torque.thro_filted = 0.0f;
	_torque.thro_ration = _torque.thro_ration_last = 0.0f;
	_torque.torque_req = _torque.torque_real = _torque.torque_acc_ = 0.0f;
	_torque.gear = mc_get_internal_gear();
}

void thro_torque_init(void) {
	thro_torque_reset();
	_torque.spd_filted = 0.0f;
}

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

float thro_torque_gear_map(s16 rpm, u8 gear) {
	mc_gear_t *_current_gear = mc_get_gear_config_by_gear(gear);
	if (_current_gear == NULL) {
		return 0;
	}

	if (rpm > _current_gear->n_max_speed) {
		rpm = _current_gear->n_max_speed;
	}
	if (rpm <= 1000) {
		return gear_rpm_torque(_current_gear->n_torque[0], _current_gear->n_max_trq);
	}

	for (int i = 1; i < GEAR_SPEED_TRQ_NUM; i++) {
		if (rpm <= 1000 * (i + 1)) { //线性插值
			float trq1 = gear_rpm_torque(_current_gear->n_torque[i-1], _current_gear->n_max_trq);
			float min_rpm = (i * 1000);
			float trq2 = gear_rpm_torque(_current_gear->n_torque[i], _current_gear->n_max_trq);
			float max_rpm = (i + 1) * 1000;
			if (trq1 > trq2) {
				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
			}else {
				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
			}
		}
	}
	return gear_rpm_torque(_current_gear->n_torque[GEAR_SPEED_TRQ_NUM-1], _current_gear->n_max_trq);
}

/* 获取油门开度 */
static float thro_ration(float f_throttle) {
	if (f_throttle <= nv_get_foc_params()->n_startThroVol) {
		return 0;
	}
	float delta = f_throttle - (nv_get_foc_params()->n_startThroVol);
	int ration = (delta * 100.0f) / (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);	
	return ((float)ration)/100.0f;
}

float thro_ration_to_voltage(float r) {
	if (r == 0) {
		return 0;
	}
	float vol = nv_get_foc_params()->n_startThroVol + r * (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);
	return fclamp(vol, 0, nv_get_foc_params()->n_endThroVol);
}

float thro_get_ration(float f_thro) {
	return thro_ration(f_thro);
}

static float _thro_torque_for_accelerate(float ration) {
	float max_torque = thro_torque_gear_map((s16)_torque.spd_filted, _torque.gear);
	float thro_torque = max_torque * ration;

	float acc_r = 1.0f;
	if (_torque.thro_ration_last < 1.0f) {
		acc_r = (ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
	}
	acc_r = fclamp(acc_r, 0, 1.0f);
	float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);
	if (acc_torque < 0) {
		acc_torque = 0;
	}
	/*
	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
	   如果差值在正负5以上，需要step 补偿
	*/
	float torque_acc_ = thro_torque - acc_torque;
	float step = 0.0f;
	if (ABS(torque_acc_) < 5) {
		torque_acc_ = 0;
	}else {
		float acc_t = mc_get_gear_config()->n_accl_time;
		step = torque_acc_ / (acc_t + 0.00001f);
	}
	step_towards(&_torque.torque_acc_, torque_acc_, step);
	return (acc_torque + _torque.torque_acc_);
}


static float thro_torque_for_accelerate(void) {
	return _thro_torque_for_accelerate(_torque.thro_ration);
}

static float thro_torque_for_decelerate(void) {
	if (_torque.thro_ration_last == 0.0f) {
		return 0;
	}
	float dec_r = _torque.thro_ration / _torque.thro_ration_last;
	dec_r = fclamp(dec_r, 0.0f, 1.0f);
	return dec_r * _torque.torque_real;
}

static void thro_torque_request(void) {
#ifdef CONFIG_CRUISE_ENABLE_ACCL
	if (mc_is_cruise_enabled() && mc_throttle_released()) {
		return;
	}
#endif
	if (mc_throttle_released() && eCtrl_enable_eBrake(true)) {
		return;
	}
	if (!mc_throttle_released()) {
		float curr_rpm = PMSM_FOC_GetSpeed();
		if (_torque.accl) { //加速需求
			float ref_torque = thro_torque_for_accelerate();
			float hold_torque = PMSM_FOC_Get()->out.f_autohold_trq;
			if (hold_torque < 0) { //下坡驻车,最小给0扭矩
				hold_torque = 0;
			}
			ref_torque	= MAX(hold_torque, ref_torque);
			if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
				if (_torque.torque_req < hold_torque) {
					_torque.torque_req = hold_torque;
					eCtrl_reset_Torque(hold_torque);
				}
			}else {
				PMSM_FOC_Get()->out.f_autohold_trq = 0;
			}
			/* 处理加速ramp时间的变化，需要缓慢变小，变大可以立即处理，
			*  加速时间缓慢变小可以防止突然大扭矩加速
			*/
			u16 now_ramp_time = eCtrl_get_torque_acc_time();
			u16 next_ramp_time = mc_get_gear_config()->n_accl_time;
			if (curr_rpm < CONFIG_ZERO_SPEED_RAMP_RMP) {
				next_ramp_time = mc_get_gear_config()->n_zero_accl;
			}
			if (next_ramp_time == 0) {
				next_ramp_time = 100;
			}
			if (now_ramp_time != next_ramp_time) {
				if (next_ramp_time > now_ramp_time) {
					eCtrl_set_accl_time(next_ramp_time);
				}else {
					float f_now = (float)now_ramp_time;
					float f_next = (float)next_ramp_time;
					step_towards(&f_now, f_next, 0.5f);
					if (f_now <= 10) {
						f_now = 10;
					}
					eCtrl_set_accl_time((u16)f_now);
				}
			}
			_torque.torque_req = ref_torque;
		}else {
			float ref_torque = thro_torque_for_decelerate();
			/* autohold 启动的情况下，转把在0位置附近小幅抖动 */
			if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
				float hold_torque = PMSM_FOC_Get()->out.f_autohold_trq;
				ref_torque = MAX(hold_torque, ref_torque);
			}
			_torque.torque_req = ref_torque;
		}
		PMSM_FOC_Set_Torque(_torque.torque_req);
	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
		_torque.torque_req = 0.0f;
		PMSM_FOC_Set_Torque(_torque.torque_req);
	}
}

#define THRO_REF_LP_CEOF 0.2f
static void thro_torque_filter(float f_throttle) {
	if (mc_throttle_released()){
		thro_torque_reset();
	}else {
		LowPass_Filter(_torque.thro_filted, f_throttle, THRO_REF_LP_CEOF);
	}
	float curr_rpm = PMSM_FOC_GetSpeed();
	if (curr_rpm == 0) {
		_torque.spd_filted = 0;
	}else {
		LowPass_Filter(_torque.spd_filted, curr_rpm, 0.01f);
	}
}
static float acc_r, acc_r_last, acc_trq;
static float dec_r, dec_r_last, dec_trq;
void thro_torque_process(u8 run_mode, float f_throttle) {

	thro_torque_filter(f_throttle);

	float thro_r = thro_ration(_torque.thro_filted);
	_torque.gear = mc_get_internal_gear();

	if (thro_r > _torque.thro_ration) {
		if (!_torque.accl) {
			_torque.thro_ration_last = _torque.thro_ration;
			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
				_torque.torque_real = 0;
			}
			_torque.torque_acc_ = 0;
			acc_r = thro_r;
			acc_r_last = _torque.thro_ration_last;
			acc_trq = _torque.torque_real;
		}
		_torque.accl = true;
	}else if (thro_r < _torque.thro_ration) {
		if (_torque.accl) {
			_torque.thro_ration_last = _torque.thro_ration;
			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
				_torque.torque_real = 0;
			}
			dec_r = thro_r;
			dec_r_last = _torque.thro_ration_last;
			dec_trq = _torque.torque_real;
		}
		_torque.accl = false;
	}

	_torque.thro_ration = thro_r;
	if (run_mode == CTRL_MODE_TRQ) {
		thro_torque_request();
	}else if (run_mode == CTRL_MODE_SPD) {
		if (!mc_is_cruise_enabled()) {
			float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.thro_ration;
			PMSM_FOC_Set_Speed(speed_Ref);
		}
	}else if (run_mode == CTRL_MODE_EBRAKE) {
		float vel = PMSM_FOC_GetSpeed();
		float ebrk_trq = motor_get_ebreak_toruqe(vel);

		if (ebrk_trq >= -PMSM_FOC_GetEbrkTorque()/2){
			eCtrl_Set_eBrk_RampTime(1);
		}
		if (ebrk_trq != 0) {
			eCtrl_set_TgtTorque(ebrk_trq);
		}

		if (eCtrl_get_FinalTorque() < 0.0001f && vel < CONFIG_MIN_RPM_EXIT_EBRAKE) {
			eCtrl_enable_eBrake(false);
			thro_torque_reset();
		}
		if (!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f))) {
			eCtrl_enable_eBrake(false);
			thro_torque_reset();
		}
	}
}

/* 定速巡航需要判断是否需要加速 */
float get_user_request_torque(void) {
	if (_torque.accl) {
		return thro_torque_for_accelerate();
	}
	return thro_torque_for_decelerate();
}

void thro_torque_log(void) {
	sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
	sys_debug("ration %f - %f - %f - %d\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate(), _torque.gear);
	sys_debug("acc:%f,%f,%f\n", acc_r, acc_r_last, acc_trq);
	sys_debug("dec:%f,%f,%f\n", dec_r, dec_r_last, dec_trq);
}