mirror of https://github.com/ArduPilot/ardupilot
AR_Motors: re-work omni output to scale evenly at saturation
This commit is contained in:
parent
1d5ea351c4
commit
934a4e26c9
|
@ -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_steering = steering / 4500.0f;
|
||||||
const float scaled_lateral = lateral / 100.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;
|
float thr_str_ltr_max = 1;
|
||||||
for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
|
for (uint8_t i=0; i<_motors_num; i++) {
|
||||||
thr_str_ltr_out = (scaled_throttle * _throttle_factor[i]) +
|
// Each motor outputs throttle + steering + lateral
|
||||||
|
thr_str_ltr_out[i] = (scaled_throttle * _throttle_factor[i]) +
|
||||||
(scaled_steering * _steering_factor[i]) +
|
(scaled_steering * _steering_factor[i]) +
|
||||||
(scaled_lateral * _lateral_factor[i]);
|
(scaled_lateral * _lateral_factor[i]);
|
||||||
if (fabsf(thr_str_ltr_out) > thr_str_ltr_max) {
|
// record the largest output above 1
|
||||||
thr_str_ltr_max = fabsf(thr_str_ltr_out);
|
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
|
// 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 {
|
} else {
|
||||||
// handle disarmed case
|
// handle disarmed case
|
||||||
|
|
Loading…
Reference in New Issue