Plane:Takeoff on rudder arm only after return to neutral

ArduPlane/Parameters.cpp

@@ -1092,6 +1092,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Bitmask: 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL
     // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode
     // @Bitmask: 12: Enable FBWB style loiter altitude control
+    // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces
     // @User: Advanced
     AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
 

ArduPlane/Plane.h

@@ -402,11 +402,13 @@ private:
     struct {
         uint32_t last_tkoff_arm_time;
         uint32_t last_check_ms;
+        uint32_t rudder_takeoff_warn_ms;
         uint32_t last_report_ms;
         bool launchTimerStarted;
         uint8_t accel_event_counter;
         uint32_t accel_event_ms;
         uint32_t start_time_ms;
+        bool waiting_for_rudder_neutral;
     } takeoff_state;
 
     // ground steering controller state
@@ -1103,6 +1105,7 @@ private:
     void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,
                                 SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const;
     void flaperon_update();
+    void indicate_waiting_for_rud_neutral_to_takeoff(void);
 
     // is_flying.cpp
     void update_is_flying_5Hz(void);

ArduPlane/defines.h

@@ -8,6 +8,8 @@
 
 #define MIN_AIRSPEED_MIN 5 // m/s, used for arming check and speed scaling
 
+#define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats
+
 // failsafe
 // ----------------------
 enum failsafe_state {
@@ -161,7 +163,7 @@ enum FlightOptions {
     CENTER_THROTTLE_TRIM = (1<<10),
     DISABLE_GROUND_PID_SUPPRESSION = (1<<11),
     ENABLE_LOITER_ALT_CONTROL = (1<<12),
-
+    INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13),
 };
 
 enum CrowFlapOptions {

ArduPlane/mode.cpp

@@ -33,6 +33,9 @@ bool Mode::enter()
     // cancel inverted flight
     plane.auto_state.inverted_flight = false;
     
+    // cancel waiting for rudder neutral
+    plane.takeoff_state.waiting_for_rudder_neutral = false;
+
     // don't cross-track when starting a mission
     plane.auto_state.next_wp_crosstrack = false;
 

ArduPlane/quadplane.cpp

@@ -2955,9 +2955,9 @@ void QuadPlane::takeoff_controller(void)
         // start motor spinning if not spinning already so user sees it is armed
         set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         takeoff_start_time_ms = now;
-        if (now - rudder_takeoff_warn_ms > 3000) {
-            gcs().send_text(MAV_SEVERITY_WARNING, "takeoff wait rudder release");
-            rudder_takeoff_warn_ms = now;
+        if (now - plane.takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release");
+            plane.takeoff_state.rudder_takeoff_warn_ms = now;
         }
         return;
     }

ArduPlane/quadplane.h

@@ -580,7 +580,6 @@ private:
     AP_Float maximum_takeoff_airspeed;
     uint32_t takeoff_start_time_ms;
     uint32_t takeoff_time_limit_ms;
-    uint32_t rudder_takeoff_warn_ms;
 
     float last_land_final_agl;
 

ArduPlane/radio.cpp

@@ -139,6 +139,7 @@ void Plane::rudder_arm_disarm_check()
 				arming.arm(AP_Arming::Method::RUDDER);
                 rudder_arm_timer = 0;
                 seen_neutral_rudder = false;
+                takeoff_state.rudder_takeoff_warn_ms = now;
             }
 		} else {
 			// not at full right rudder

ArduPlane/servos.cpp

@@ -968,6 +968,23 @@ void Plane::landing_neutral_control_surface_servos(void)
  
 }
 
+/*
+  sets rudder/vtail , and elevon to indicator positions that we are in a rudder arming waiting for neutral stick state
+*/
+void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void)
+{
+    if (takeoff_state.waiting_for_rudder_neutral)  {
+        SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
+        channel_function_mixer(SRV_Channel::k_rudder,  SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
+        if (!SRV_Channels::function_assigned(SRV_Channel::k_rudder) && !SRV_Channels::function_assigned(SRV_Channel::k_vtail_left)) {
+            // if no rudder indication possible, neutral elevons during wait becuase on takeoff stance they are usually both full up
+            SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, 0);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, 0);        
+        }
+    }
+}
+
+
 /*
   run configured output mixer. This takes calculated servo_out values
   for each channel and calculates PWM values, then pushes them to
@@ -999,6 +1016,11 @@ void Plane::servos_output(void)
     //  set control surface servos to neutral
     landing_neutral_control_surface_servos();
     
+    // set rudder arm waiting for neutral control throws (rudder neutral, aileron/rt vtail/rt elevon to full right)
+    if (g2.flight_options & FlightOptions::INDICATE_WAITING_FOR_RUDDER_NEUTRAL) {
+        indicate_waiting_for_rud_neutral_to_takeoff();
+    }
+
     // support MANUAL_RCMASK
     if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
         SRV_Channels::copy_radio_in_out_mask(uint32_t(g2.manual_rc_mask.get()));

ArduPlane/takeoff.cpp

@@ -30,6 +30,24 @@ bool Plane::auto_takeoff_check(void)
 
     takeoff_state.last_check_ms = now;
     
+    //check if waiting for rudder neutral after rudder arm
+    if (plane.arming.last_arm_method() == AP_Arming::Method::RUDDER &&
+        !seen_neutral_rudder) {
+        // we were armed with rudder but have not seen rudder neutral yet
+        takeoff_state.waiting_for_rudder_neutral = true;
+        // warn if we have been waiting a long time
+        if (now - takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release");
+            takeoff_state.rudder_takeoff_warn_ms = now;
+        }
+        // since we are still waiting, dont takeoff
+        return false;
+    } else {
+       // we did not arm by rudder or rudder has returned to neutral
+       // make sure we dont indicate we are in the waiting state with servo position indicator
+       takeoff_state.waiting_for_rudder_neutral = false;
+    }  
+
     // Check for bad GPS
     if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
         // no auto takeoff without GPS lock