Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 09:24:01 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_Motors: skid-steering vehicle limit fix

Browse Source 
we should set the steering and throttle limits flags if we are not getting the desired output
This commit is contained in:
Randy Mackay 2022-02-04 09:35:41 +09:00 committed by Peter Barker
parent f25755a05f
commit 64a1ab9a93
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AR_Motors/AP_MotorsUGV.cpp
View File

@ -750,6 +750,10 @@ 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) {
// store pre-scaled values so we can set limit flags afterwards
const float steering_scaled_orig = steering_scaled;
const float throttle_scaled_orig = throttle_scaled;
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) {
@ -761,6 +765,16 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
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);
}
// update limits if either steering or throttle has been reduced
if (fabsf(steering_scaled) < fabsf(steering_scaled_orig)) {
limit.steer_left |= is_negative(steering_scaled_orig);
limit.steer_right |= is_positive(steering_scaled_orig);
}
if (fabsf(throttle_scaled) < fabsf(throttle_scaled_orig)) {
limit.throttle_lower |= is_negative(throttle_scaled_orig);
limit.throttle_upper |= is_positive(throttle_scaled_orig);
}
}
// add in throttle and steering