Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_Motors: re-work omni output to scale evenly at saturation

This commit is contained in:
Iampete1 2021-09-09 22:27:24 +01:00 committed by Randy Mackay
parent 1d5ea351c4
commit 934a4e26c9
1 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AR_Motors/AP_MotorsUGV.cpp
View File

@ -792,20 +792,30 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float
const float scaled_steering = steering / 4500.0f;
const float scaled_lateral = lateral / 100.0f;
float thr_str_ltr_out;
float thr_str_ltr_out[AP_MOTORS_NUM_MOTORS_MAX];
float thr_str_ltr_max = 1;
for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
thr_str_ltr_out = (scaled_throttle * _throttle_factor[i]) +
for (uint8_t i=0; i<_motors_num; i++) {
// Each motor outputs throttle + steering + lateral
thr_str_ltr_out[i] = (scaled_throttle * _throttle_factor[i]) +
(scaled_steering * _steering_factor[i]) +
(scaled_lateral * _lateral_factor[i]);
if (fabsf(thr_str_ltr_out) > thr_str_ltr_max) {
thr_str_ltr_max = fabsf(thr_str_ltr_out);
// record the largest output above 1
if (fabsf(thr_str_ltr_out[i]) > thr_str_ltr_max) {
thr_str_ltr_max = fabsf(thr_str_ltr_out[i]);
}
float output_vectored = (thr_str_ltr_out / thr_str_ltr_max);
}
// Scale all outputs back evenly such that the lagest fits
const float output_scale = 1 / thr_str_ltr_max;
for (uint8_t i=0; i<_motors_num; i++) {
// send output for each motor
output_throttle(SRV_Channels::get_motor_function(i), 100.0f * output_vectored);
output_throttle(SRV_Channels::get_motor_function(i), thr_str_ltr_out[i] * 100.0f * output_scale);
}
if (output_scale < 1.0) {
// cant tell which command resulted in the scale back, so limit all
limit.steer_left = true;
limit.steer_right = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
}
} else {
// handle disarmed case