AP_Arming: Allow arming when forcing DCM and no GPS configured

* If we force AP to use DCM, and disable all GPS's, we never expect a home location unless a user uses the GCS to arm * Allow users to arm DCM with all pre-arm checks enabled in this special case * This allows using AP as a simple stabilizer (FBWA) on plane GPS), they can now arm * Existing behavior isn't changed because it's gated by an option bit Signed-off-by: Ryan Friedman <[email protected]>
ArduPilot · Jan 8, 2025 · da40b24 · da40b24
1 parent 8875479
commit da40b24
Show file tree

Hide file tree

Showing 7 changed files with 81 additions and 3 deletions.
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -1130,6 +1130,8 @@ class Plane : public AP_Vehicle {
     bool set_mode(Mode& new_mode, const ModeReason reason);
     bool set_mode(const uint8_t mode, const ModeReason reason) override;
     bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason);
+    // called when an attempt to change into a mode is unsuccessful
+    void mode_change_failed(const Mode *mode, const char *reason);
     void check_long_failsafe();
     void check_short_failsafe();
     void startup_INS(void);

diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
@@ -209,6 +209,10 @@ void Mode::update_target_altitude()
 // returns true if the vehicle can be armed in this mode
 bool Mode::pre_arm_checks(size_t buflen, char *buffer) const
 {
+    if (requires_home() && !ahrs.home_is_set()) {
+        hal.util->snprintf(buffer, buflen, "mode requires home");
+        return false;
+    }
     if (!_pre_arm_checks(buflen, buffer)) {
         if (strlen(buffer) == 0) {
             // If no message is provided add a generic one
@@ -351,3 +355,11 @@ bool Mode::use_battery_compensation() const
 
     return true;
 }
+
+void Plane::mode_change_failed(const Mode *mode, const char *reason)
+{
+    gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason);
+    LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number()));
+    // make sad noise
+    AP_Notify::events.user_mode_change_failed = 1;
+}
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -110,6 +110,9 @@ class Mode
     // true if mode can have terrain following disabled by switch
     virtual bool allows_terrain_disable() const { return false; }
 
+    // true if mode requires the AHRS to have a home before using this mode.
+    virtual bool requires_home() const { return true; };
+
     // true if automatic switch to thermal mode is supported.
     virtual bool does_automatic_thermal_switch() const {return false; }
 
@@ -205,6 +208,8 @@ friend class ModeQAcro;
 
     void stabilize_quaternion();
 
+    bool requires_home() const override { return false; };
+
 protected:
 
     // ACRO controller state
@@ -246,6 +251,8 @@ class ModeAuto : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
     bool is_landing() const override;
 
     void do_nav_delay(const AP_Mission::Mission_Command& cmd);
@@ -299,6 +306,8 @@ class ModeAutoTune : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
     void run() override;
 
 protected:
@@ -327,6 +336,10 @@ class ModeGuided : public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    // Once offboard mode exists, only global position control modes require home.
+    // Low level velocity and acceleration controls don't require home.
+    bool requires_home() const override { return true; };
+
     // handle a guided target request from GCS
     bool handle_guided_request(Location target_loc) override;
 
@@ -361,6 +374,8 @@ class ModeCircle: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    bool requires_home() const override { return false; };
+
 protected:
 
     bool _enter() override;
@@ -395,6 +410,8 @@ class ModeLoiter : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
 protected:
 
     bool _enter() override;
@@ -442,6 +459,8 @@ class ModeManual : public Mode
     // true if voltage correction should be applied to throttle
     bool use_battery_compensation() const override { return false; }
 
+    bool requires_home() const override { return false; };
+
 };
 
 
@@ -464,6 +483,8 @@ class ModeRTL : public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
 protected:
 
     bool _enter() override;
@@ -488,6 +509,8 @@ class ModeStabilize : public Mode
 
     void run() override;
 
+    bool requires_home() const override { return false; };
+
 private:
     void stabilize_stick_mixing_direct();
 
@@ -501,6 +524,8 @@ class ModeTraining : public Mode
     const char *name() const override { return "TRAINING"; }
     const char *name4() const override { return "TRAN"; }
 
+    bool requires_home() const override { return false; };
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -525,6 +550,8 @@ class ModeInitializing : public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    bool requires_home() const override { return false; };
+
 protected:
     bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
 
@@ -543,6 +570,8 @@ class ModeFBWA : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    bool requires_home() const override { return false; };
+
     void run() override;
 
 };
@@ -566,6 +595,8 @@ class ModeFBWB : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    bool requires_home() const override { return false; };
+
     void update_target_altitude() override {};
 
 protected:
@@ -585,6 +616,8 @@ class ModeCruise : public Mode
 
     bool does_automatic_thermal_switch() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -623,6 +656,8 @@ class ModeAvoidADSB : public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
 protected:
 
     bool _enter() override;
@@ -642,6 +677,7 @@ class ModeQStabilize : public Mode
     bool is_vtol_man_throttle() const override { return true; }
     virtual bool is_vtol_man_mode() const override { return true; }
     bool allows_throttle_nudging() const override { return true; }
+    bool requires_home() const override { return true; };
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
@@ -669,6 +705,7 @@ class ModeQHover : public Mode
 
     bool is_vtol_mode() const override { return true; }
     virtual bool is_vtol_man_mode() const override { return true; }
+    bool requires_home() const override { return true; };
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
@@ -697,6 +734,7 @@ friend class Plane;
 
     bool is_vtol_mode() const override { return true; }
     virtual bool is_vtol_man_mode() const override { return true; }
+    bool requires_home() const override { return true; };
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
@@ -721,6 +759,7 @@ class ModeQLand : public Mode
     const char *name4() const override { return "QLND"; }
 
     bool is_vtol_mode() const override { return true; }
+    bool requires_home() const override { return true; };
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
@@ -742,6 +781,7 @@ class ModeQRTL : public Mode
     const char *name4() const override { return "QRTL"; }
 
     bool is_vtol_mode() const override { return true; }
+    bool requires_home() const override { return true; };
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
@@ -780,6 +820,7 @@ class ModeQAcro : public Mode
     bool is_vtol_mode() const override { return true; }
     bool is_vtol_man_throttle() const override { return true; }
     virtual bool is_vtol_man_mode() const override { return true; }
+    bool requires_home() const override { return false; };
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
@@ -802,6 +843,7 @@ class ModeQAutotune : public Mode
 
     bool is_vtol_mode() const override { return true; }
     virtual bool is_vtol_man_mode() const override { return true; }
+    bool requires_home() const override { return true; };
 
     void run() override;
 
@@ -837,6 +879,8 @@ class ModeTakeoff: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
     // var_info for holding parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -886,6 +930,8 @@ class ModeThermal: public Mode
     // we need to run the speed/height controller
     bool does_auto_throttle() const override { return true; }
 
+    bool requires_home() const override { return true; };
+
 protected:
 
     bool exit_heading_aligned() const;

diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
@@ -286,6 +286,16 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
         return false;
     }
 
+    // Block mode changes to modes that require home if we don't have a home, we need a home, and we're armed.
+    // Mode changes without a home, while disarmed, are always ok.
+    if (new_mode.requires_home() &&
+        !ahrs.home_is_set() && 
+        hal.util->get_soft_armed()
+        ) {
+        mode_change_failed(&new_mode, "requires home");
+        return false;
+    }
+
     // backup current control_mode and previous_mode
     Mode &old_previous_mode = *previous_mode;
     Mode &old_mode = *control_mode;

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -189,8 +189,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
 
     // @Param: OPTIONS
     // @DisplayName: Optional AHRS behaviour
-    // @Description: This controls optional AHRS behaviour. Setting DisableDCMFallbackFW will change the AHRS behaviour for fixed wing aircraft in fly-forward flight to not fall back to DCM when the EKF stops navigating. Setting DisableDCMFallbackVTOL will change the AHRS behaviour for fixed wing aircraft in non fly-forward (VTOL) flight to not fall back to DCM when the EKF stops navigating. Setting DontDisableAirspeedUsingEKF disables the EKF based innovation check for airspeed consistency
-    // @Bitmask: 0:DisableDCMFallbackFW, 1:DisableDCMFallbackVTOL, 2:DontDisableAirspeedUsingEKF
+    // @Description: This controls optional AHRS behaviour. Setting DisableDCMFallbackFW will change the AHRS behaviour for fixed wing aircraft in fly-forward flight to not fall back to DCM when the EKF stops navigating. Setting DisableDCMFallbackVTOL will change the AHRS behaviour for fixed wing aircraft in non fly-forward (VTOL) flight to not fall back to DCM when the EKF stops navigating. Setting DontDisableAirspeedUsingEKF disables the EKF based innovation check for airspeed consistency. Setting AllowArmingWithoutHome lets you arm a fixed wing in modes that don't require GPS such as MANUAL or FBWA.
+    // @Bitmask: 0:DisableDCMFallbackFW, 1:DisableDCMFallbackVTOL, 2:DontDisableAirspeedUsingEKF, 3:AllowArmingWithoutHome
     // @User: Advanced
     AP_GROUPINFO("OPTIONS",  18, AP_AHRS, _options, 0),
 

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -683,6 +683,12 @@ class AP_AHRS {
     // get access to an EKFGSF_yaw estimator
     const EKFGSF_yaw *get_yaw_estimator(void) const;
 
+    // get whether the AHRS allows arming without a home set in modes that don't require a home.
+    // used in plane for flying in modes such as MANUAL and FBWA.
+    bool allows_arming_without_home() {
+        return option_set(Options::ALLOW_ARMING_WITHOUT_HOME);
+    }
+
 private:
 
     // roll/pitch/yaw euler angles, all in radians
@@ -1016,6 +1022,7 @@ class AP_AHRS {
         DISABLE_DCM_FALLBACK_FW=(1U<<0),
         DISABLE_DCM_FALLBACK_VTOL=(1U<<1),
         DISABLE_AIRSPEED_EKF_CHECK=(1U<<2),
+        ALLOW_ARMING_WITHOUT_HOME=(1U<<3),
     };
     AP_Int16 _options;
 

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
@@ -636,7 +636,8 @@ bool AP_Arming::gps_checks(bool report)
             }
         }
 
-        if (!AP::ahrs().home_is_set()) {
+        // Plane uses an option to allow users to fly without a home.
+        if (!AP::ahrs().allows_arming_without_home() && !AP::ahrs().home_is_set()) {
             check_failed(ARMING_CHECK_GPS, report, "AHRS: waiting for home");
             return false;
         }