Improve EKF core IMU failover logic - WIP #11139

jschall · 2019-04-18T01:04:56Z

This patch

Only improves the IMU failover logic within an EKF core - it does not change the logic for EKF core switching
Moves the core IMU selection logic from AP_NavEKF_core to AP_NavEKF
Resets the IMU bias states and covariances when an EKF core switches IMUs
Uses the following logic to select a new accelerometer and gyro for an EKF core when its accelerometer and gyro become unhealthy:

    Try to find a paired gyro and accel that are both healthy w/ INS_USE=1 and configured to be used in EK2_IMU_MASK
    If that failed, try to find a paired gyro and accel that are both healthy w/ INS_USE=1
    If that failed, fall back on ins.get_primary_gyro and ins.get_primary_accel

TODO:

Implement for EKF3
Trigger a primary core change when the primary core's IMU becomes unhealthy - no change in functionality here
Implement switching back to the initial IMU if the initial IMU becomes healthy for some period of time
Pre-arm check for consistency between EK2_IMU_MASK and INS_USE*
SITL test for failed IMU

…functions

tridge · 2019-04-18T01:07:37Z

small issue. Resetting the biases means if we get a single-frame bad health on an IMU we lose all biases when we recover.

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

jschall · 2019-04-18T01:09:37Z

small issue. Resetting the biases means if we get a single-frame bad health on an IMU we lose all biases when we recover.

Actually, at the moment, it wouldn't "recover". The core would stay with the IMU we switched to, provided it remains healthy.

The EKF core could potentially store the current estimated gyro bias for each IMU and switch them rather than resetting.

tridge · 2019-04-18T01:11:13Z

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

