Added flight IMU pitch, roll, yaw attitudes to blackbox log (betaflig…

…ht#14020)

* added attitude fields desriptions

* the attitude (imu pitch, roll, yaw) added to blackbox log

* Global variable imuAttitudeQuaternion[3] is added to store current normalized imu attitude quaternion

* IMU attitude quaternion added to log instead of Euler angles

* blackbox logging of IMU attitude quaterions is replaced to GYRO group

* Revert "blackbox logging of IMU attitude quaterions is replaced to GYRO group"

This reverts commit a6020ed.

* code refactoring: use quaternion as global variable instead of float[3] array

* USE_ACC apply for attitude log

* quaterion struct data type is changed to union quaternion_t

* resolved issue of Test module

* Code style improvement: removed empty codes row

Co-authored-by: Mark Haslinghuis <[email protected]>

* Added STATIC_ASSERT check of quaternion components sequence.

Co-authored-by: Petr Ledvina <[email protected]>

* Code style improvement

Co-authored-by: Petr Ledvina <[email protected]>

* Resolved code issue

Co-authored-by: Mark Haslinghuis <[email protected]>

* resolved wrong data type name

* STATIC_ASSERT error resolved

Co-authored-by: Mark Haslinghuis <[email protected]>

* Improved assert condition

Co-authored-by: Petr Ledvina <[email protected]>

* added ATTITUDE condition for attitude blackbox log

* added blackbox_disable_attitude cli command

* Attitude position changed in blackbox disabling fields list

* resolved issue wrong quaternion log record condition

---------

Co-authored-by: Mark Haslinghuis <[email protected]>
Co-authored-by: Petr Ledvina <[email protected]>

src/main/blackbox/blackbox.c

@@ -71,6 +71,7 @@
 #include "flight/position.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/imu.h"
 
 #include "io/beeper.h"
 #include "io/gps.h"
@@ -245,9 +246,14 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"gyroUnfilt",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYROUNFILT)},
     {"gyroUnfilt",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYROUNFILT)},
     {"gyroUnfilt",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYROUNFILT)},
+#ifdef USE_ACC
     {"accSmooth",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
     {"accSmooth",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
     {"accSmooth",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
+    {"imuQuaternion",    0, SIGNED,   .Ipredict = PREDICT(0),  .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ATTITUDE)},
+    {"imuQuaternion",    1, SIGNED,   .Ipredict = PREDICT(0),  .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ATTITUDE)},
+    {"imuQuaternion",    2, SIGNED,   .Ipredict = PREDICT(0),  .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ATTITUDE)},
+#endif
     {"debug",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
     {"debug",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
     {"debug",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
@@ -354,7 +360,10 @@ typedef struct blackboxMainState_s {
     int16_t setpoint[4];
     int16_t gyroADC[XYZ_AXIS_COUNT];
     int16_t gyroUnfilt[XYZ_AXIS_COUNT];
+#ifdef USE_ACC
     int16_t accADC[XYZ_AXIS_COUNT];
+    int16_t imuAttitudeQuaternion[XYZ_AXIS_COUNT];
+#endif
     int16_t debug[DEBUG16_VALUE_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
     int16_t servo[MAX_SUPPORTED_SERVOS];
@@ -563,6 +572,9 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
     case CONDITION(ACC):
         return sensors(SENSOR_ACC) && isFieldEnabled(FIELD_SELECT(ACC));
 
+    case CONDITION(ATTITUDE):
+        return sensors(SENSOR_ACC) && isFieldEnabled(FIELD_SELECT(ATTITUDE));
+
     case CONDITION(DEBUG_LOG):
         return (debugMode != DEBUG_NONE) && isFieldEnabled(FIELD_SELECT(DEBUG_LOG));
 
@@ -726,9 +738,15 @@ static void writeIntraframe(void)
         blackboxWriteSigned16VBArray(blackboxCurrent->gyroUnfilt, XYZ_AXIS_COUNT);
     }
 
+#ifdef USE_ACC
     if (testBlackboxCondition(CONDITION(ACC))) {
         blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
     }
+
+    if (testBlackboxCondition(CONDITION(ATTITUDE))) {
+        blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT);
+    }
+#endif
 
     if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
         blackboxWriteSigned16VBArray(blackboxCurrent->debug, DEBUG16_VALUE_COUNT);
@@ -904,9 +922,17 @@ static void writeInterframe(void)
     if (testBlackboxCondition(CONDITION(GYROUNFILT))) {
         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroUnfilt),   XYZ_AXIS_COUNT);
     }
+
+#ifdef USE_ACC
     if (testBlackboxCondition(CONDITION(ACC))) {
         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
     }
+
+    if (testBlackboxCondition(CONDITION(ATTITUDE))) {
+        blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion), XYZ_AXIS_COUNT);
+    }
+#endif
+
     if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT);
     }
