Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 17:55:30 +03:00
Code Issues Wiki Activity

IMU attitude quaternion added to log instead of Euler angles

Browse source
This commit is contained in:
demvlad 2024-11-13 10:08:21 +03:00
parent d776e83788
commit 01c84a206b
1 changed files with 7 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/blackbox/blackbox.c
View file

@ -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", 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", 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)}, {"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)}, {"imuQuaternion", 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)}, {"imuQuaternion", 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", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
#endif #endif
{"debug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)}, {"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", 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]; int16_t gyroUnfilt[XYZ_AXIS_COUNT];
#ifdef USE_ACC #ifdef USE_ACC
int16_t accADC[XYZ_AXIS_COUNT]; int16_t accADC[XYZ_AXIS_COUNT];
int16_t attitude[XYZ_AXIS_COUNT]; int16_t imuAttitudeQuaternion[XYZ_AXIS_COUNT];
#endif #endif
int16_t debug[DEBUG16_VALUE_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
@ -718,7 +718,7 @@ static void writeIntraframe(void)
if (testBlackboxCondition(CONDITION(ACC))) { if (testBlackboxCondition(CONDITION(ACC))) {
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT);
} }
if (testBlackboxCondition(CONDITION(DEBUG_LOG))) { if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
@ -891,7 +891,7 @@ static void writeInterframe(void)
} }
if (testBlackboxCondition(CONDITION(ACC))) { if (testBlackboxCondition(CONDITION(ACC))) {
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); 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))) { if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT); 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); blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale);
#if defined(USE_ACC) #if defined(USE_ACC)
blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]); 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 #endif
#ifdef USE_MAG #ifdef USE_MAG
blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]); blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);