Merge pull request #1070 from iNavFlight/imu-singularity-protection

IMU sanity protection

src/main/blackbox/blackbox.c

@@ -1457,6 +1457,9 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
             blackboxWriteUnsignedVB(data->loggingResume.logIteration);
             blackboxWriteUnsignedVB(data->loggingResume.currentTimeUs);
         break;
+        case FLIGHT_LOG_EVENT_IMU_FAILURE:
+            blackboxWrite(0);
+        break;
         case FLIGHT_LOG_EVENT_LOG_END:
             blackboxPrint("End of log");
             blackboxWrite(0);

src/main/blackbox/blackbox_fielddefs.h

@@ -109,6 +109,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
     FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
     FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
+    FLIGHT_LOG_EVENT_IMU_FAILURE = 40,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
 

src/main/flight/imu.c

@@ -21,15 +21,17 @@
 #include <stdint.h>
 #include <math.h>
 
-#include "common/maths.h"
+#include "build/build_config.h"
 
 #include "platform.h"
 
-#include "build/build_config.h"
+#include "blackbox/blackbox.h"
-#include "build/debug.h"
 
 #include "common/axis.h"
 #include "common/filter.h"
+#include "common/maths.h"
+
+#include "config/feature.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -246,6 +248,43 @@ static float imuGetPGainScaleFactor(void)
     }
 }
 
+static void imuResetOrientationQuaternion(const float ax, const float ay, const float az)
+{
+    const float accNorm = sqrtf(ax * ax + ay * ay + az * az);
+
+    q0 = az + accNorm;
+    q1 = ay;
+    q2 = -ax;
+    q3 = 0.0f;
+
+    const float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+static void imuCheckAndResetOrientationQuaternion(const float ax, const float ay, const float az)
+{
+    // Check if some calculation in IMU update yield NAN or zero quaternion
+    // Reset quaternion from accelerometer - this might be incorrect, but it's better than no attitude at all
+
+    const bool isNan = (isnan(q0) || isnan(q1) || isnan(q2) || isnan(q3));
+    const bool isInf = (isinf(q0) || isinf(q1) || isinf(q2) || isinf(q3));
+    const bool isZero = (ABS(q0) < 1e-3f && ABS(q1) < 1e-3f && ABS(q2) < 1e-3f && ABS(q3) < 1e-3f);
+
+    if (isNan || isZero || isInf) {
+        imuResetOrientationQuaternion(ax, ay, az);
+
+#ifdef BLACKBOX
+        if (feature(FEATURE_BLACKBOX)) {
+            blackboxLogEvent(FLIGHT_LOG_EVENT_IMU_FAILURE, NULL);
+        }
+#endif
+    }
+}
+
 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 int accWeight, float ax, float ay, float az,
                                 bool useMag, float mx, float my, float mz,
@@ -391,6 +430,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     q2 *= recipNorm;
     q3 *= recipNorm;
 
+    // Check for invalid quaternion
+    imuCheckAndResetOrientationQuaternion(ax, ay, az);
+
     // Pre-compute rotation matrix from quaternion
     imuComputeRotationMatrix();
 }