Copter: Standby functions

ArduCopter/Copter.cpp

@@ -144,6 +144,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(ekf_check,             10,     75),
     SCHED_TASK(gpsglitch_check,       10,     50),
     SCHED_TASK(landinggear_update,    10,     75),
+    SCHED_TASK(standby_update,        100,    75),
     SCHED_TASK(lost_vehicle_check,    10,     50),
     SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180),
     SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550),

ArduCopter/Copter.h

@@ -582,6 +582,8 @@ private:
         float takeoff_alt_cm;
     } gndeffect_state;
 
+    bool standby_active;
+
     // set when we are upgrading parameters from 3.4
     bool upgrading_frame_params;
 
@@ -744,6 +746,9 @@ private:
     // landing_gear.cpp
     void landinggear_update();
 
+    // standby.cpp
+    void standby_update();
+
     // Log.cpp
     void Log_Write_Control_Tuning();
     void Log_Write_Performance();

ArduCopter/RC_Channel.cpp

@@ -77,6 +77,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_
     case AUX_FUNC::PRECISION_LOITER:
     case AUX_FUNC::INVERTED:
     case AUX_FUNC::WINCH_ENABLE:
+    case AUX_FUNC::STAND_BY:
     case AUX_FUNC::SURFACE_TRACKING:
         do_aux_function(ch_option, ch_flag);
         break;
@@ -542,6 +543,22 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
 #endif
             break;
 
+        case AUX_FUNC::STAND_BY: {
+            switch (ch_flag) {
+                case HIGH:
+                    copter.standby_active = true;
+                    copter.Log_Write_Event(DATA_STAND_BY_ENABLE);
+                    gcs().send_text(MAV_SEVERITY_INFO, "Stand By Enabled");
+                    break;
+                default:
+                    copter.standby_active = false;
+                    copter.Log_Write_Event(DATA_STAND_BY_DISABLE);
+                    gcs().send_text(MAV_SEVERITY_INFO, "Stand By Disabled");
+                    break;
+                }
+            break;
+        }
+
         case AUX_FUNC::SURFACE_TRACKING:
             switch (ch_flag) {
             case LOW:

ArduCopter/crash_check.cpp

@@ -23,6 +23,12 @@ void Copter::crash_check()
         return;
     }
 
+    // exit immediately if in standby
+    if (standby_active) {
+        crash_counter = 0;
+        return;
+    }
+
     // return immediately if we are not in an angle stabilize flight mode or we are flipping
     if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) {
         crash_counter = 0;
@@ -73,6 +79,11 @@ void Copter::thrust_loss_check()
         return;
     }
 
+    // exit immediately if in standby
+    if (standby_active) {
+        return;
+    }
+
     // check for desired angle over 15 degrees
     // todo: add thrust angle to AC_AttitudeControl
     const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
@@ -142,6 +153,11 @@ void Copter::parachute_check()
         return;
     }
 
+    // exit immediately if in standby
+    if (standby_active) {
+        return;
+    }
+
     // call update to give parachute a chance to move servo or relay back to off position
     parachute.update();
 

ArduCopter/land_detector.cpp

@@ -54,6 +54,9 @@ void Copter::update_land_detector()
 #endif
             set_land_complete(false);
         }
+    } else if (standby_active) {
+        // land detector will not run in standby mode
+        land_detector_count = 0;
     } else {
 
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/standby.cpp

@@ -0,0 +1,22 @@
+#include "Copter.h"
+
+// Run standby functions at approximately 100 Hz to limit maximum variable build up
+//
+// When standby is active:
+//      all I terms are continually reset
+//      heading error is reset to zero
+//      position errors are reset to zero
+//      crash_check is disabled
+//      thrust_loss_check is disabled
+//      parachute_check is disabled
+//      and landing detection is disabled.
+void Copter::standby_update()
+{
+    if (!standby_active) {
+        return;
+    }
+
+    attitude_control->reset_rate_controller_I_terms();
+    attitude_control->set_yaw_target_to_current_heading();
+    pos_control->standby_xyz_reset();
+}