|
@@ -20,7 +20,7 @@ static throttle_torque_t _throttle;
|
|
|
|
|
|
|
|
void throttle_torque_reset(void) {
|
|
void throttle_torque_reset(void) {
|
|
|
_throttle.accl = false;
|
|
_throttle.accl = false;
|
|
|
- _throttle.thro_ration = _throttle.thro_ration_last = 0.0f;
|
|
|
|
|
|
|
+ _throttle.throttle_opening = _throttle.throttle_opening_last = 0.0f;
|
|
|
_throttle.torque_req = _throttle.torque_real = _throttle.torque_acc_ = 0.0f;
|
|
_throttle.torque_req = _throttle.torque_real = _throttle.torque_acc_ = 0.0f;
|
|
|
_throttle.gear = mc_get_internal_gear();
|
|
_throttle.gear = mc_get_internal_gear();
|
|
|
}
|
|
}
|
|
@@ -126,7 +126,7 @@ void throttle_force_detect(void) {
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* 获取转把电压对应的油门开度 */
|
|
/* 获取转把电压对应的油门开度 */
|
|
|
-float throttle_vol_to_open_ration(float thro_val) {
|
|
|
|
|
|
|
+float throttle_vol_to_opening(float thro_val) {
|
|
|
if (thro_val <= throttle_start_vol()) {
|
|
if (thro_val <= throttle_start_vol()) {
|
|
|
return 0;
|
|
return 0;
|
|
|
}
|
|
}
|
|
@@ -138,11 +138,11 @@ float throttle_vol_to_open_ration(float thro_val) {
|
|
|
/* 获取油门开度 */
|
|
/* 获取油门开度 */
|
|
|
float throttle_get_open_ration(void) {
|
|
float throttle_get_open_ration(void) {
|
|
|
float thro_val = throttle_get_signal();
|
|
float thro_val = throttle_get_signal();
|
|
|
- return throttle_vol_to_open_ration(thro_val);
|
|
|
|
|
|
|
+ return throttle_vol_to_opening(thro_val);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* 获取油门开度对应的转把电压 */
|
|
/* 获取油门开度对应的转把电压 */
|
|
|
-float throttle_open_ration_to_vol(float r) {
|
|
|
|
|
|
|
+float throttle_opening_to_vol(float r) {
|
|
|
if (r == 0) {
|
|
if (r == 0) {
|
|
|
return 0;
|
|
return 0;
|
|
|
}else if (r > 1.0f) {
|
|
}else if (r > 1.0f) {
|
|
@@ -209,8 +209,8 @@ static float _throttle_torque_for_accelerate(float ration) {
|
|
|
float thro_torque = max_torque * ration;
|
|
float thro_torque = max_torque * ration;
|
|
|
|
|
|
|
|
float acc_r = 1.0f;
|
|
float acc_r = 1.0f;
|
|
|
- if (_throttle.thro_ration_last < 1.0f) {
|
|
|
|
|
- acc_r = (ration - _throttle.thro_ration_last)/ (1.0f - _throttle.thro_ration_last);
|
|
|
|
|
|
|
+ if (_throttle.throttle_opening_last < 1.0f) {
|
|
|
|
|
+ acc_r = (ration - _throttle.throttle_opening_last)/ (1.0f - _throttle.throttle_opening_last);
|
|
|
}
|
|
}
|
|
|
acc_r = fclamp(acc_r, 0, 1.0f);
|
|
acc_r = fclamp(acc_r, 0, 1.0f);
|
|
|
float acc_torque = _throttle.torque_real + acc_r * (max_torque - _throttle.torque_real);
|
|
float acc_torque = _throttle.torque_real + acc_r * (max_torque - _throttle.torque_real);
|
|
@@ -234,32 +234,32 @@ static float _throttle_torque_for_accelerate(float ration) {
|
|
|
|
|
|
|
|
|
|
|
|
|
static float throttle_torque_for_accelerate(void) {
|
|
static float throttle_torque_for_accelerate(void) {
|
|
|
- return _throttle_torque_for_accelerate(_throttle.thro_ration);
|
|
|
|
|
|
|
+ return _throttle_torque_for_accelerate(_throttle.throttle_opening);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static float throttle_torque_for_decelerate(void) {
|
|
static float throttle_torque_for_decelerate(void) {
|
|
|
- if (_throttle.thro_ration_last == 0.0f) {
|
|
|
|
|
|
|
+ if (_throttle.throttle_opening_last == 0.0f) {
|
|
|
return 0;
|
|
return 0;
|
|
|
}
|
|
}
|
|
|
- float dec_r = _throttle.thro_ration / _throttle.thro_ration_last;
|
|
|
|
|
|
|
+ float dec_r = _throttle.throttle_opening / _throttle.throttle_opening_last;
|
|
|
dec_r = fclamp(dec_r, 0.0f, 1.0f);
|
|
dec_r = fclamp(dec_r, 0.0f, 1.0f);
|
|
|
return dec_r * _throttle.torque_real;
|
|
return dec_r * _throttle.torque_real;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
float throttle_get_open_ration_filted(void) {
|
|
float throttle_get_open_ration_filted(void) {
|
|
|
- return _throttle.thro_ration;
|
|
|
|
|
|
|
+ return _throttle.throttle_opening;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
#define THRO_RPM_LP_CEOF 0.01f
|
|
#define THRO_RPM_LP_CEOF 0.01f
|
|
|
float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
|
|
float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
|
|
|
- float thro_r = throttle_vol_to_open_ration(vol);
|
|
|
|
|
|
|
+ float throttle_opening = throttle_vol_to_opening(vol);
|
|
|
float vel = mot_contrl_get_speed(ctrl);
|
|
float vel = mot_contrl_get_speed(ctrl);
|
|
|
_throttle.gear = mc_get_internal_gear();
|
|
_throttle.gear = mc_get_internal_gear();
|
|
|
LowPass_Filter(_throttle.vel_filted, vel, THRO_RPM_LP_CEOF);
|
|
LowPass_Filter(_throttle.vel_filted, vel, THRO_RPM_LP_CEOF);
|
|
|
|
|
|
|
|
- if (thro_r > _throttle.thro_ration) {
|
|
|
|
|
|
|
+ if (throttle_opening > _throttle.throttle_opening) {
|
|
|
if (!_throttle.accl) {
|
|
if (!_throttle.accl) {
|
|
|
- _throttle.thro_ration_last = _throttle.thro_ration;
|
|
|
|
|
|
|
+ _throttle.throttle_opening_last = _throttle.throttle_opening;
|
|
|
_throttle.torque_real = _throttle.torque_req;
|
|
_throttle.torque_real = _throttle.torque_req;
|
|
|
if (_throttle.torque_real < 0) { //电子刹车的时候,扭矩可能为负
|
|
if (_throttle.torque_real < 0) { //电子刹车的时候,扭矩可能为负
|
|
|
_throttle.torque_real = 0;
|
|
_throttle.torque_real = 0;
|
|
@@ -267,9 +267,9 @@ float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
|
|
|
_throttle.torque_acc_ = 0;
|
|
_throttle.torque_acc_ = 0;
|
|
|
}
|
|
}
|
|
|
_throttle.accl = true;
|
|
_throttle.accl = true;
|
|
|
- }else if (thro_r < _throttle.thro_ration) {
|
|
|
|
|
|
|
+ }else if (throttle_opening < _throttle.throttle_opening) {
|
|
|
if (_throttle.accl) {
|
|
if (_throttle.accl) {
|
|
|
- _throttle.thro_ration_last = _throttle.thro_ration;
|
|
|
|
|
|
|
+ _throttle.throttle_opening_last = _throttle.throttle_opening;
|
|
|
_throttle.torque_real = ctrl->target_torque_raw;
|
|
_throttle.torque_real = ctrl->target_torque_raw;
|
|
|
/* 如果扭矩给定的ramp没有结束,使用原始扭矩请求作为减扭矩的起始点 */
|
|
/* 如果扭矩给定的ramp没有结束,使用原始扭矩请求作为减扭矩的起始点 */
|
|
|
if (_throttle.torque_req - line_ramp_get_interp(&ctrl->input_torque) >= 10.0f ) {
|
|
if (_throttle.torque_req - line_ramp_get_interp(&ctrl->input_torque) >= 10.0f ) {
|
|
@@ -281,7 +281,7 @@ float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
|
|
|
}
|
|
}
|
|
|
_throttle.accl = false;
|
|
_throttle.accl = false;
|
|
|
}
|
|
}
|
|
|
- _throttle.thro_ration = thro_r;
|
|
|
|
|
|
|
+ _throttle.throttle_opening = throttle_opening;
|
|
|
if (_throttle.accl) {
|
|
if (_throttle.accl) {
|
|
|
return throttle_torque_for_accelerate();
|
|
return throttle_torque_for_accelerate();
|
|
|
}else {
|
|
}else {
|
|
@@ -348,6 +348,6 @@ float get_user_request_torque(void) {
|
|
|
|
|
|
|
|
|
|
|
|
|
void throttle_log(void) {
|
|
void throttle_log(void) {
|
|
|
- sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.thro_ration, _throttle.thro_ration_last, _throttle.torque_real, _throttle.torque_req);
|
|
|
|
|
|
|
+ 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("thro: %f-%f, %f\n", throttle_start_vol(), throttle_end_vol(), mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear));
|
|
sys_debug("thro: %f-%f, %f\n", throttle_start_vol(), throttle_end_vol(), mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear));
|
|
|
}
|
|
}
|