Plane: refactor more of servos output code to be easier to read

ArduPlane/Plane.h

@@ -1039,6 +1039,10 @@ private:
     void stabilize();
     void set_servos_idle(void);
     void set_servos();
+    void set_servos_manual_passthrough(void);
+    void set_servos_controlled(void);
+    void set_servos_old_elevons(void);
+    void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
     bool allow_reverse_thrust(void);
     void update_aux();
     void update_is_flying_5Hz(void);

ArduPlane/servos.cpp

@@ -318,53 +318,11 @@ uint16_t Plane::throttle_min(void) const
 };
 
 
-/*****************************************
-* Set the flight control servos based on the current calculated values
-*****************************************/
-void Plane::set_servos(void)
-{
-    // this is to allow the failsafe module to deliberately crash 
-    // the plane. Only used in extreme circumstances to meet the
-    // OBC rules
-    if (afs.should_crash_vehicle()) {
-        afs.terminate_vehicle();
-        return;
-    }
-
-    int16_t last_throttle = channel_throttle->get_radio_out();
-
-    // do any transition updates for quadplane
-    quadplane.update();    
-
-    if (control_mode == AUTO && auto_state.idle_mode) {
-        // special handling for balloon launch
-        set_servos_idle();
-        return;
-    }
-
-    /*
-      see if we are doing ground steering.
+/*
+  pass through channels in manual mode
  */
-    if (!steering_control.ground_steering) {
-        // we are not at an altitude for ground steering. Set the nose
-        // wheel to the rudder just in case the barometer has drifted
-        // a lot
-        steering_control.steering = steering_control.rudder;
-    } else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) {
-        // we are within the ground steering altitude but don't have a
-        // dedicated steering channel. Set the rudder to the ground
-        // steering output
-        steering_control.rudder = steering_control.steering;
-    }
-    channel_rudder->set_servo_out(steering_control.rudder);
-
-    // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
-    steering_control.ground_steering = false;
-
-    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder);
-    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering);
-
-    if (control_mode == MANUAL) {
+void Plane::set_servos_manual_passthrough(void)
+{
     // do a direct pass through of radio values
     if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
         channel_roll->set_radio_out(channel_roll->get_radio_in());
@@ -390,17 +348,13 @@ void Plane::set_servos(void)
     // of the 2nd aileron
     RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input);
     RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input);
+}
 
-    } else {
-        if (g.mix_mode == 0) {
-            // both types of secondary aileron are slaved to the roll servo out
-            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
-            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
-
-            // both types of secondary elevator are slaved to the pitch servo out
-            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
-            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
-        }else{
+/*
+  old (deprecated) elevon support
+ */
+void Plane::set_servos_old_elevons(void)
+{
     /*Elevon mode*/
     float ch1;
     float ch2;
@@ -430,41 +384,14 @@ void Plane::set_servos(void)
     // directly set the radio_out values for elevon mode
     channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)));
     channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)));
-        }
-
-        // push out the PWM values
-        if (g.mix_mode == 0) {
-            channel_roll->calc_pwm();
-            channel_pitch->calc_pwm();
-        }
-        channel_rudder->calc_pwm();
-
-#if THROTTLE_OUT == 0
-        channel_throttle->set_servo_out(0);
-#else
-        // convert 0 to 100% (or -100 to +100) into PWM
-        int8_t min_throttle = aparm.throttle_min.get();
-        int8_t max_throttle = aparm.throttle_max.get();
-
-        if (min_throttle < 0 && !allow_reverse_thrust()) {
-           // reverse thrust is available but inhibited.
-           min_throttle = 0;
-        }
-
-        if (control_mode == AUTO) {
-            if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
-                min_throttle = 0;
-            }
+}
 
-            if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
-                if(aparm.takeoff_throttle_max != 0) {
-                    max_throttle = aparm.takeoff_throttle_max;
-                } else {
-                    max_throttle = aparm.throttle_max;
-                }
-            }
-        }
 
+/*
+  calculate any throttle limits based on the watt limiter
+ */
+void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
+{
     uint32_t now = millis();
     if (battery.overpower_detected()) {
         // overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
@@ -506,6 +433,61 @@ void Plane::set_servos(void)
     if (min_throttle < 0) {
         min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);
     }
+}
+    
+
+
+/*
+  setup output channels all non-manual modes
+ */
+void Plane::set_servos_controlled(void)
+{
+    if (g.mix_mode != 0) {
+        set_servos_old_elevons();
+    } else {
+        // both types of secondary aileron are slaved to the roll servo out
+        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
+        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
+        
+        // both types of secondary elevator are slaved to the pitch servo out
+        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
+        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
+
+        // push out the PWM values
+        channel_roll->calc_pwm();
+        channel_pitch->calc_pwm();
+    }
+
+    channel_rudder->calc_pwm();
+    
+#if THROTTLE_OUT == 0
+    channel_throttle->set_servo_out(0);
+#else
+    // convert 0 to 100% (or -100 to +100) into PWM
+    int8_t min_throttle = aparm.throttle_min.get();
+    int8_t max_throttle = aparm.throttle_max.get();
+    
+    if (min_throttle < 0 && !allow_reverse_thrust()) {
+        // reverse thrust is available but inhibited.
+        min_throttle = 0;
+    }
+    
+    if (control_mode == AUTO) {
+        if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
+            min_throttle = 0;
+        }
+        
+        if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
+            if(aparm.takeoff_throttle_max != 0) {
+                max_throttle = aparm.takeoff_throttle_max;
+            } else {
+                max_throttle = aparm.throttle_max;
+            }
+        }
+    }
+
+    // apply watt limiter
+    throttle_watt_limiter(min_throttle, max_throttle);
 
     channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), 
                                                     min_throttle,
@@ -546,6 +528,58 @@ void Plane::set_servos(void)
         channel_throttle->calc_pwm();
     }
 #endif
+}
+
+/*****************************************
+* Set the flight control servos based on the current calculated values
+*****************************************/
+void Plane::set_servos(void)
+{
+    // this is to allow the failsafe module to deliberately crash 
+    // the plane. Only used in extreme circumstances to meet the
+    // OBC rules
+    if (afs.should_crash_vehicle()) {
+        afs.terminate_vehicle();
+        return;
+    }
+
+    int16_t last_throttle = channel_throttle->get_radio_out();
+
+    // do any transition updates for quadplane
+    quadplane.update();    
+
+    if (control_mode == AUTO && auto_state.idle_mode) {
+        // special handling for balloon launch
+        set_servos_idle();
+        return;
+    }
+
+    /*
+      see if we are doing ground steering.
+     */
+    if (!steering_control.ground_steering) {
+        // we are not at an altitude for ground steering. Set the nose
+        // wheel to the rudder just in case the barometer has drifted
+        // a lot
+        steering_control.steering = steering_control.rudder;
+    } else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) {
+        // we are within the ground steering altitude but don't have a
+        // dedicated steering channel. Set the rudder to the ground
+        // steering output
+        steering_control.rudder = steering_control.steering;
+    }
+    channel_rudder->set_servo_out(steering_control.rudder);
+
+    // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
+    steering_control.ground_steering = false;
+
+    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder);
+    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering);
+
+    if (control_mode == MANUAL) {
+        set_servos_manual_passthrough();
+    } else {
+        set_servos_controlled();
     }
 
     // Auto flap deployment