AP_MotorsUGV: add output_throttle to handle scaling and relay

this function hides the difference in scaling between the regular throttle
(-100 to +100) and the throttleLeft and throttleRight (-1000 to +1000)
Also handles the setting of the relay for brushed-with-relay motors

APMrover2/AP_MotorsUGV.cpp

@@ -184,16 +184,53 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
             motor_left += dir * steering_scaled;
         }
     }
+
+    // send pwm value to each motor
+    output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left);
+    output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right);
+}
+
+// output throttle value to main throttle channel, left throttle or right throttle.  throttle should be scaled from -100 to 100
+void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle)
+{
+    // sanity check servo function
+    if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight) {
+        return;
+    }
+
+    // constrain output
+    throttle = constrain_float(throttle, -100.0f, 100.0f);
+
+    // set relay if necessary
     if (_pwm_type == PWM_TYPE_BRUSHED) {
-        const bool dirLeft = is_positive(motor_left);
-        const bool dirRight = is_positive(motor_right);
-        _relayEvents.do_set_relay(0, dirLeft);
-        _relayEvents.do_set_relay(1, dirRight);
-        motor_left = fabsf(motor_left);
-        motor_right = fabsf(motor_right);
+        switch (function) {
+            case SRV_Channel::k_throttle:
+            case SRV_Channel::k_throttleLeft:
+                _relayEvents.do_set_relay(0, is_negative(throttle));
+                break;
+            case SRV_Channel::k_throttleRight:
+                _relayEvents.do_set_relay(1, is_negative(throttle));
+                break;
+            default:
+                // do nothing
+                break;
+        }
+        throttle = fabsf(throttle);
+    }
+
+    // output to servo channel
+    switch (function) {
+        case SRV_Channel::k_throttle:
+            SRV_Channels::set_output_scaled(function,  throttle);
+            break;
+        case SRV_Channel::k_throttleLeft:
+        case SRV_Channel::k_throttleRight:
+            SRV_Channels::set_output_scaled(function,  throttle*10.0f);
+            break;
+        default:
+            // do nothing
+            break;
     }
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft,  1000.0f * motor_left);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor_right);
 }
 
 // slew limit throttle for one iteration

APMrover2/AP_MotorsUGV.h

@@ -57,6 +57,9 @@ protected:
     // output to skid steering channels
     void output_skid_steering(bool armed, float steering, float throttle);
 
+    // output throttle (-100 ~ +100) to a throttle channel.  Sets relays if required
+    void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle);
+
     // slew limit throttle for one iteration
     void slew_limit_throttle(float dt);