-    if (ekf_type() != 0) {
-        return get_primary_IMU_index();
+    int8_t imu = -1;
+    switch (ekf_type()) {


should this be active_EKF_type() ? ie. if we are failed onto DCM in plane, then I think we shouldn't be asking the failed EKF which IMU to use

Maybe, I didn't modify the function I copied.

tridge · 2019-04-18T01:12:36Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1464,6 +1464,10 @@ void AP_InertialSensor::wait_for_sample(void)
 */
 bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
 {
+    if (i >= INS_MAX_INSTANCES) {


should be get_gyro_count() not INS_MAX_INSTANCES I think

That sounds wrong to me.

This is a minimum change as it moves the i < INS_MAX_INSTANCES check from the EKF into AP_InertialSensor.

Ah, never mind, it was checking get_accel_count. Are you sure that is correct? Why would get_accel_count and get_gyro_count be different? In the case where they are different, are the gyros and accels guaranteed to be assigned contiguous indices?

tridge · 2019-04-18T01:13:04Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1481,6 +1485,10 @@ bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
 */
 bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
 {
+    if (i >= INS_MAX_INSTANCES) {


should be get_accel_count()

tridge · 2019-04-18T01:13:38Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1497,7 +1505,7 @@ bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity)
 float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
 {
    float ret;
-    if (_delta_velocity_valid[i]) {
+    if (i < INS_MAX_INSTANCES && _delta_velocity_valid[i]) {


get_accel_count()

tridge · 2019-04-18T01:13:57Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1512,7 +1520,7 @@ float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
 float AP_InertialSensor::get_delta_angle_dt(uint8_t i) const
 {
    float ret;
-    if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
+    if (i < INS_MAX_INSTANCES && _delta_angle_valid[i] && _delta_angle_dt[i] > 0) {


get_gyro_count()

tridge · 2019-04-18T01:23:09Z

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

    const AP_InertialSensor &ins = AP::ins();

-    if (ins_index < ins.get_gyro_count()) {


keep bool return here

tridge · 2019-04-18T01:23:46Z

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -42,10 +42,11 @@ NavEKF2_core::NavEKF2_core(void) :
 }

 // setup this core backend
-bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index)
+bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _gyro_index, uint8_t _accel_index, uint8_t _core_index)


i don't think changing setup_core() API is needed? the selections get overwritten anyway

tridge · 2019-04-18T01:24:43Z

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

+{
+    if (gyro_index != _new_gyro_index) {
+        // NOTE: theoretically, we should wait until we reach the end of the IMU buffer prior to resetting these
+        stateStruct.gyro_bias.zero();


I'm concerned about a very short IMU outage (1 ms timeout for example) causing biases to reset

tridge · 2019-04-18T01:25:04Z

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

+        P[13][13] = P[12][12];
+        P[14][14] = P[12][12];
+
+        gcs().send_text(MAV_SEVERITY_ERROR, "EKF2 %u (g%u a%u) switched to gyro %u", (unsigned)core_index, (unsigned)gyro_index, (unsigned)accel_index, _new_gyro_index);


we need to rate limit this msg I think

tridge · 2019-04-18T01:25:31Z

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

+        // Z delta velocity bias
+        P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
+
+        gcs().send_text(MAV_SEVERITY_ERROR, "EKF2 %u (g%u a%u) switched to accel %u", (unsigned)core_index, (unsigned)gyro_index, (unsigned)accel_index, _new_accel_index);


I don't think we should zero accel biases, and we should rate limit the msg

…if they have been healthy in the past

…lthy

jschall · 2019-04-18T19:52:08Z

I have pushed changes to switch EKF cores back to their original IMU if the original IMU becomes healthy. This required a change to AP_InertialSensor to remove latching of IMU health to false. Biases are now preserved (bias covariance is not).

proficnc · 2019-04-19T04:14:07Z

do you need Feal to test fly this? If so, can you please send him a firmware?

…

On Fri, Apr 19, 2019 at 3:52 AM jschall ***@***.***> wrote: I have pushed changes to switch EKF cores back to their original IMU if the original IMU becomes healthy. This required a change to AP_InertialSensor to remove latching of IMU health to false. Biases are now preserved (bias covariance is not). — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#11139 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/ABDJ7Q4VKSSEYIBEIMVCISDPRDGPHANCNFSM4HGY5WGA> .

priseborough · 2019-04-22T03:40:19Z

This has ended up modifying more lines of code inside the estimator than I originally envisioned would be necessary, so will need a bit longer to review the changes properly. It does however raise the question as to how this is going to be tested, otherwise we could be swapping an unknown bug for a known one.

priseborough · 2019-04-22T03:54:21Z

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -485,4 +493,9 @@ class NavEKF2 {
    // 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
    void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary);
+
+    uint8_t core_initial_imu[8];


Can we please make sure when adding new class variables that there is s description in the comments as per the rest of the code. As a minimum it should indicate what that block of variable is used for.

tridge · 2019-05-06T23:52:13Z

I think the work in here could still be merged with some work between Paul and Jon, marked as WIP

tridge · 2019-08-05T01:41:20Z

this has been obsoleted by other changes to master to address the same issue

jschall added 3 commits April 17, 2019 15:32

AP_InertialSensor: protect against out-of-range indices on get_delta …

d57a3a7

…functions

AP_NavEKF2: use only data from assigned IMU

ce51800

AP_NavEKF2: add function to switch IMU indices

0ebbca2

tridge reviewed Apr 18, 2019

View reviewed changes

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp Outdated Show resolved Hide resolved

jschall added 2 commits April 17, 2019 18:13

AP_NavEKF2: improve core IMU failover logic

5cd1a9b

AP_AHRS: use new AP_NavEKF2 functions for accel and gyro indices

9b7169f

jschall force-pushed the pr-imu-selection branch from 0f2bb24 to 9b7169f Compare April 18, 2019 01:14

tridge requested changes Apr 18, 2019

View reviewed changes

tridge and others added 7 commits April 18, 2019 12:42

SITL: added IMU failure masks

e6d643c

AP_InertialSensor: allow testing of IMU failure

7a818d1

AP_InertialSensor: added RC switch for killing IMUs

ab2fc4c

RC_Channel: added RC switch for killing IMUs

3b8bed7

AP_InertialSensor: remove logic that marks healthy IMUs as unhealthy …

9047507

…if they have been healthy in the past

AP_NavEKF2: preserve IMU biases when switching cores between IMUs

a938896

AP_NavEKF2: switch cores back to their original IMU if it becomes hea…

404e08c

…lthy

priseborough reviewed Apr 22, 2019

View reviewed changes

tridge mentioned this pull request Apr 22, 2019

Fix IMU failover (simpler alternative) #11165

Closed

tridge added the WIP label May 6, 2019

rmackay9 changed the title ~~Improve EKF core IMU failover logic~~ Improve EKF core IMU failover logic - WIP May 6, 2019

tridge closed this Aug 5, 2019

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Improve EKF core IMU failover logic - WIP #11139

Improve EKF core IMU failover logic - WIP #11139

jschall commented Apr 18, 2019 •

edited

tridge commented Apr 18, 2019

jschall commented Apr 18, 2019 •

edited

tridge Apr 18, 2019

jschall Apr 18, 2019

tridge Apr 18, 2019

jschall Apr 18, 2019

jschall Apr 18, 2019

jschall Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

tridge Apr 18, 2019

jschall commented Apr 18, 2019

proficnc commented Apr 19, 2019 via email

priseborough commented Apr 22, 2019

priseborough Apr 22, 2019

tridge commented May 6, 2019

tridge commented Aug 5, 2019

		const AP_InertialSensor &ins = AP::ins();

		if (ins_index < ins.get_gyro_count()) {

Improve EKF core IMU failover logic - WIP #11139

Improve EKF core IMU failover logic - WIP #11139

Conversation

jschall commented Apr 18, 2019 • edited

tridge commented Apr 18, 2019

jschall commented Apr 18, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jschall commented Apr 18, 2019

proficnc commented Apr 19, 2019 via email

priseborough commented Apr 22, 2019

Choose a reason for hiding this comment

tridge commented May 6, 2019

tridge commented Aug 5, 2019

jschall commented Apr 18, 2019 •

edited

jschall commented Apr 18, 2019 •

edited