Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: arming and mode init checks use ekf_position_ok

Browse Source
This commit is contained in:
Randy Mackay 2018-12-18 19:28:20 +09:00
parent 3ff1bb7714
commit 80e9a54aed
4 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APMrover2/AP_Arming.cpp
View File

@ -55,6 +55,16 @@ bool AP_Arming_Rover::gps_checks(bool display_failure)
return false; return false;
} }
// ensure position esetimate is ok
if (!rover.ekf_position_ok()) {
const char *reason = AP::ahrs().prearm_failure_reason();
if (reason == nullptr) {
reason = "Need Position Estimate";
}
check_failed(ARMING_CHECK_NONE, display_failure, "%s", reason);
return false;
}
// call parent gps checks // call parent gps checks
return AP_Arming::gps_checks(display_failure); return AP_Arming::gps_checks(display_failure);
} }

2
APMrover2/GCS_Mavlink.cpp
View File

@ -62,7 +62,7 @@ uint32_t GCS_MAVLINK_Rover::custom_mode() const
MAV_STATE GCS_MAVLINK_Rover::system_status() const MAV_STATE GCS_MAVLINK_Rover::system_status() const
{ {
if (rover.failsafe.triggered != 0) { if ((rover.failsafe.triggered != 0) || rover.failsafe.ekf) {
return MAV_STATE_CRITICAL; return MAV_STATE_CRITICAL;
} }
if (rover.control_mode == &rover.mode_initializing) { if (rover.control_mode == &rover.mode_initializing) {

1
APMrover2/defines.h
View File

@ -121,6 +121,7 @@ enum mode_reason_t {
MODE_REASON_CRASH_FAILSAFE, MODE_REASON_CRASH_FAILSAFE,
MODE_REASON_MISSION_COMMAND, MODE_REASON_MISSION_COMMAND,
MODE_REASON_FENCE_BREACH, MODE_REASON_FENCE_BREACH,
MODE_REASON_EKF_FAILSAFE,
}; };
enum pilot_steer_type_t { enum pilot_steer_type_t {

5
APMrover2/mode.cpp
View File

@ -27,10 +27,7 @@ bool Mode::enter()
rover.ahrs.get_filter_status(filt_status); rover.ahrs.get_filter_status(filt_status);
// check position estimate. requires origin and at least one horizontal position flag to be true // check position estimate. requires origin and at least one horizontal position flag to be true
Location origin; const bool position_ok = rover.ekf_position_ok() && !rover.failsafe.ekf;
const bool position_ok = ahrs.get_origin(origin) &&
(filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs ||
filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel);
if (requires_position() && !position_ok) { if (requires_position() && !position_ok) {
return false; return false;
} }