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

libraries/AR_Motors/AP_MotorsUGV.cpp

@@ -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