aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Add arming check for ahrs health

Browse Source
This commit is contained in:
Jacob Walser 2017-04-14 16:37:36 -04:00
parent b7de1eb88a
commit 1ff656df2d
2 changed files with 28 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduSub/AP_Arming_Sub.cpp
View File

@ -49,5 +49,31 @@ bool AP_Arming_Sub::pre_arm_checks(bool report)
return true;
}
return AP_Arming::pre_arm_checks(report) & rc_check(report);
return AP_Arming::pre_arm_checks(report) & rc_check(report) & ins_checks(report);
}
bool AP_Arming_Sub::ins_checks(bool report)
{
// call parent class checks
if (!AP_Arming::ins_checks(report)) {
return false;
}
// additional plane specific checks
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_INS)) {
if (!ahrs.healthy()) {
if (report) {
const char *reason = ahrs.prearm_failure_reason();
if (reason) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason);
} else {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: AHRS not healthy");
}
}
return false;
}
}
return true;
}

1
ArduSub/AP_Arming_Sub.h
View File

@ -14,5 +14,6 @@ public:
bool pre_arm_checks(bool report) override;
protected:
bool ins_checks(bool report) override;
enum HomeState home_status() const override;
};