From 01c84a206b68e4faa9d382756a5e63ce97f154da Mon Sep 17 00:00:00 2001 From: demvlad Date: Wed, 13 Nov 2024 10:08:21 +0300 Subject: [PATCH] IMU attitude quaternion added to log instead of Euler angles --- src/main/blackbox/blackbox.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2bdb1b8075..422d74e9e6 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -246,9 +246,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"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)}, - {"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)}, - {"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)}, - {"attitude", 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(ACC)}, + {"imuQuaternion", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)}, + {"imuQuaternion", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)}, #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)}, @@ -358,7 +358,7 @@ typedef struct blackboxMainState_s { int16_t gyroUnfilt[XYZ_AXIS_COUNT]; #ifdef USE_ACC int16_t accADC[XYZ_AXIS_COUNT]; - int16_t attitude[XYZ_AXIS_COUNT]; + int16_t imuAttitudeQuaternion[XYZ_AXIS_COUNT]; #endif int16_t debug[DEBUG16_VALUE_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; @@ -718,7 +718,7 @@ static void writeIntraframe(void) if (testBlackboxCondition(CONDITION(ACC))) { blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); - blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); + blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT); } if (testBlackboxCondition(CONDITION(DEBUG_LOG))) { @@ -891,7 +891,7 @@ static void writeInterframe(void) } if (testBlackboxCondition(CONDITION(ACC))) { blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); - blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT); + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion), XYZ_AXIS_COUNT); } if (testBlackboxCondition(CONDITION(DEBUG_LOG))) { blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT); @@ -1204,7 +1204,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale); #if defined(USE_ACC) blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]); - blackboxCurrent->attitude[i] = lrintf(attitude.raw[i]); + blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion[i] * 0x7FFF); //Scale to int16 by value 0x7FFF = 2^15 - 1 #endif #ifdef USE_MAG blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);