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

Merge pull request #1157 from cleanflight/blackbox-pause

Browse source 
Add pause/resume switch support to Blackbox
This commit is contained in:
Dominic Clifton 2015-08-03 21:26:36 +01:00
commit ae0c29125d
11 changed files with 164 additions and 72 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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

7
src/main/blackbox/blackbox_fielddefs.h
View file

@ -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

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

@ -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);

2
src/main/blackbox/blackbox_io.h
View file

@ -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);

7
src/main/common/maths.c
View file

@ -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];
}
}

4
src/main/common/maths.h
View file

@ -94,4 +94,6 @@ float cos_approx(float x);
#else
#define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(x)
#endif
#endif
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);

27
src/main/io/rc_controls.c
View file

@ -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)

2
src/main/io/rc_controls.h
View file

@ -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);

11
src/main/io/serial_msp.c
View file

@ -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)