Rover: add setup_servo_output to allow ouput reconfiguration

APMrover2/AP_MotorsUGV.cpp

@@ -65,15 +65,8 @@ AP_MotorsUGV::AP_MotorsUGV(AP_ServoRelayEvents &relayEvents) :
 
 void AP_MotorsUGV::init()
 {
-    // k_steering are limited to -45;45 degree
-    SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
-
-    // k_throttle are in power percent so -100 ... 100
-    SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
-
-    // skid steering left/right throttle as -1000 to 1000 values
-    SRV_Channels::set_angle(SRV_Channel::k_throttleLeft,  1000);
-    SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
+    // setup servo ouput
+    setup_servo_output();
 
     // setup pwm type
     setup_pwm_type();
@@ -215,6 +208,20 @@ void AP_MotorsUGV::slew_limit_throttle(float dt)
     }
 }
 
+// setup servo output
+void AP_MotorsUGV::setup_servo_output()
+{
+    // k_steering are limited to -45;45 degree
+    SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
+
+    // k_throttle are in power percent so -100 ... 100
+    SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
+
+    // skid steering left/right throttle as -1000 to 1000 values
+    SRV_Channels::set_angle(SRV_Channel::k_throttleLeft,  1000);
+    SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
+}
+
 // setup pwm output type
 void AP_MotorsUGV::setup_pwm_type()
 {

APMrover2/AP_MotorsUGV.h

@@ -24,6 +24,8 @@ public:
     // setup output in case of main CPU failure
     void setup_safety_output();
 
+    void setup_servo_output();
+
     // set steering as a value from -4500 to +4500
     float get_steering() const { return _steering; }
     void set_steering(float steering) { _steering = steering; }

APMrover2/radio.cpp

@@ -14,8 +14,10 @@ void Rover::set_control_channels(void)
     channel_steer->set_angle(SERVO_MAX);
     channel_throttle->set_angle(100);
 
-    // For a rover safety is TRIM throttle
+    // Allow to reconfigure ouput when not armed
     if (!arming.is_armed()) {
+        g2.motors.setup_servo_output();
+        // For a rover safety is TRIM throttle
         g2.motors.setup_safety_output();
     }
     // setup correct scaling for ESCs like the UAVCAN PX4ESC which