Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: replace prearm_healthy with pre_arm_check

This commit is contained in:
Randy Mackay 2020-08-11 14:03:41 +09:00 committed by Peter Barker
parent 1d97416a51
commit b2d4622320
3 changed files with 13 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -906,13 +906,21 @@ bool NavEKF2::healthy(void) const
return core[primary].healthy();
}
bool NavEKF2::all_cores_healthy(void) const
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool NavEKF2::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
if (!core) {
hal.util->snprintf(failure_msg, failure_msg_len, "no EKF2 cores");
return false;
}
for (uint8_t i = 0; i < num_cores; i++) {
if (!core[i].healthy()) {
const char *failure = core[primary].prearm_failure_reason();
if (failure != nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, failure);
} else {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 core %d unhealthy", (int)i);
}
return false;
}
}
@ -1519,21 +1527,6 @@ uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) const
return core[primary].getLastVelNorthEastReset(vel);
}
// report the reason for why the backend is refusing to initialise
const char *NavEKF2::prearm_failure_reason(void) const
{
if (!core) {
return initFailureReason[int(initFailure)];
}
for (uint8_t i = 0; i < num_cores; i++) {
const char * failure = core[i].prearm_failure_reason();
if (failure != nullptr) {
return failure;
}
}
return nullptr;
}
// Returns the amount of vertical position change due to the last reset or core switch in metres
// Returns the time of the last reset or 0 if no reset or core switch has ever occurred
// Where there are multiple consumers, they must access this function on the same frame as each other

8
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -61,8 +61,9 @@ public:
// Check basic filter health metrics and return a consolidated health status
bool healthy(void) const;
// Ensure that all started cores are considered healthy
bool all_cores_healthy(void) const;
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// returns the index of the primary core
// return -1 if no primary core selected
@ -313,9 +314,6 @@ public:
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosDownReset(float &posDelta);
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const;
// set and save the _baroAltNoise parameter
void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };

2
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -13,7 +13,7 @@ extern const AP_HAL::HAL& hal;
Monitor magnetometer innovations to see if the heading is good enough to use GPS
Return true if all criteria pass for 10 seconds
We also record the failure reason so that prearm_failure_reason()
We also record the failure reason so that pre_arm_check()
can give a good report to the user on why arming is failing
This sets gpsGoodToAlign class variable