Plane : Improved LAND_THEN_NEUTRL to work on all control surfaces

This fixes the issue #18593

Co-Authored-By: Peter Hall <33176108+IamPete1@users.noreply.github.com>

ArduPlane/Plane.h

@@ -1070,6 +1070,7 @@ private:
     void dspoiler_update(void);
     void airbrake_update(void);
     void servo_output_mixers(void);
+    void landing_neutral_control_surface_servos(void);
     void servos_output(void);
     void servos_auto_trim(void);
     void servos_twin_engine_mix();

ArduPlane/servos.cpp

@@ -897,24 +897,6 @@ void Plane::set_servos(void)
         }
     }
 
-    if (landing.get_then_servos_neutral() > 0 &&
-            control_mode == &mode_auto &&
-            landing.get_disarm_delay() > 0 &&
-            landing.is_complete() &&
-            !arming.is_armed()) {
-        // after an auto land and auto disarm, set the servos to be neutral just
-        // in case we're upside down or some crazy angle and straining the servos.
-        if (landing.get_then_servos_neutral() == 1) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, 0);
-            SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
-            SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
-        } else if (landing.get_then_servos_neutral() == 2) {
-            SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::Limit::ZERO_PWM);
-            SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::Limit::ZERO_PWM);
-            SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::Limit::ZERO_PWM);
-        }
-    }
-
     uint8_t override_pct;
     if (g2.ice_control.throttle_override(override_pct)) {
         // the ICE controller wants to override the throttle for starting
@@ -925,6 +907,37 @@ void Plane::set_servos(void)
     servos_output();
 }
 
+/*
+    This sets servos to neutral if it is a control surface servo in auto mode
+*/
+void Plane::landing_neutral_control_surface_servos(void)
+{
+    if (!(landing.get_then_servos_neutral() > 0 &&
+            control_mode == &mode_auto &&
+            landing.get_disarm_delay() > 0 &&
+            landing.is_complete() &&
+            !arming.is_armed())) {
+                return;
+    }
+
+
+    // after an auto land and auto disarm, set the servos to be neutral just
+    // in case we're upside down or some crazy angle and straining the servos.
+    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS ; i++) {
+            SRV_Channel *chan = SRV_Channels::srv_channel(i);
+            if (chan == nullptr || !SRV_Channel::is_control_surface(chan->get_function())) {
+                continue;
+            }
+
+            if (landing.get_then_servos_neutral() == 1) {
+                SRV_Channels::set_output_scaled(chan->get_function(), 0);
+            } else if (landing.get_then_servos_neutral() == 2) {
+                SRV_Channels::set_output_limit(chan->get_function(), SRV_Channel::Limit::ZERO_PWM);
+            }
+    }
+ 
+}
+
 /*
   run configured output mixer. This takes calculated servo_out values
   for each channel and calculates PWM values, then pushes them to
@@ -949,6 +962,9 @@ void Plane::servos_output(void)
     // run vtail and elevon mixers
     servo_output_mixers();
 
+    //  set control surface servos to neutral
+    landing_neutral_control_surface_servos();
+
     // support MANUAL_RCMASK
     if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
         SRV_Channels::copy_radio_in_out_mask(uint16_t(g2.manual_rc_mask.get()));

libraries/SRV_Channel/SRV_Channel.cpp

@@ -221,3 +221,33 @@ bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function)
             (function >= SRV_Channel::k_boost_throttle && function <= SRV_Channel::k_motor12) ||
             function == k_engine_run_enable);
 }
+
+// return true if function is for a control surface
+bool SRV_Channel::is_control_surface(SRV_Channel::Aux_servo_function_t function)
+{
+    switch (function)
+    {
+    case SRV_Channel::Aux_servo_function_t::k_flap:  
+    case SRV_Channel::Aux_servo_function_t::k_flap_auto:
+    case SRV_Channel::Aux_servo_function_t::k_aileron:
+    case SRV_Channel::Aux_servo_function_t::k_dspoilerLeft1:
+    case SRV_Channel::Aux_servo_function_t::k_dspoilerLeft2:
+    case SRV_Channel::Aux_servo_function_t::k_dspoilerRight1:
+    case SRV_Channel::Aux_servo_function_t::k_dspoilerRight2:
+    case SRV_Channel::Aux_servo_function_t::k_elevator:
+    case SRV_Channel::Aux_servo_function_t::k_rudder:
+    case SRV_Channel::Aux_servo_function_t::k_flaperon_left:
+    case SRV_Channel::Aux_servo_function_t::k_flaperon_right:
+    case SRV_Channel::Aux_servo_function_t::k_elevon_left:
+    case SRV_Channel::Aux_servo_function_t::k_elevon_right:
+    case SRV_Channel::Aux_servo_function_t::k_vtail_left:
+    case SRV_Channel::Aux_servo_function_t::k_vtail_right:
+    case SRV_Channel::Aux_servo_function_t::k_airbrake:
+        return true;
+
+    default:
+        return false;
+    }
+
+    return false;
+}

libraries/SRV_Channel/SRV_Channel.h

@@ -232,6 +232,9 @@ public:
     // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
     static bool should_e_stop(SRV_Channel::Aux_servo_function_t function);
 
+    // return true if function is for a control surface
+    static bool is_control_surface(SRV_Channel::Aux_servo_function_t function);
+
     // return the function of a channel
     SRV_Channel::Aux_servo_function_t get_function(void) const {
         return (SRV_Channel::Aux_servo_function_t)function.get();