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
Merged
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -51,9 +51,9 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
check_failed(display_failure, "Motor Interlock Enabled");
}

// succeed if pre arm checks are disabled
// if pre arm checks are disabled run only the mandatory checks
if (checks_to_perform == 0) {
return true;
return mandatory_checks(display_failure);
}

return fence_checks(display_failure)
@@ -362,17 +362,9 @@ bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
// performs pre_arm gps related checks and returns true if passed
bool AP_Arming_Copter::gps_checks(bool display_failure)
{
AP_Notify::flags.pre_arm_gps_check = false;

const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();

// always check if inertial nav has started and is ready
if (!ahrs.prearm_healthy()) {
const char *reason = ahrs.prearm_failure_reason();
if (reason == nullptr) {
reason = "AHRS not healthy";
}
check_failed(display_failure, "%s", reason);
// run mandatory gps checks first
if (!mandatory_gps_checks(display_failure)) {
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}

@@ -392,46 +384,6 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
return true;
}

// ensure GPS is ok
if (!copter.position_ok()) {
const char *reason = ahrs.prearm_failure_reason();
if (reason == nullptr) {
if (!mode_requires_gps && fence_requires_gps) {
// clarify to user why they need GPS in non-GPS flight mode
reason = "Fence enabled, need 3D Fix";
} else {
reason = "Need 3D Fix";
}
}
check_failed(display_failure, "%s", reason);
return false;
}

// check for GPS glitch (as reported by EKF)
nav_filter_status filt_status;
if (ahrs.get_filter_status(filt_status)) {
if (filt_status.flags.gps_glitching) {
check_failed(display_failure, "GPS glitching");
return false;
}
}

// check EKF compass variance is below failsafe threshold
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF compass variance");
return false;
}

// check home and EKF origin are not too far
if (copter.far_from_EKF_origin(ahrs.get_home())) {
check_failed(display_failure, "EKF-home variance");
return false;
}

// return true immediately if gps check is disabled
if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) {
AP_Notify::flags.pre_arm_gps_check = true;
@@ -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.

return false;
}

// call parent gps checks
if (!AP_Arming::gps_checks(display_failure)) {
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}

@@ -523,6 +477,80 @@ bool AP_Arming_Copter::proximity_checks(bool display_failure) const
return true;
}

// performs mandatory gps checks. returns true if passed
bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
{
// always check if inertial nav has started and is ready
const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
if (!ahrs.prearm_healthy()) {
const char *reason = ahrs.prearm_failure_reason();
if (reason == nullptr) {
reason = "AHRS not healthy";
}
check_failed(display_failure, "%s", reason);
return false;
}

// check if flight mode requires GPS
bool mode_requires_gps = copter.flightmode->requires_GPS();

// check if fence requires GPS
bool fence_requires_gps = false;
#if AC_FENCE == ENABLED
// if circular or polygon fence is enabled we need GPS
fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
#endif

// return true if GPS is not required
if (!mode_requires_gps && !fence_requires_gps) {
return true;
}

// ensure GPS is ok
if (!copter.position_ok()) {
const char *reason = ahrs.prearm_failure_reason();
if (reason == nullptr) {
if (!mode_requires_gps && fence_requires_gps) {
// clarify to user why they need GPS in non-GPS flight mode
reason = "Fence enabled, need 3D Fix";
} else {
reason = "Need 3D Fix";
}
}
check_failed(display_failure, "%s", reason);
return false;
}

// check for GPS glitch (as reported by EKF)
nav_filter_status filt_status;
if (ahrs.get_filter_status(filt_status)) {
if (filt_status.flags.gps_glitching) {
check_failed(display_failure, "GPS glitching");
return false;
}
}

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

float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF compass variance");
return false;
}

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

check_failed(display_failure, "EKF-home variance");
return false;
}

// if we got here all must be ok
return true;
}


// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
@@ -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.

{
// call mandatory gps checks and update notify status because regular gps checks will not run
const bool result = mandatory_gps_checks(display_failure);
AP_Notify::flags.pre_arm_gps_check = result;
return result;
}

void AP_Arming_Copter::set_pre_arm_check(bool b)
{
copter.ap.pre_arm_check = b;
@@ -34,6 +34,9 @@ class AP_Arming_Copter : public AP_Arming
bool proximity_checks(bool display_failure) const override;
bool arm_checks(AP_Arming::Method method) override;

// mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced
bool mandatory_checks(bool display_failure) override;

// NOTE! the following check functions *DO* call into AP_Arming:
bool ins_checks(bool display_failure) override;
bool compass_checks(bool display_failure) override;
@@ -46,6 +49,7 @@ class AP_Arming_Copter : public AP_Arming
bool motor_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure);
bool oa_checks(bool display_failure);
bool mandatory_gps_checks(bool display_failure);

void set_pre_arm_check(bool b);

@@ -864,7 +864,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks)
return false;
}

if (!do_arming_checks || (pre_arm_checks(true) && arm_checks(method))) {
if ((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true) && arm_checks(method))) {
armed = true;

//TODO: Log motor arming
@@ -133,6 +133,9 @@ class AP_Arming {
bool servo_checks(bool report) const;
bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4]) const;

// mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced
virtual bool mandatory_checks(bool report) { return true; }

// returns true if a particular check is enabled
bool check_enabled(const enum AP_Arming::ArmingChecks check) const;
// returns a mavlink severity which should be used if a specific check fails
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.