@@ -1208,7 +1234,6 @@ static void loadMainState(timeUs_t currentTimeUs)
     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
 
     blackboxCurrent->time = currentTimeUs;
-
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         blackboxCurrent->axisPID_P[i] = lrintf(pidData[i].P);
         blackboxCurrent->axisPID_I[i] = lrintf(pidData[i].I);
@@ -1222,6 +1247,8 @@ static void loadMainState(timeUs_t currentTimeUs)
 
 #if defined(USE_ACC)
         blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]);
+        STATIC_ASSERT(offsetof(quaternion_t, w) == 0, "Code expects quaternion in w, x, y, z order");
+        blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF);  //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w 
 #endif
 #ifdef USE_MAG
         blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);

src/main/blackbox/blackbox_fielddefs.h

@@ -74,7 +74,7 @@ typedef enum FlightLogFieldCondition {
     FLIGHT_LOG_FIELD_CONDITION_GYROUNFILT,
     FLIGHT_LOG_FIELD_CONDITION_ACC,
     FLIGHT_LOG_FIELD_CONDITION_DEBUG_LOG,
-
+    FLIGHT_LOG_FIELD_CONDITION_ATTITUDE,
     FLIGHT_LOG_FIELD_CONDITION_NEVER,
 
     FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS,
@@ -90,6 +90,7 @@ typedef enum FlightLogFieldSelect_e { // no more than 32
     FLIGHT_LOG_FIELD_SELECT_ALTITUDE,
     FLIGHT_LOG_FIELD_SELECT_RSSI,
     FLIGHT_LOG_FIELD_SELECT_GYRO,
+    FLIGHT_LOG_FIELD_SELECT_ATTITUDE,
     FLIGHT_LOG_FIELD_SELECT_ACC,
     FLIGHT_LOG_FIELD_SELECT_DEBUG_LOG,
     FLIGHT_LOG_FIELD_SELECT_MOTOR,

src/main/cli/settings.c

@@ -857,6 +857,7 @@ const clivalue_t valueTable[] = {
     { "blackbox_disable_gyrounfilt",VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_GYROUNFILT, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) },
 #if defined(USE_ACC)
     { "blackbox_disable_acc",       VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_ACC,   PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) },
+    { "blackbox_disable_attitude",  VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_ATTITUDE,   PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) },
 #endif
     { "blackbox_disable_debug",     VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_DEBUG_LOG,   PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) },
     { "blackbox_disable_motors",    VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_MOTOR,   PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) },

src/main/flight/imu.c

@@ -101,14 +101,15 @@ STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
 #endif // USE_ACC
 
 // quaternion of sensor frame relative to earth frame
-STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
+STATIC_UNIT_TESTED quaternion_t q = QUATERNION_INITIALIZE;
 STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE;
 // headfree quaternions
-quaternion headfree = QUATERNION_INITIALIZE;
-quaternion offset = QUATERNION_INITIALIZE;
+quaternion_t headfree = QUATERNION_INITIALIZE;
+quaternion_t offset = QUATERNION_INITIALIZE;
 
 // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 attitudeEulerAngles_t attitude = EULER_INITIALIZE;
