Copter: allow ok_to_enter to return a failure message

ArduCopter/mode.cpp

@@ -185,8 +185,16 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
         return false;
     }
 
-    if (!new_flightmode->ok_to_enter()) {
+    char *failure_reason = nullptr;
+    if (!new_flightmode->ok_to_enter(failure_reason)) {
         gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
+        if (failure_reason != nullptr) {
+            gcs().send_text(MAV_SEVERITY_WARNING,
+                            "%s: %s",
+                            new_flightmode->name(),
+                            failure_reason);
+            free(failure_reason);
+        }
         Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
         return false;
     }
@@ -626,7 +634,7 @@ uint16_t Copter::Mode::get_pilot_speed_dn()
     return copter.get_pilot_speed_dn();
 }
 
-bool Copter::Mode::ok_to_enter_gps_checks() const
+bool Copter::Mode::ok_to_enter_gps_checks(char *&failure_reason) const
 {
     if (!requires_GPS()) {
         // this mode doesn't require position information
@@ -642,22 +650,24 @@ bool Copter::Mode::ok_to_enter_gps_checks() const
     if (copter.position_ok()) {
         return true;
     }
+    UNUSED_RESULT(asprintf(&failure_reason, "Position required"));
     return false;
 }
 
-bool Copter::Mode::ok_to_enter() const
+bool Copter::Mode::ok_to_enter(char *&failure_reason) const
 {
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter a non-manual throttle mode if the
     // rotor runup is not complete
     if (motors->armed() &&
         !has_manual_throttle() &&
         !motors->rotor_runup_complete()){
+        UNUSED_RESULT(asprintf(&failure_reason, ""));
         return false;
     }
 #endif
 
-    if (!ok_to_enter_gps_checks()) {
+    if (!ok_to_enter_gps_checks(failure_reason)) {
         return false;
     }
 

ArduCopter/mode.h

@@ -13,7 +13,7 @@ class Mode {
 
 protected:
 
-    virtual bool ok_to_enter() const;
+    virtual bool ok_to_enter(char *&failure_reason) const;
     virtual void enter() = 0;
     virtual void run() = 0;
 
@@ -113,7 +113,7 @@ class Mode {
 
 private:
 
-    bool ok_to_enter_gps_checks() const;
+    bool ok_to_enter_gps_checks(char *&failure_reason) const;
 };
 
 
@@ -125,7 +125,7 @@ class ModeAcro : public Mode {
     // inherit constructor
     using Copter::Mode::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&reason) const override;
     void enter() override;
     virtual void run() override;
 
@@ -194,7 +194,7 @@ class ModeAuto : public Mode {
         mission(_mission),
         Mode() {}
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     void run() override;
 
@@ -243,7 +243,7 @@ class ModeAuto : public Mode {
 
 private:
 
-    bool ok_to_enter_check_takeoff_cmd() const;
+    bool ok_to_enter_check_takeoff_cmd(char *&failure_reason) const;
 
     bool verify_command(const AP_Mission::Mission_Command& cmd);
 
@@ -356,7 +356,7 @@ class ModeAutoTune : public Mode {
     // inherit constructor
     using Copter::Mode::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     void run() override;
 
@@ -593,7 +593,7 @@ class ModeFlip : public Mode {
     // inherit constructor
     using Copter::Mode::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     void run() override;
 
@@ -626,7 +626,7 @@ class ModeFlowHold : public Mode {
     // need a constructor for parameters
     ModeFlowHold(void);
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&reason) const override;
     void enter() override;
     void run(void) override;
 
@@ -962,7 +962,7 @@ class ModeSmartRTL : public ModeRTL {
     // inherit constructor
     using Copter::ModeRTL::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     void run() override;
 
@@ -1023,7 +1023,7 @@ class ModeStabilize : public Mode {
     // inherit constructor
     using Copter::Mode::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     virtual void run() override;
 
@@ -1065,7 +1065,7 @@ class ModeThrow : public Mode {
     // inherit constructor
     using Copter::Mode::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     void run() override;
 
@@ -1104,7 +1104,7 @@ class ModeAvoidADSB : public ModeGuided {
     // inherit constructor
     using Copter::ModeGuided::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void enter() override;
     void run() override;
 
@@ -1131,7 +1131,7 @@ class ModeFollow : public ModeGuided {
     // inherit constructor
     using Copter::ModeGuided::Mode;
 
-    bool ok_to_enter() const override;
+    bool ok_to_enter(char *&failure_reason) const override;
     void run() override;
 
     bool requires_GPS() const override { return true; }

ArduCopter/mode_acro.cpp

@@ -8,14 +8,15 @@
  * Init and run calls for acro flight mode
  */
 
-bool Copter::ModeAcro::ok_to_enter() const
+bool Copter::ModeAcro::ok_to_enter(char *&failure_reason) const
 {
    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
    if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
            (get_pilot_desired_throttle(channel_throttle->get_control_in(), copter.g2.acro_thr_mid) > get_non_takeoff_throttle())) {
+       UNUSED_RESULT(asprintf(&failure_reason, "Throttle too high"));
        return false;
    }
-   return Copter::Mode::ok_to_enter();
+   return Copter::Mode::ok_to_enter(failure_reason);
 }
 
 void Copter::ModeAcro::enter()

ArduCopter/mode_auto.cpp

@@ -19,22 +19,22 @@
  *  Code in this file implements the navigation commands
  */
 
-bool Copter::ModeAuto::ok_to_enter_check_takeoff_cmd() const
+bool Copter::ModeAuto::ok_to_enter_check_takeoff_cmd(char *&failure_reason) const
 {
     if (!ap.land_complete) {
         // we're already flying, so we don't need a takeoff_cmd
         return true;
     }
 
     if (!mission.starts_with_takeoff_cmd()) {
-            // gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
+        UNUSED_RESULT(asprintf(&failure_reason, "Missing Takeoff Cmd"));
         return false;
     }
     return true;
 }
 
 // auto_init - initialise auto controller
-bool Copter::ModeAuto::ok_to_enter() const
+bool Copter::ModeAuto::ok_to_enter(char *&failure_reason) const
 {
     // allow changing into auto mode if disarmed, regardless of
     // the mission having a takeoff.  Arming is not permitted in
@@ -44,13 +44,14 @@ bool Copter::ModeAuto::ok_to_enter() const
             // can't enter auto without at least one non-home-position
             // command.  Note our home position counts towards
             // num_commands()!
+            UNUSED_RESULT(asprintf(&failure_reason, "Too few mission items"));
             return false;
         }
-        if (!ok_to_enter_check_takeoff_cmd()) {
+        if (!ok_to_enter_check_takeoff_cmd(failure_reason)) {
             return false;
         }
     }
-    return Copter::Mode::ok_to_enter();
+    return Copter::Mode::ok_to_enter(failure_reason);
 }
 
 void Copter::ModeAuto::enter()

ArduCopter/mode_autotune.cpp

@@ -162,25 +162,30 @@ void Copter::ModeAutoTune::stop()
 }
 
 // start - Initialize autotune flight mode
-bool Copter::ModeAutoTune::ok_to_enter() const
+bool Copter::ModeAutoTune::ok_to_enter(char *&failure_reason) const
 {
-    // only allow flip from Stabilize, AltHold,  PosHold or Loiter modes
-    if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD &&
-        copter.control_mode != LOITER && copter.control_mode != POSHOLD) {
+    // only allow autotune from Stabilize, AltHold,  PosHold or Loiter modes
+    if (copter.control_mode != STABILIZE &&
+        copter.control_mode != ALT_HOLD &&
+        copter.control_mode != LOITER &&
+        copter.control_mode != POSHOLD) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Must enter from stabilize, alt_hold, loiter or poshold"));
         return false;
     }
 
     // ensure throttle is above zero
     if (ap.throttle_zero) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Throttle too low"));
         return false;
     }
 
     // ensure we are flying
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Must be flying"));
         return false;
     }
 
-    return Copter::Mode::ok_to_enter();
+    return Copter::Mode::ok_to_enter(failure_reason);
 }
 
 const char *Copter::ModeAutoTune::level_issue_string() const

ArduCopter/mode_avoid_adsb.cpp

@@ -10,10 +10,10 @@
  */
 
 // ok_to-enter - returns true if it is OK to enter this mode
-bool Copter::ModeAvoidADSB::ok_to_enter() const
+bool Copter::ModeAvoidADSB::ok_to_enter(char *&failure_reason) const
 {
     // re-use guided mode
-    return Copter::ModeGuided::ok_to_enter();
+    return Copter::ModeGuided::ok_to_enter(failure_reason);
 }
 
 // initialise avoid_adsb controller

ArduCopter/mode_flip.cpp

@@ -38,31 +38,35 @@ int8_t    flip_roll_dir;            // roll direction (-1 = roll left, 1 = roll
 int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 = pitch back)
 
 // flip_init - initialise flip controller
-bool Copter::ModeFlip::ok_to_enter() const
+bool Copter::ModeFlip::ok_to_enter(char *&failure_reason) const
 {
     // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
     if (copter.control_mode != ACRO &&
         copter.control_mode != STABILIZE &&
         copter.control_mode != ALT_HOLD &&
         copter.control_mode != FLOWHOLD) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Must enter from Acro, Stabilize, FlowHold or Alt Hold"));
         return false;
     }
 
     // if in acro or stabilize ensure throttle is above zero
     if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Throttle too low"));
         return false;
     }
 
     // ensure roll input is less than 40deg
     if (abs(channel_roll->get_control_in()) >= 4000) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Not level"));
         return false;
     }
 
     // only allow flip when flying
     if (!motors->armed() || ap.land_complete) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Not armed"));
         return false;
     }
-    return Copter::Mode::ok_to_enter();
+    return Copter::Mode::ok_to_enter(failure_reason);
 }
 
 void Copter::ModeFlip::enter()

ArduCopter/mode_flowhold.cpp

@@ -75,16 +75,18 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
 #define CONTROL_FLOWHOLD_EARTH_FRAME 0
 
 // flowhold_init - initialise flowhold controller
-bool Copter::ModeFlowHold::ok_to_enter() const
+bool Copter::ModeFlowHold::ok_to_enter(char *&failure_reason) const
 {
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter Flow Hold if the Rotor Runup is not complete
     if (!motors->rotor_runup_complete()){
+        UNUSED_RESULT(asprintf(&failure_reason, "Rotor runup not complete"));
         return false;
     }
 #endif
 
     if (!copter.optflow.enabled() || !copter.optflow.healthy()) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Optical flow not healthy"));
         return false;
     }
 

ArduCopter/mode_follow.cpp

@@ -25,13 +25,14 @@
 #endif
 
 // initialise follow mode
-bool Copter::ModeFollow::ok_to_enter() const
+bool Copter::ModeFollow::ok_to_enter(char *&failure_reason) const
 {
     // re-use guided mode
     if (!g2.follow.enabled()) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Follow not enabled"));
         return false;
     }
-    return Copter::ModeGuided::ok_to_enter();
+    return Copter::ModeGuided::ok_to_enter(failure_reason);
 }
 
 void Copter::ModeFollow::run()

ArduCopter/mode_smart_rtl.cpp

@@ -9,12 +9,13 @@
  * Once the copter is close to home, it will run a standard land controller.
  */
 
-bool Copter::ModeSmartRTL::ok_to_enter() const
+bool Copter::ModeSmartRTL::ok_to_enter(char *&failure_reason) const
 {
     if (!g2.smart_rtl.is_active()) {
+        UNUSED_RESULT(asprintf(&failure_reason, "SmartRTL inactive"));
         return false;
     }
-    return Copter::Mode::ok_to_enter();
+    return Copter::Mode::ok_to_enter(failure_reason);
 }
 
 void Copter::ModeSmartRTL::enter()

ArduCopter/mode_stabilize.cpp

@@ -5,15 +5,16 @@
  */
 
 // stabilize_init - initialise stabilize controller
-bool Copter::ModeStabilize::ok_to_enter() const
+bool Copter::ModeStabilize::ok_to_enter(char *&failure_reason) const
 {
     // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
     if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
             (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
+        UNUSED_RESULT(asprintf(&failure_reason, "Throttle too high"));
         return false;
     }
 
-    return Copter::Mode::ok_to_enter();
+    return Copter::Mode::ok_to_enter(failure_reason);
 }
 
 void Copter::ModeStabilize::enter()