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

AP_NavEKF2: provide reasons for init failures

Browse Source
This commit is contained in:
Mark Whitehorn 2019-10-29 11:39:19 -06:00 committed by Andrew Tridgell
parent 561920aad6
commit 95ea6ad807
2 changed files with 41 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -621,7 +621,11 @@ void NavEKF2::check_log_write(void)
// Initialise the filter
bool NavEKF2::InitialiseFilter(void)
{
init_failure = INIT_FAILURES::UNKNOWN;
if (_enable == 0) {
if (_ahrs->get_ekf_type() == 2) {
init_failure = INIT_FAILURES::NO_ENABLE;
}
return false;
}
const AP_InertialSensor &ins = AP::ins();
@ -642,20 +646,26 @@ bool NavEKF2::InitialiseFilter(void)
if (core == nullptr) {
// don't run multiple filters for 1 IMU
// this prevents dereferencing a null _ahrs pointer in NavEKF2_core::getEulerAngles
if (ins.get_accel_count() == 0) {
init_failure = INIT_FAILURES::NO_IMUS;
return false;
}
if (_imuMask == 0) {
init_failure = INIT_FAILURES::NO_MASK;
return false;
}
// don't allow more filters than IMUs
uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set(_imuMask.get() & mask);
// count IMUs from mask
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
num_cores++;
}
}
num_cores = __builtin_popcount(_imuMask);
// check if there is enough memory to create the EKF cores
if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
init_failure = INIT_FAILURES::NO_MEM;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
_enable.set(0);
return false;
@ -665,6 +675,7 @@ bool NavEKF2::InitialiseFilter(void)
core = (NavEKF2_core*)hal.util->malloc_type(sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
if (core == nullptr) {
_enable.set(0);
init_failure = INIT_FAILURES::NO_ALLOC;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
return false;
}
@ -1455,7 +1466,7 @@ uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) const
const char *NavEKF2::prearm_failure_reason(void) const
{
if (!core) {
return "no EKF2 cores";
return init_failure_reason[int(init_failure)];
}
for (uint8_t i = 0; i < num_cores; i++) {
const char * failure = core[i].prearm_failure_reason();

22
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -493,6 +493,28 @@ private:
// time of last lane switch
uint32_t lastLaneSwitch_ms;
enum class INIT_FAILURES {
OK,
NO_MEM,
NO_ENABLE,
NO_IMUS,
NO_MASK,
NO_ALLOC,
UNKNOWN,
NUM_INIT_FAILURES
};
// initialization failure reasons
const char* init_failure_reason[int(INIT_FAILURES::NUM_INIT_FAILURES)] {
"OK",
"insufficient memory available",
"EK2_enable is false",
"no IMUs available",
"EK2_IMU_MASK is zero",
"memory allocation failed",
"unknown initialization failure"
};
INIT_FAILURES init_failure;
// update the yaw reset data to capture changes due to a lane switch
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary