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

Copter: ensure we have a position estimate before arming in Loiter, PosHold, etc #12928

Merged
merged 2 commits into from Dec 3, 2019

Conversation

@rmackay9
Copy link
Contributor

rmackay9 commented Nov 29, 2019

This PR resolves issue #11435 which was a serious safety hazard because users could attempt to fly in a mode that requires a position estimate without actually having one by either setting ARMING_CHECK = 0 or using the new "arm throttle force" feature.

To highlight the danger, in SITL I was able set GPS_TYPE = 0 and then arm and fly in Loiter mode. The result was the vehicle pitched over at 45degrees and would barely respond to user input.

This has been tested in SITL by doing the following:

  • param set GPS_TYPE 0
  • LOITER
  • arm throttle force

The vehicle refused to arm and instead displayed the message "PreArm: EKF2 still initialising". This is not a great message but compared to Copter-3.6 which simply displays, "PreArm:" (with no message) this is an improvement.

The same behaviour was observed with this similar test:

  • param set ARMING_CHECK 0
  • LOITER
  • arm throttle

Also to ensure the vehicle could still be armed in non-GPS modes the following test was done:

  • STABILIZE
  • arm throttle

As expected the vehicle could be armed.

@rmackay9 rmackay9 requested a review from peterbarker Nov 29, 2019
These are checks that can never be bypassed
@rmackay9 rmackay9 force-pushed the rmackay9:copter-always-do-arming-checks branch from 0cc83d6 to 28d8b2e Nov 29, 2019
@rmackay9 rmackay9 force-pushed the rmackay9:copter-always-do-arming-checks branch from 28d8b2e to fc682a1 Nov 29, 2019
@rmackay9 rmackay9 mentioned this pull request Nov 29, 2019
21 of 38 tasks complete
@@ -441,11 +393,13 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
// warn about hdop separately - to prevent user confusion with no gps lock
if (copter.gps.get_hdop() > copter.g.gps_hdop_good) {
check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP");
AP_Notify::flags.pre_arm_gps_check = false;

This comment has been minimized.

Copy link
@peterbarker

peterbarker Nov 29, 2019

Contributor

I wonder if this could go into check_failed; the first field should be enough to switch on or whatever.

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Dec 2, 2019

Author Contributor

@peterbarker, the problem is that it needs to be set to true some times as well and I'd rather not spread that logic out over too many functions.

@@ -643,6 +671,15 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
return AP_Arming::arm_checks(method);
}

// mandatory checks that will be run if ARMING_CHECK is zero or arming forced
bool AP_Arming_Copter::mandatory_checks(bool display_failure)

This comment has been minimized.

Copy link
@peterbarker

peterbarker Nov 29, 2019

Contributor

This confuses me because we already have some mandatory checks in bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) (https://github.com/ardupilot/ardupilot/blob/master/ArduCopter/AP_Arming.cpp#L38)

If force-arming bypassed those then I think we're agreed there's a bug as forced-arming should be the same as setting the arming-check bitmask to zero. If we have such a bug then the checks I've referenced aren't being run, and that's not good.

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Dec 2, 2019

Author Contributor

@peterbarker, yes and I struggled with all this going back and forth on where to put the "mandatory_gps_checks" and the interlock checks. The problem is that beyond providing the failure message to the user we also need to set the "AP_Notify::flags.pre_arm_gps_check" flag. If ARMING_CHECK = 0 or using the "force" armed then the flag needs to be set based on the shorter set of checks, otherwise the flag needs to be set based on the full set. There are a couple of ways to accomplish this but I thought this method was as good as any.

}
}

// check EKF compass variance is below failsafe threshold

This comment has been minimized.

Copy link
@peterbarker

peterbarker Nov 29, 2019

Contributor

Whoops. Not a GPS check!

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Dec 2, 2019

Author Contributor

I think we use "gps check" as an analogy for "position estimate check". I don't think it adds value to split them into two separate checks at this point. Maybe in the future when we enhance the non-GPS navigation features.

}

// check home and EKF origin are not too far
if (copter.far_from_EKF_origin(ahrs.get_home())) {

This comment has been minimized.

Copy link
@peterbarker

peterbarker Nov 29, 2019

Contributor

Also no a GPS check! (the method has "gps" in the name :-)

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Dec 2, 2019

Author Contributor

as mentioned above, "gps check" is analogous to "position estimate check" at the moment. It's an existing bit of fuzziness I think that's not worth trying to correct at the moment.

@CraigElder CraigElder removed the DevCallTopic label Dec 3, 2019
@rmackay9 rmackay9 merged commit 2c3beb0 into ArduPilot:master Dec 3, 2019
4 checks passed
4 checks passed
ArduPilot.ardupilot #20191129.19 succeeded
Details
ArduPilot.ardupilot (Cygwin SITL build) Cygwin SITL build succeeded
Details
continuous-integration/travis-ci/pr The Travis CI build passed
Details
semaphoreci The build passed on Semaphore.
Details
@rmackay9 rmackay9 deleted the rmackay9:copter-always-do-arming-checks branch Dec 3, 2019
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
3 participants
You can’t perform that action at this time.