aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: MOT_STR_THR_MIX adjusts steering vs throttle priority

Browse Source
This commit is contained in:
Randy Mackay 2020-07-31 10:01:13 +09:00
parent 77df7c7a39
commit 98b953eb80
2 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Rover/AP_MotorsUGV.cpp
View File

@ -96,6 +96,13 @@ const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = {
// @User: Advanced
AP_GROUPINFO("SPD_SCA_BASE", 11, AP_MotorsUGV, _speed_scale_base, 1.0f),
// @Param: STR_THR_MIX
// @DisplayName: Motor steering vs throttle prioritisation
// @Description: Steering vs Throttle priorisation. Higher numbers prioritise steering, lower numbers prioritise throttle. Only valid for Skid Steering vehicles
// @Range: 0.2 1.0
// @User: Advanced
AP_GROUPINFO("STR_THR_MIX", 12, AP_MotorsUGV, _steering_throttle_mix, 0.5f),
AP_GROUPEND
};
@ -671,8 +678,17 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
// check for saturation and scale back throttle and steering proportionally
const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled);
if (saturation_value > 1.0f) {
steering_scaled = steering_scaled / saturation_value;
throttle_scaled = throttle_scaled / saturation_value;
const float str_thr_mix = constrain_float(_steering_throttle_mix, 0.0f, 1.0f);
const float fair_scaler = 1.0f / saturation_value;
if (str_thr_mix >= 0.5f) {
// prioritise steering over throttle
steering_scaled *= linear_interpolate(fair_scaler, 1.0f, str_thr_mix, 0.5f, 1.0f);
throttle_scaled = (1.0f - fabsf(steering_scaled)) * (is_negative(throttle_scaled) ? -1.0f : 1.0f);
} else {
// prioritise throttle over steering
throttle_scaled *= linear_interpolate(fair_scaler, 1.0f, 0.5f - str_thr_mix, 0.0f, 0.5f);
steering_scaled = (1.0f - fabsf(throttle_scaled)) * (is_negative(steering_scaled) ? -1.0f : 1.0f);
}
}
// add in throttle and steering

1
Rover/AP_MotorsUGV.h
View File

@ -173,6 +173,7 @@ protected:
AP_Float _thrust_curve_expo; // thrust curve exponent from -1 to +1 with 0 being linear
AP_Float _vector_throttle_base; // throttle level above which steering is scaled down when using vector thrust. zero to disable vectored thrust
AP_Float _speed_scale_base; // speed above which steering is scaled down when using regular steering/throttle vehicles. zero to disable speed scaling
AP_Float _steering_throttle_mix; // Steering vs Throttle priorisation. Higher numbers prioritise steering, lower numbers prioritise throttle. Only valid for Skid Steering vehicles
// internal variables
float _steering; // requested steering as a value from -4500 to +4500