Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Arming: Allow arming when forcing DCM and no GPS configured #29012

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+61 −3
Open

AP_Arming: Allow arming when forcing DCM and no GPS configured #29012

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
12 changes: 12 additions & 0 deletions ArduPlane/mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
26 changes: 26 additions & 0 deletions ArduPlane/mode.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; }

Expand Down Expand Up @@ -205,6 +208,8 @@ friend class ModeQAcro;

void stabilize_quaternion();

bool requires_home() const override { return false; };

protected:

// ACRO controller state
Expand Down Expand Up @@ -327,6 +332,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;

Expand Down Expand Up @@ -361,6 +370,8 @@ class ModeCircle: public Mode

bool does_auto_throttle() const override { return true; }

bool requires_home() const override { return false; };

protected:

bool _enter() override;
Expand Down Expand Up @@ -442,6 +453,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; };

};


Expand Down Expand Up @@ -488,6 +501,8 @@ class ModeStabilize : public Mode

void run() override;

bool requires_home() const override { return false; };

private:
void stabilize_stick_mixing_direct();

Expand All @@ -501,6 +516,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;

Expand All @@ -525,6 +542,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; }

Expand All @@ -543,6 +562,8 @@ class ModeFBWA : public Mode

bool mode_allows_autotuning() const override { return true; }

bool requires_home() const override { return false; };

void run() override;

};
Expand All @@ -566,6 +587,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:
Expand Down Expand Up @@ -780,6 +803,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;
Expand Down Expand Up @@ -837,6 +861,8 @@ class ModeTakeoff: public Mode

bool does_auto_throttle() const override { return true; }

bool requires_home() const override { return false; };

// var_info for holding parameter information
static const struct AP_Param::GroupInfo var_info[];

Expand Down
10 changes: 10 additions & 0 deletions ArduPlane/system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_AHRS/AP_AHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),

Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_AHRS/AP_AHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_Arming/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Copy link
Collaborator Author

@Ryanf55 Ryanf55 Jan 7, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Perhaps we should move this check to the vehicle. On plane, I would like should check whether our current mode requires home. If it does, and we don't have a home yet, arming should be blocked.

In chat with Randy:

  • In copter, you can switch modes while disarmed. AUTO missions should start execution, but not make progress till you arm.
  • AHRS home is the RTL point, position estimate is from EKF origin. It should be safe to treat having a home and having an ekf origin as the same.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

image

As implemented, you can switch to LOITER while disarmed, but won't be allowed to arm till you set home.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

* AHRS home is the RTL point, position estimate is from EKF origin. It should be safe to treat having a home and having an ekf origin as the same.

... or, you know, you could check to see if you have a navigation origin instead....

If your failsafes include "RTL", then you might go further and require home to be set then.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are you saying we might want to add two methods to the mode class to prevent mode entry:

  1. The one I currently have of requires_home, which uses AP_AHRS::home_is_set()
  2. Another of requires_navigation_origin, which uses AP_AHRS::get_origin()

I agree that some of the modes only need the navigation origin, and I'm happy to add that to that check to mode entry if we are ok with the flash.

check_failed(ARMING_CHECK_GPS, report, "AHRS: waiting for home");
return false;
}
Expand Down
Loading