+quaternion_t imuAttitudeQuaternion = QUATERNION_INITIALIZE;  
 
 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 3);
 
@@ -126,7 +127,7 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .mag_declination = 0
 );
 
-static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
+static void imuQuaternionComputeProducts(quaternion_t *quat, quaternionProducts *quatProd)
 {
     quatProd->ww = quat->w * quat->w;
     quatProd->wx = quat->w * quat->x;
@@ -275,7 +276,7 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt,
     gy *= (0.5f * dt);
     gz *= (0.5f * dt);
 
-    quaternion buffer;
+    quaternion_t buffer;
     buffer.w = q.w;
     buffer.x = q.x;
     buffer.y = q.y;
@@ -309,10 +310,12 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
        attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf));
        attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf));
        attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
+       imuAttitudeQuaternion = headfree; 
     } else {
        attitude.values.roll = lrintf(atan2_approx(rMat.m[2][1], rMat.m[2][2]) * (1800.0f / M_PIf));
        attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat.m[2][0])) * (1800.0f / M_PIf));
        attitude.values.yaw = lrintf((-atan2_approx(rMat.m[1][0], rMat.m[0][0]) * (1800.0f / M_PIf)));
+       imuAttitudeQuaternion = q; //using current q quaternion  for blackbox log
     }
 
     if (attitude.values.yaw < 0) {
@@ -752,7 +755,7 @@ float getCosTiltAngle(void)
     return rMat.m[2][2];
 }
 
-void getQuaternion(quaternion *quat)
+void getQuaternion(quaternion_t *quat)
 {
    quat->w = q.w;
    quat->x = q.x;
@@ -818,7 +821,7 @@ bool imuQuaternionHeadfreeOffsetSet(void)
     }
 }
 
-void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result)
+void imuQuaternionMultiplication(quaternion_t *q1, quaternion_t *q2, quaternion_t *result)
 {
     const float A = (q1->w + q1->x) * (q2->w + q2->x);
     const float B = (q1->z - q1->y) * (q2->y - q2->z);

src/main/flight/imu.h

@@ -30,9 +30,12 @@
 // Exported symbols
 extern bool canUseGPSHeading;
 
-typedef struct {
-    float w,x,y,z;
-} quaternion;
+typedef union {
+    float v[4];
+    struct {
+        float w, x, y, z;
+    };
+} quaternion_t;
 #define QUATERNION_INITIALIZE  {.w=1, .x=0, .y=0,.z=0}
 
 typedef struct {
@@ -53,6 +56,7 @@ typedef union {
 
 extern attitudeEulerAngles_t attitude;
 extern matrix33_t rMat;
+extern quaternion_t imuAttitudeQuaternion; //attitude quaternion to use in blackbox
 
 typedef struct imuConfig_s {
     uint16_t imu_dcm_kp;          // DCM filter proportional gain ( x 10000)
@@ -73,7 +77,7 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
 
 float getSinPitchAngle(void);
 float getCosTiltAngle(void);
-void getQuaternion(quaternion * q);
+void getQuaternion(quaternion_t * q);
 void imuUpdateAttitude(timeUs_t currentTimeUs);
 
 void imuInit(void);

src/test/unit/flight_imu_unittest.cc

@@ -67,7 +67,7 @@ extern "C" {
                              const float dcmKpGain);
     float imuCalcMagErr(void);
     float imuCalcCourseErr(float courseOverGround);
-    extern quaternion q;
+    extern quaternion_t q;
     extern matrix33_t rMat;
     extern bool attitudeIsEstablished;
 
@@ -86,7 +86,7 @@ extern "C" {
 
 const float sqrt2over2 = sqrtf(2) / 2.0f;
 
-void quaternion_from_axis_angle(quaternion* q, float angle, float x, float y, float z) {
+void quaternion_from_axis_angle(quaternion_t* q, float angle, float x, float y, float z) {
     vector3_t a = {{x, y, z}};
     vector3Normalize(&a, &a);
     q->w = cos(angle / 2);