diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 29c63e6f4e..2231573c5d 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -230,6 +230,14 @@ After downloading the log, be sure to erase the chip to make it ready for reuse If you try to start recording a new flight when the dataflash is already full, Blackbox logging will be disabled and nothing will be recorded. +### Usage - Logging switch +If you're recording to an onboard flash chip, you probably want to disable Blackbox recording when not required in order +to save storage space. To do this, you can add a Blackbox flight mode to one of your AUX channels on the Configurator's +modes tab. Once you've added a mode, Blackbox will only log flight data when the mode is active. + +A log header will always be recorded at arming time, even if logging is paused. You can freely pause and resume logging +while in flight. + ## Converting logs to CSV or PNG After your flights, you'll have a series of flight log files with a .TXT extension. You'll need to decode these with the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis, or render them into a series of PNG diff --git a/docs/Modes.md b/docs/Modes.md index eeb0dd0eb6..115f273e02 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -27,6 +27,7 @@ auxillary receiver channels and other events such as failsafe detection. | 20 | 19 | TELEMETRY | Enable telemetry via switch | | 21 | 20 | AUTOTUNE | Autotune Pitch/Roll PIDs | | 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) | +| 26 | 25 | BLACKBOX | Enable BlackBox logging | ## Mode details diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1192793bb8..1695746347 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -256,6 +256,7 @@ typedef enum BlackboxState { BLACKBOX_STATE_SEND_GPS_G_HEADER, BLACKBOX_STATE_SEND_SLOW_HEADER, BLACKBOX_STATE_SEND_SYSINFO, + BLACKBOX_STATE_PAUSED, BLACKBOX_STATE_RUNNING, BLACKBOX_STATE_SHUTTING_DOWN } BlackboxState; @@ -349,6 +350,8 @@ static blackboxMainState_t blackboxHistoryRing[3]; // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old) static blackboxMainState_t* blackboxHistory[3]; +static bool blackboxModeActivationConditionPresent = false; + static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) { switch (condition) { @@ -454,9 +457,6 @@ static void blackboxSetState(BlackboxState newState) xmitState.headerIndex = 0; break; case BLACKBOX_STATE_RUNNING: - blackboxIteration = 0; - blackboxPFrameIndex = 0; - blackboxIFrameIndex = 0; blackboxSlowFrameIterationTimer = SLOW_FRAME_INTERVAL; //Force a slow frame to be written on the first iteration break; case BLACKBOX_STATE_SHUTTING_DOWN: @@ -479,25 +479,24 @@ static void writeIntraframe(void) blackboxWriteUnsignedVB(blackboxIteration); blackboxWriteUnsignedVB(blackboxCurrent->time); - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x]); - } - - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->axisPID_I[x]); - } + blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT); + // Don't bother writing the current D term if the corresponding PID setting is zero for (x = 0; x < XYZ_AXIS_COUNT; x++) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]); } } - for (x = 0; x < 3; x++) { - blackboxWriteSignedVB(blackboxCurrent->rcCommand[x]); - } + // Write roll, pitch and yaw first: + blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); - blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] + /* + * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. + * Throttle lies in range [minthrottle..maxthrottle]: + */ + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -516,9 +515,7 @@ static void writeIntraframe(void) #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->magADC[x]); - } + blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT); } #endif @@ -538,24 +535,20 @@ static void writeIntraframe(void) blackboxWriteUnsignedVB(blackboxCurrent->rssi); } - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->gyroADC[x]); - } - - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->accSmooth[x]); - } + blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT); + blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT); //Motors can be below minthrottle when disarmed, but that doesn't happen much blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); - //Motors tend to be similar to each other + //Motors tend to be similar to each other so use the first motor's value as a predictor of the others for (x = 1; x < motorCount; x++) { blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { - blackboxWriteSignedVB(blackboxHistory[0]->servo[5] - 1500); + //Assume the tail spends most of its time around the center + blackboxWriteSignedVB(blackboxCurrent->servo[5] - 1500); } //Rotate our history buffers: @@ -568,6 +561,20 @@ static void writeIntraframe(void) blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing; } +static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count) +{ + int16_t *curr = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory); + int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory); + int16_t *prev2 = (int16_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory); + + for (int i = 0; i < count; i++) { + // Predictor is the average of the previous two history states + int32_t predictor = (prev1[i] + prev2[i]) / 2; + + blackboxWriteSignedVB(curr[i] - predictor); + } +} + static void writeInterframe(void) { int x; @@ -586,18 +593,14 @@ static void writeInterframe(void) */ blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); - } - - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x]; - } + arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); /* * The PID I field changes very slowly, most of the time +-2, so use an encoding * that can pack all three fields into one byte in that situation. */ + arraySubInt32(deltas, blackboxCurrent->axisPID_I, blackboxLast->axisPID_I, XYZ_AXIS_COUNT); blackboxWriteTag2_3S32(deltas); /* @@ -657,18 +660,10 @@ static void writeInterframe(void) blackboxWriteTag8_8SVB(deltas, optionalFieldCount); - //Since gyros, accs and motors are noisy, base the prediction on the average of the history: - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->gyroADC[x] - (blackboxHistory[1]->gyroADC[x] + blackboxHistory[2]->gyroADC[x]) / 2); - } - - for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2); - } - - for (x = 0; x < motorCount; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); - } + //Since gyros, accs and motors are noisy, base their predictions on the average of the history: + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT); + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT); + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), motorCount); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); @@ -795,6 +790,12 @@ void startBlackbox(void) * cache those now. */ blackboxBuildConditionCache(); + + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX); + + blackboxIteration = 0; + blackboxPFrameIndex = 0; + blackboxIFrameIndex = 0; /* * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when @@ -811,7 +812,7 @@ void startBlackbox(void) */ void finishBlackbox(void) { - if (blackboxState == BLACKBOX_STATE_RUNNING) { + if (blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED) { blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL); blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN); @@ -1099,7 +1100,8 @@ static bool blackboxWriteSysinfo() */ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) { - if (blackboxState != BLACKBOX_STATE_RUNNING) { + // Only allow events to be logged after headers have been written + if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) { return; } @@ -1141,6 +1143,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWriteSignedVB(data->inflightAdjustment.newValue); } break; + case FLIGHT_LOG_EVENT_LOGGING_RESUME: + blackboxWriteUnsignedVB(data->loggingResume.logIteration); + blackboxWriteUnsignedVB(data->loggingResume.currentTime); + break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); blackboxWrite(0); @@ -1175,11 +1181,28 @@ static bool blackboxShouldLogPFrame(uint32_t pFrameIndex) return (pFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num; } +static bool blackboxShouldLogIFrame() { + return blackboxPFrameIndex == 0; +} + +// Called once every FC loop in order to keep track of how many FC loop iterations have passed +static void blackboxAdvanceIterationTimers() +{ + blackboxSlowFrameIterationTimer++; + blackboxIteration++; + blackboxPFrameIndex++; + + if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) { + blackboxPFrameIndex = 0; + blackboxIFrameIndex++; + } +} + // Called once every FC loop in order to log the current state static void blackboxLogIteration() { // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames - if (blackboxPFrameIndex == 0) { + if (blackboxShouldLogIFrame()) { /* * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding * an additional item to write at the same time) @@ -1226,15 +1249,6 @@ static void blackboxLogIteration() //Flush every iteration so that our runtime variance is minimized blackboxDeviceFlush(); - - blackboxSlowFrameIterationTimer++; - blackboxIteration++; - blackboxPFrameIndex++; - - if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) { - blackboxPFrameIndex = 0; - blackboxIFrameIndex++; - } } /** @@ -1305,10 +1319,33 @@ void handleBlackbox(void) blackboxSetState(BLACKBOX_STATE_RUNNING); } break; + case BLACKBOX_STATE_PAUSED: + // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from + if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) { + // Write a log entry so the decoder is aware that our large time/iteration skip is intended + flightLogEvent_loggingResume_t resume; + + resume.logIteration = blackboxIteration; + resume.currentTime = currentTime; + + blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume); + blackboxSetState(BLACKBOX_STATE_RUNNING); + + blackboxLogIteration(); + } + + // Keep the logging timers ticking so our log iteration continues to advance + blackboxAdvanceIterationTimers(); + break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 + if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) { + blackboxSetState(BLACKBOX_STATE_PAUSED); + } else { + blackboxLogIteration(); + } - blackboxLogIteration(); + blackboxAdvanceIterationTimers(); break; case BLACKBOX_STATE_SHUTTING_DOWN: //On entry of this state, startTime is set and a flush is performed diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 078a7deb4a..72ffa81481 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -107,6 +107,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, + FLIGHT_LOG_EVENT_LOGGING_RESUME = 14, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -147,6 +148,11 @@ typedef struct flightLogEvent_inflightAdjustment_t { float newFloatValue; } flightLogEvent_inflightAdjustment_t; +typedef struct flightLogEvent_loggingResume_t { + uint32_t logIteration; + uint32_t currentTime; +} flightLogEvent_loggingResume_t; + typedef union flightLogEventData_t { flightLogEvent_syncBeep_t syncBeep; @@ -154,6 +160,7 @@ typedef union flightLogEventData_t flightLogEvent_autotuneCycleResult_t autotuneCycleResult; flightLogEvent_autotuneTargets_t autotuneTargets; flightLogEvent_inflightAdjustment_t inflightAdjustment; + flightLogEvent_loggingResume_t loggingResume; } flightLogEventData_t; typedef struct flightLogEvent_t diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index fb0df826f3..02b01db45a 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -154,6 +154,20 @@ void blackboxWriteSignedVB(int32_t value) blackboxWriteUnsignedVB(zigzagEncode(value)); } +void blackboxWriteSignedVBArray(int32_t *array, int count) +{ + for (int i = 0; i < count; i++) { + blackboxWriteSignedVB(array[i]); + } +} + +void blackboxWriteSigned16VBArray(int16_t *array, int count) +{ + for (int i = 0; i < count; i++) { + blackboxWriteSignedVB(array[i]); + } +} + void blackboxWriteS16(int16_t value) { blackboxWrite(value & 0xFF); diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 89a1ad2530..b2e7d0d164 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -41,6 +41,8 @@ int blackboxPrint(const char *s); void blackboxWriteUnsignedVB(uint32_t value); void blackboxWriteSignedVB(int32_t value); +void blackboxWriteSignedVBArray(int32_t *array, int count); +void blackboxWriteSigned16VBArray(int16_t *array, int count); void blackboxWriteS16(int16_t value); void blackboxWriteTag2_3S32(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index fc95978374..8f1ae62f79 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -237,3 +237,10 @@ int32_t quickMedianFilter9(int32_t * v) QMF_SORT(p[4], p[2]); return p[4]; } + +void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count) +{ + for (int i = 0; i < count; i++) { + dest[i] = array1[i] - array2[i]; + } +} diff --git a/src/main/common/maths.h b/src/main/common/maths.h index ce96a80e9c..689d09fc69 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -94,4 +94,6 @@ float cos_approx(float x); #else #define sin_approx(x) sinf(x) #define cos_approx(x) cosf(x) -#endif \ No newline at end of file +#endif + +void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 69eb7f9fc7..aad42b80b4 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -310,6 +310,21 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat } +bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId) +{ + uint8_t index; + + for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { + modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; + + if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { + return true; + } + } + + return false; +} + bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) { if (!IS_RANGE_USABLE(range)) { return false; @@ -732,20 +747,10 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) { - uint8_t index; - escAndServoConfig = escAndServoConfigToUse; pidProfile = pidProfileToUse; - isUsingSticksToArm = true; - - for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; - if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { - isUsingSticksToArm = false; - break; - } - } + isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); } void resetAdjustmentStates(void) diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index de15935c44..0249334264 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -46,6 +46,7 @@ typedef enum { BOXSERVO1, BOXSERVO2, BOXSERVO3, + BOXBLACKBOX, CHECKBOX_ITEM_COUNT } boxId_e; @@ -239,3 +240,4 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); +bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 57962ef6bd..4fd0a9dfa1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -345,7 +345,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXSERVO1, "SERVO1;", 23 }, { BOXSERVO2, "SERVO2;", 24 }, { BOXSERVO3, "SERVO3;", 25 }, - + { BOXBLACKBOX, "BLACKBOX;", 26 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -692,6 +692,12 @@ void mspInit(serialConfig_t *serialConfig) } #endif +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)){ + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; + } +#endif + memset(mspPorts, 0x00, sizeof(mspPorts)); mspAllocateSerialPorts(serialConfig); } @@ -812,7 +818,8 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE | IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | - IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM; + IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); if (flag)