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.

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

Conversation

Ryanf55
Copy link
Collaborator

@Ryanf55 Ryanf55 commented Jan 5, 2025

Purpose

If I do not have a GPS and don't use "Set Home Here” in mission planner because I'm just flying FPV without a GCS, I want to be able to keep all arming checks enabled. This is for running AP as a simple stabilizer (FBWA) with minimal peripherals.

I never expect a home location, and do NOT expect AUTO, RTL, and other GPS-reliant modes to be allowed.
Seems like having a home location is merely a convenience for the GCS, and I shouldn't rely on global position or local position if I only fly in FBWA and ACRO, so why do we make it required?

Approach

This PR introduces mode transition rejection into Plane based on needing a home. I have re-used the mode rejection from Copter, modeled after ArduCopter/Mode::requires_GPS(). Mode changes in plane can now be rejected, but only while armed.
While disarmed, like Copter, you can change to any mode, but arming checks in Plane now check whether your mode requires a home. If home is not set, you can't arm.

Note - without changes to the EKF, I recommend using DCM to fly without any position aiding. It won't fly as well.

Wiki

This change will allow this wiki to be complete: ArduPilot/ardupilot_wiki#6508

Details

  • If we force AP to use DCM, and disable all GPS's, we never expect a home location. Currently, AP will prevent arming.
  • 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

Testing performed

This sets up a no-gps plane that is locked to DCM, and then demonstrates we can now arm.

./Tools/autotest/sim_vehicle.py -v Plane --console --map -D -w
param set STALL_PREVENTION 0
param set GPS1_TYPE 0
param set AHRS_EKF_TYPE 0
# Enable the new no-home-required for arming
param bitmask set AHRS_OPTIONS 3
map set showsimpos 1
reboot
# Move the map to follow the sim position.
mode fbwa
# Wait for "pre-arm good"
arm throttle
rc 2 2000
rc 3 1500

image

If you cause an RC failsafe, then you see it will stay in circle, instead of have a flyaway in RTL. This is because the short failsafe is circle, and the long failsafe is RTL. I assert this is an improvement to not fly to 0, 0.

rc 3 900

image

To discuss

  • If we do NOT have a GPS, but we DO HAVE visual odom enabled, but we don't have a home, should we be allowed to arm? I suspect no currently -> Correct
  • If we are using EKF3, and DO NOT have any GPS's configured, and no other position sources, should we require a home position to arm? I suspect yes. -> Unless the user enables the no home required option.
  • Do we want to allow plane users to arm without a home, and add protection in mode change to block change from FBWA to AUTO if there's no home? -> Yes, added.

Follow up

Improve mode transitions to cope with some modes not having a home, such as RTL long action should go to GLIDE rather than stay in circle.

@IamPete1
Copy link
Member

IamPete1 commented Jan 6, 2025

I think I would argue that having home is so fundamental to the way AP works that trying to fly without it is going to cause lots of unusual issues. All of planes altitude handling is relative to home for example.

If we did want to make the decision to allow flying without home we would need to update all the modes than need it to refuse entry. So you would not even be allowed to enter RTL for example.

@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Jan 6, 2025

I think I would argue that having home is so fundamental to the way AP works that trying to fly without it is going to cause lots of unusual issues. All of planes altitude handling is relative to home for example.

If we did want to make the decision to allow flying without home we would need to update all the modes than need it to refuse entry. So you would not even be allowed to enter RTL for example.

Yea, if we could refuse entry, it would prevent the flyaway risk if we allow our users to fly without home/gps. Similar to Copter's current requires_GPS() function, we could add a requires_home() function for plane. I would be happy to make that change if the other devs are ok allow flying in some modes without home.

Copy link
Contributor

@tridge tridge left a comment

Choose a reason for hiding this comment

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

you'd also need some changes in Plane::calc_speed_scaler() and some failsafe checking
I also think you will find situations where the attitude goes badly off, really does need an airspeed estimate based on drag+throttle

libraries/AP_Arming/AP_Arming.cpp Outdated Show resolved Hide resolved
@Ryanf55 Ryanf55 marked this pull request as draft January 7, 2025 05:38
@Ryanf55 Ryanf55 force-pushed the arming-allow-forced-dcm-no-gps-to-arm branch from 3a6ecb3 to a327d34 Compare January 7, 2025 07:25
@Ryanf55 Ryanf55 marked this pull request as ready for review January 7, 2025 07:28
@@ -286,6 +286,12 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
return false;
}

if (new_mode.requires_home() &&
!ahrs.home_is_set()) {
Copy link
Collaborator Author

@Ryanf55 Ryanf55 Jan 7, 2025

Choose a reason for hiding this comment

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

Do I need to use quadplane.ahrs in the condition, or is this sufficient? AHRS is a singleton, so I am not really sure why it would matter which one you used.

@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Jan 7, 2025

I've rescoped just to cover no-home (instead of no-gps no-airspeed) to make it a smaller scope change. inverse TECS and improving no-airspeed no-gps airspeed scaling can come later. This now includes mode change rejection. Additionally, you can now arm without home set in modes that don't require home if you set AHRS_OPTIONS bit 3. The method to do mode rejection was copied from Copter

Because master requires home to be set in all modes in all modes, this is a NFC for any current user, unless they try changing mode while disarmed BEFORE they have a home into a mode that requires home. Maybe we could disable the home checks while disarmed to allow the transition, and then check the mode at arm time if we don't want impact changing modes while waiting for home?

@@ -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

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.

@Hwurzburg Hwurzburg added WIP WikiNeeded needs wiki update labels Jan 7, 2025
@Ryanf55 Ryanf55 force-pushed the arming-allow-forced-dcm-no-gps-to-arm branch from a327d34 to da40b24 Compare January 8, 2025 03:13
ArduPlane/mode.h Outdated Show resolved Hide resolved
ArduPlane/mode.h Outdated
@@ -837,6 +879,8 @@ class ModeTakeoff: public Mode

bool does_auto_throttle() const override { return true; }

bool requires_home() const override { return true; };
Copy link
Contributor

Choose a reason for hiding this comment

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

Why does TAKEOFF mode need home? This is an example question applicable to other modes you've marked as "true".

Copy link
Collaborator Author

@Ryanf55 Ryanf55 Jan 8, 2025

Choose a reason for hiding this comment

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

TAKEOFF surprised me. When I tested it, the plane did not take off without home. It just sits armed on the runway drawing 0A.

This is because takeoff mode relies on having home here in the update() call:

if (!(plane.current_loc.initialised() && AP::ahrs().home_is_set())) {

It would be nice if Takeoff could just climb to the altitude using baro alt above arm height, but that doesn't work yet. Happy to change that in a future PR.

I need to test each of the Q* modes; they are all biased to requiring home right now till I confirm they are safe without it.

Copy link
Contributor

Choose a reason for hiding this comment

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

OK, bad example; TAKEOFF is home-relative, so that one makes sense :-)

@@ -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
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.

* 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]>
@Ryanf55 Ryanf55 force-pushed the arming-allow-forced-dcm-no-gps-to-arm branch from da40b24 to 3e64ee5 Compare January 8, 2025 06:57
@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Jan 8, 2025

calc_speed_scaler

I attempted an AP_Airspeed backend for synthetic airspeed. For the throttle effect on airspeed, I wanted to use the plane parameters throttle_cruise, airspeed_min and airspeed_max, but I can't get the Plane parameters in the airspeed sensor class. I don't see any reasonable code structure to make it an airspeed backend

Branch: https://github.com/Ryanf55/ardupilot/tree/airspeed-synthetic

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants