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

AP_NavEKF3: replace prearm_healthy with pre_arm_check

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

25
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1030,13 +1030,21 @@ bool NavEKF3::healthy(void) const
return core[primary].healthy(); return core[primary].healthy();
} }
bool NavEKF3::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 NavEKF3::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{ {
if (!core) { if (!core) {
hal.util->snprintf(failure_msg, failure_msg_len, "no EKF3 cores");
return false; return false;
} }
for (uint8_t i = 0; i < num_cores; i++) { for (uint8_t i = 0; i < num_cores; i++) {
if (!core[i].healthy()) { 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, "EKF3 core %d unhealthy", (int)i);
}
return false; return false;
} }
} }
@ -1796,21 +1804,6 @@ uint32_t NavEKF3::getLastVelNorthEastReset(Vector2f &vel) const
return core[primary].getLastVelNorthEastReset(vel); return core[primary].getLastVelNorthEastReset(vel);
} }
// report the reason for why the backend is refusing to initialise
const char *NavEKF3::prearm_failure_reason(void) const
{
if (!core) {
return "no EKF3 cores";
}
for (uint8_t i = 0; i < num_cores; i++) {
const char * failure = core[primary].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 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 // 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 // Where there are multiple consumers, they must access this function on the same frame as each other

7
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -59,8 +59,8 @@ public:
// Check basic filter health metrics and return a consolidated health status // Check basic filter health metrics and return a consolidated health status
bool healthy(void) const; bool healthy(void) const;
// Check that all cores are started and healthy // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool all_cores_healthy(void) const; bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// Update instance error scores for all available cores // Update instance error scores for all available cores
float updateCoreErrorScores(void); float updateCoreErrorScores(void);
@ -398,9 +398,6 @@ public:
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosDownReset(float &posDelta); 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 // set and save the _baroAltNoise parameter
void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); }; void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };

2
libraries/AP_NavEKF3/AP_NavEKF3_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 Monitor magnetometer innovations to see if the heading is good enough to use GPS
Return true if all criteria pass for 10 seconds 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 can give a good report to the user on why arming is failing
*/ */
void NavEKF3_core::calcGpsGoodToAlign(void) void NavEKF3_core::calcGpsGoodToAlign(void)