去掉“先不区分加减速，统一通过油门开度获取当前扭矩指令”的提交，改为以前的方式

Signed-off-by: kevin <huhui@sharkgulf.com>

Applications/foc/motor/throttle.c

@@ -20,8 +20,8 @@ static throttle_torque_t _throttle;
 
 void throttle_torque_reset(void) {
 	_throttle.accl = false;
-	_throttle.throttle_opening = _throttle.throttle_opening_last = 0.0f;
-	_throttle.torque_req = _throttle.torque_real = _throttle.torque_acc_ = 0.0f;
+	_throttle.pedal_opening = _throttle.prev_pedal_opening = 0.0f;
+	_throttle.torque_req = _throttle.torque_real = _throttle.torque_compensation = 0.0f;
 	_throttle.gear = mc_get_internal_gear();
 }
 
@@ -128,17 +128,13 @@ void throttle_force_detect(void) {
 }
 
 /* 获取转把电压对应的油门开度 */
-#define THRO_CURVE_K  (-1.5f)
-
 float throttle_vol_to_opening(float thro_val) {
 	if (thro_val <= throttle_start_vol()) {
 		return 0;
 	}
 	float delta = thro_val - throttle_start_vol();
 	int ration = (delta * 100.0f) / throttle_vol_range();
-	float opening = ((float)ration)/100.0f;
-	float curve = throttle_curve(THRO_CURVE_K, opening);
-	return curve;
+	return ((float)ration)/100.0f;
 }
 
 /* 获取油门开度 */
@@ -210,25 +206,89 @@ void throttle_detect(bool ready) {
 	}
 }
 
+static float _throttle_torque_for_accelerate(float ration) {
+	float max_torque = mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear);
+	float thro_torque = max_torque * ration;
+
+	float pedal_acc = 1.0f;
+	if (_throttle.prev_pedal_opening < 1.0f) {
+		pedal_acc = (ration - _throttle.prev_pedal_opening)/ (1.0f - _throttle.prev_pedal_opening);
+	}
+	pedal_acc = fclamp(pedal_acc, 0, 1.0f);
+	float acc_torque = _throttle.torque_real + pedal_acc * (max_torque - _throttle.torque_real);
+	if (acc_torque < 0) {
+		acc_torque = 0;
+	}
+	/*
+	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
+	*/
+	float torque_compensation = thro_torque - acc_torque;
+	float step = 0.0f;
+	if (torque_compensation > 0) {
+		float acc_t = mc_gear_conf()->accl_time;
+		step = torque_compensation / (acc_t + 0.00001f);
+	}else {
+		torque_compensation = 0;
+	}
+	step_towards(&_throttle.torque_compensation, torque_compensation, step);
+	return (acc_torque + _throttle.torque_compensation);
+}
+
+
+static float throttle_torque_for_accelerate(void) {
+	return _throttle_torque_for_accelerate(_throttle.pedal_opening);
+}
+
+static float throttle_torque_for_decelerate(void) {
+	if (_throttle.prev_pedal_opening == 0.0f) {
+		return 0;
+	}
+	float pedal_dec = _throttle.pedal_opening / _throttle.prev_pedal_opening;
+	pedal_dec = fclamp(pedal_dec, 0.0f, 1.0f);
+	return pedal_dec * _throttle.torque_real;
+}
+
 float throttle_get_open_ration_filted(void) {
-	return _throttle.throttle_opening;
+	return _throttle.pedal_opening;
 }
 
 #define THRO_RPM_LP_CEOF 0.01f
 float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
-	float throttle_opening = throttle_vol_to_opening(vol);
+	float pedal_opening = throttle_vol_to_opening(vol);
 	float vel = mot_contrl_get_speed(ctrl);
 	_throttle.gear = mc_get_internal_gear();
 	LowPass_Filter(_throttle.vel_filted, vel, THRO_RPM_LP_CEOF);
-	if (throttle_opening > _throttle.throttle_opening) {
+
+	if (pedal_opening > _throttle.pedal_opening) {
+		if (!_throttle.accl) {
+			_throttle.prev_pedal_opening = _throttle.pedal_opening;
+			_throttle.torque_real = _throttle.torque_req;
+			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
+				_throttle.torque_real = 0;
+			}
+			_throttle.torque_compensation = 0;
+		}
 		_throttle.accl = true;
-	}else if (throttle_opening < _throttle.throttle_opening) {
+	}else if (pedal_opening < _throttle.pedal_opening) {
+		if (_throttle.accl) {
+			_throttle.prev_pedal_opening = _throttle.pedal_opening;
+			_throttle.torque_real = ctrl->target_torque_raw;
+			/* 如果扭矩给定的ramp没有结束，使用原始扭矩请求作为减扭矩的起始点 */
+			if (_throttle.torque_req - line_ramp_get_interp(&ctrl->ramp_input_torque) >= 10.0f ) {
+				_throttle.torque_real = _throttle.torque_req;
+			}
+			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
+				_throttle.torque_real = 0;
+			}
+		}
 		_throttle.accl = false;
 	}
-	_throttle.throttle_opening = throttle_opening;
-
-	float max_torque = mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear);
-	return max_torque * throttle_opening;
+	_throttle.pedal_opening = pedal_opening;
+	if (_throttle.accl) {
+		return throttle_torque_for_accelerate();
+	}else {
+		return throttle_torque_for_decelerate();
+	}
 }
 
 void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
@@ -269,6 +329,7 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
 		}
 		_throttle.torque_req = ref_torque;
 	}else {
+		float ref_torque = throttle_torque_for_decelerate();
 		/* autohold 启动的情况下，转把在0位置附近小幅抖动 */
 		if (curr_vel <= CONFIG_ZERO_SPEED_RPM) {
 			float hold_torque = ctrl->autohold_torque;
@@ -281,12 +342,14 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
 
 /* 定速巡航需要判断是否需要加速 */
 float get_user_request_torque(void) {
-	float max_torque = mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear);
-	return max_torque * _throttle.throttle_opening;
+	if (_throttle.accl) {
+		return throttle_torque_for_accelerate();
+	}
+	return throttle_torque_for_decelerate();
 }
 
 
 void throttle_log(void) {
-	sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.throttle_opening, _throttle.throttle_opening_last, _throttle.torque_real, _throttle.torque_req);
+	sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.pedal_opening, _throttle.prev_pedal_opening, _throttle.torque_real, _throttle.torque_req);
 	sys_debug("thro: %f-%f, %f\n", throttle_start_vol(), throttle_end_vol(), mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear));
 }

Applications/foc/motor/throttle.h

@@ -10,10 +10,10 @@ typedef struct {
 	bool  accl;
 	float torque_req;
 	float torque_real;
-	float torque_acc_;
+	float torque_compensation;
 	float vel_filted;
-	float throttle_opening;
-	float throttle_opening_last;
+	float pedal_opening;
+	float prev_pedal_opening;
 	u8    gear;
 }throttle_torque_t;