rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Support logging Vbat, baro, mag 

Don't bother logging PID "D" results if the corresponding D setting is
zero
This commit is contained in:
Nicholas Sherlock 2014-12-27 19:17:44 +13:00
parent ff9dad067c
commit d195880bb8
3 changed files with 276 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

320
src/main/blackbox/blackbox.c
View File

@ -165,15 +165,26 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
{"axisI[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, {"axisI[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
{"axisI[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, {"axisI[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
{"axisI[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, {"axisI[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
{"axisD[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"axisD[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
{"axisD[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"axisD[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
{"axisD[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"axisD[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
/* rcCommands are encoded together as a group in P-frames: */ /* rcCommands are encoded together as a group in P-frames: */
{"rcCommand[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
/* Throttle is always in the range [minthrottle..maxthrottle]: */ /* Throttle is always in the range [minthrottle..maxthrottle]: */
{"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(ALWAYS)},
#ifdef MAG
{"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
{"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
{"magADC[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
#endif
#ifdef BARO
{"BaroAlt", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
#endif
/* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */ /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
{"gyroData[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroData[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroData[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroData[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@ -246,6 +257,12 @@ static serialPort_t *blackboxPort;
static portMode_t previousPortMode; static portMode_t previousPortMode;
static uint32_t previousBaudRate; static uint32_t previousBaudRate;
/*
* We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
* This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
* to encode:
*/
static uint16_t vbatReference;
static gpsState_t gpsHistory; static gpsState_t gpsHistory;
// Keep a history of length 2, plus a buffer for MW to store the new values into // Keep a history of length 2, plus a buffer for MW to store the new values into
@ -512,29 +529,89 @@ static void writeTag8_4S16(int32_t *values) {
blackboxWrite(buffer); blackboxWrite(buffer);
} }
/**
* Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is
* written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero).
*
* valueCount must be 8 or less.
*/
static void writeTag8_8SVB(int32_t *values, int valueCount)
{
uint8_t header;
int i;
if (valueCount > 0) {
//If we're only writing one field then we can skip the header
if (valueCount == 1) {
writeSignedVB(values[0]);
} else {
//First write a one-byte header that marks which fields are non-zero
header = 0;
// First field should be in low bits of header
for (i = valueCount - 1; i >= 0; i--) {
header <<= 1;
if (values[i] != 0)
header |= 0x01;
}
blackboxWrite(header);
for (i = 0; i < valueCount; i++)
if (values[i] != 0)
writeSignedVB(values[i]);
}
}
}
static bool testBlackboxCondition(FlightLogFieldCondition condition) static bool testBlackboxCondition(FlightLogFieldCondition condition)
{ {
switch (condition) { switch (condition) {
case FLIGHT_LOG_FIELD_CONDITION_ALWAYS: case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
return true; return true;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
return motorCount >= 1;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
return motorCount >= 2;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
return motorCount >= 3;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4:
return motorCount >= 4;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5:
return motorCount >= 5;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
return motorCount >= 6;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
return motorCount >= 7;
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
return motorCount >= 8; return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
return masterConfig.mixerMode == MIXER_TRI; return masterConfig.mixerMode == MIXER_TRI;
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2:
return currentProfile->pidProfile.P8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0] != 0;
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2:
return currentProfile->pidProfile.I8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0] != 0;
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
case FLIGHT_LOG_FIELD_CONDITION_MAG:
#ifdef MAG
return sensors(SENSOR_MAG);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_BARO:
#ifdef BARO
return sensors(SENSOR_BARO);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_NEVER: case FLIGHT_LOG_FIELD_CONDITION_NEVER:
return false; return false;
default: default:
@ -580,24 +657,45 @@ static void writeIntraframe(void)
writeUnsignedVB(blackboxIteration); writeUnsignedVB(blackboxIteration);
writeUnsignedVB(blackboxCurrent->time); writeUnsignedVB(blackboxCurrent->time);
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->axisPID_P[x]); writeSignedVB(blackboxCurrent->axisPID_P[x]);
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->axisPID_I[x]); writeSignedVB(blackboxCurrent->axisPID_I[x]);
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->axisPID_D[x]); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x))
writeSignedVB(blackboxCurrent->axisPID_D[x]);
for (x = 0; x < 3; x++) for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->rcCommand[x]); writeSignedVB(blackboxCurrent->rcCommand[x]);
writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle]
for (x = 0; x < 3; x++) /*
* Our voltage is expected to decrease over the course of the flight, so store our difference from
* the reference:
*
* Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
*/
writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
#ifdef MAG
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->magADC[x]);
}
#endif
#ifdef BARO
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO))
writeSignedVB(blackboxCurrent->BaroAlt);
#endif
for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->gyroData[x]); writeSignedVB(blackboxCurrent->gyroData[x]);
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->accSmooth[x]); writeSignedVB(blackboxCurrent->accSmooth[x]);
//Motors can be below minthrottle when disarmed, but that doesn't happen much //Motors can be below minthrottle when disarmed, but that doesn't happen much
@ -623,7 +721,7 @@ static void writeIntraframe(void)
static void writeInterframe(void) static void writeInterframe(void)
{ {
int x; int x;
int32_t deltas[4]; int32_t deltas[5];
blackboxValues_t *blackboxCurrent = blackboxHistory[0]; blackboxValues_t *blackboxCurrent = blackboxHistory[0];
blackboxValues_t *blackboxLast = blackboxHistory[1]; blackboxValues_t *blackboxLast = blackboxHistory[1];
@ -638,10 +736,10 @@ static void writeInterframe(void)
*/ */
writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]);
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x]; deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x];
/* /*
@ -650,23 +748,46 @@ static void writeInterframe(void)
*/ */
writeTag2_3S32(deltas); writeTag2_3S32(deltas);
for (x = 0; x < 3; x++) /*
writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]); * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
* always zero. So don't bother recording D results when PID D terms are zero.
for (x = 0; x < 4; x++) */
deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; for (x = 0; x < XYZ_AXIS_COUNT; x++)
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x))
writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
/* /*
* RC tends to stay the same or fairly small for many frames at a time, so use an encoding that * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
* can pack multiple values per byte: * can pack multiple values per byte:
*/ */
for (x = 0; x < 4; x++)
deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
writeTag8_4S16(deltas); writeTag8_4S16(deltas);
//Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO
int optionalFieldCount = 0;
deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
#ifdef MAG
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
for (x = 0; x < XYZ_AXIS_COUNT; x++)
deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
}
#endif
#ifdef BARO
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO))
deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
#endif
writeTag8_8SVB(deltas, optionalFieldCount);
//Since gyros, accs and motors are noisy, base the prediction on the average of the history: //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2);
for (x = 0; x < 3; x++) for (x = 0; x < XYZ_AXIS_COUNT; x++)
writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2); writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2);
for (x = 0; x < motorCount; x++) for (x = 0; x < motorCount; x++)
@ -750,6 +871,8 @@ void startBlackbox(void)
blackboxHistory[1] = &blackboxHistoryRing[1]; blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2]; blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
blackboxSetState(BLACKBOX_STATE_SEND_HEADER); blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
@ -802,25 +925,36 @@ static void loadBlackboxState(void)
blackboxCurrent->time = currentTime; blackboxCurrent->time = currentTime;
for (i = 0; i < 3; i++) for (i = 0; i < XYZ_AXIS_COUNT; i++)
blackboxCurrent->axisPID_P[i] = axisPID_P[i]; blackboxCurrent->axisPID_P[i] = axisPID_P[i];
for (i = 0; i < 3; i++) for (i = 0; i < XYZ_AXIS_COUNT; i++)
blackboxCurrent->axisPID_I[i] = axisPID_I[i]; blackboxCurrent->axisPID_I[i] = axisPID_I[i];
for (i = 0; i < 3; i++) for (i = 0; i < XYZ_AXIS_COUNT; i++)
blackboxCurrent->axisPID_D[i] = axisPID_D[i]; blackboxCurrent->axisPID_D[i] = axisPID_D[i];
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++)
blackboxCurrent->rcCommand[i] = rcCommand[i]; blackboxCurrent->rcCommand[i] = rcCommand[i];
for (i = 0; i < 3; i++) for (i = 0; i < XYZ_AXIS_COUNT; i++)
blackboxCurrent->gyroData[i] = gyroData[i]; blackboxCurrent->gyroData[i] = gyroData[i];
for (i = 0; i < 3; i++) for (i = 0; i < XYZ_AXIS_COUNT; i++)
blackboxCurrent->accSmooth[i] = accSmooth[i]; blackboxCurrent->accSmooth[i] = accSmooth[i];
for (i = 0; i < motorCount; i++) for (i = 0; i < motorCount; i++)
blackboxCurrent->motor[i] = motor[i]; blackboxCurrent->motor[i] = motor[i];
blackboxCurrent->vbatLatest = vbatLatest;
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++)
blackboxCurrent->magADC[i] = magADC[i];
#endif
#ifdef BARO
blackboxCurrent->BaroAlt = BaroAlt;
#endif
//Tail servo for tricopters //Tail servo for tricopters
blackboxCurrent->servo[5] = servo[5]; blackboxCurrent->servo[5] = servo[5];
} }
@ -881,9 +1015,15 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
if (headerXmitIndex == 0) { if (headerXmitIndex == 0) {
charsWritten += blackboxPrint(def->name); charsWritten += blackboxPrint(def->name);
} else { } else {
//The other headers are integers (format as single digit for now): //The other headers are integers
blackboxWrite(def->arr[headerXmitIndex - 1] + '0'); if (def->arr[headerXmitIndex - 1] >= 10) {
charsWritten++; blackboxWrite(def->arr[headerXmitIndex - 1] / 10 + '0');
blackboxWrite(def->arr[headerXmitIndex - 1] % 10 + '0');
charsWritten += 2;
} else {
blackboxWrite(def->arr[headerXmitIndex - 1] + '0');
charsWritten++;
}
} }
} }
} }
@ -898,15 +1038,75 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
return headerXmitIndex < headerCount; return headerXmitIndex < headerCount;
} }
void handleBlackbox(void) static int blackboxWriteSysinfo(int xmitIndex)
{ {
int i;
union floatConvert_t { union floatConvert_t {
float f; float f;
uint32_t u; uint32_t u;
} floatConvert; } floatConvert;
switch (xmitIndex) {
case 0:
blackboxPrintf("H Firmware type:Cleanflight\n");
break;
case 1:
// Pause to allow more time for previous to transmit (it exceeds our chunk size)
break;
case 2:
blackboxPrintf("H Firmware revision:%s\n", shortGitRevision);
break;
case 3:
// Pause to allow more time for previous to transmit
break;
case 4:
blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime);
break;
case 5:
// Pause to allow more time for previous to transmit
break;
case 6:
blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
break;
case 7:
blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle);
break;
case 8:
blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle);
break;
case 9:
floatConvert.f = gyro.scale;
blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
break;
case 10:
blackboxPrintf("H acc_1G:%u\n", acc_1G);
break;
case 11:
blackboxPrintf("H vbatscale:%u\n", masterConfig.batteryConfig.vbatscale);
break;
case 12:
blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig.batteryConfig.vbatmincellvoltage,
masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage);
break;
case 13:
//Pause
break;
case 14:
blackboxPrintf("H vbatref:%u\n", vbatReference);
break;
case 15:
// One more pause for good luck
break;
default:
blackboxSetState(BLACKBOX_STATE_RUNNING);
}
return xmitIndex + 1;
}
void handleBlackbox(void)
{
int i;
switch (blackboxState) { switch (blackboxState) {
case BLACKBOX_STATE_SEND_HEADER: case BLACKBOX_STATE_SEND_HEADER:
//On entry of this state, headerXmitIndex is 0 and startTime is intialised //On entry of this state, headerXmitIndex is 0 and startTime is intialised
@ -949,49 +1149,7 @@ void handleBlackbox(void)
break; break;
case BLACKBOX_STATE_SEND_SYSINFO: case BLACKBOX_STATE_SEND_SYSINFO:
//On entry of this state, headerXmitIndex is 0 //On entry of this state, headerXmitIndex is 0
switch (headerXmitIndex) { headerXmitIndex = blackboxWriteSysinfo(headerXmitIndex);
case 0:
blackboxPrintf("H Firmware type:Cleanflight\n");
break;
case 1:
// Pause to allow more time for previous to transmit (it exceeds our chunk size)
break;
case 2:
blackboxPrintf("H Firmware revision:%s\n", shortGitRevision);
break;
case 3:
// Pause to allow more time for previous to transmit
break;
case 4:
blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime);
break;
case 5:
// Pause to allow more time for previous to transmit
break;
case 6:
blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
break;
case 7:
blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle);
break;
case 8:
blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle);
break;
case 9:
floatConvert.f = gyro.scale;
blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
break;
case 10:
blackboxPrintf("H acc_1G:%u\n", acc_1G);
break;
case 11:
// One more pause for good luck
break;
default:
blackboxSetState(BLACKBOX_STATE_RUNNING);
}
headerXmitIndex++;
break; break;
case BLACKBOX_STATE_RUNNING: case BLACKBOX_STATE_RUNNING:
// On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0

16
src/main/blackbox/blackbox.h
View File

@ -17,18 +17,28 @@
#pragma once #pragma once
#include "common/axis.h"
#include <stdint.h> #include <stdint.h>
typedef struct blackboxValues_t { typedef struct blackboxValues_t {
uint32_t time; uint32_t time;
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT];
int16_t rcCommand[4]; int16_t rcCommand[4];
int16_t gyroData[3]; int16_t gyroData[XYZ_AXIS_COUNT];
int16_t accSmooth[3]; int16_t accSmooth[XYZ_AXIS_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS]; int16_t servo[MAX_SUPPORTED_SERVOS];
uint16_t vbatLatest;
#ifdef BARO
int32_t BaroAlt;
#endif
#ifdef MAG
int16_t magADC[XYZ_AXIS_COUNT];
#endif
} blackboxValues_t; } blackboxValues_t;
void initBlackbox(void); void initBlackbox(void);

28
src/main/blackbox/blackbox_fielddefs.h
View File

@ -28,6 +28,20 @@ typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
FLIGHT_LOG_FIELD_CONDITION_TRICOPTER, FLIGHT_LOG_FIELD_CONDITION_TRICOPTER,
FLIGHT_LOG_FIELD_CONDITION_MAG = 20,
FLIGHT_LOG_FIELD_CONDITION_BARO,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0 = 40,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2,
FLIGHT_LOG_FIELD_CONDITION_NEVER = 255, FLIGHT_LOG_FIELD_CONDITION_NEVER = 255,
} FlightLogFieldCondition; } FlightLogFieldCondition;
@ -57,15 +71,21 @@ typedef enum FlightLogFieldPredictor {
FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7, FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7,
//Predict 1500 //Predict 1500
FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8 FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8,
//Predict vbatref, the reference ADC level stored in the header
FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9
} FlightLogFieldPredictor; } FlightLogFieldPredictor;
typedef enum FlightLogFieldEncoding { typedef enum FlightLogFieldEncoding {
FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0, FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0, // Signed variable-byte
FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB = 1, FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB = 1, // Unsigned variable-byte
FLIGHT_LOG_FIELD_ENCODING_NEG_14BIT = 3, // Unsigned variable-byte but we negate the value before storing, value is 14 bits
FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6,
FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7, FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8, FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8,
FLIGHT_LOG_FIELD_ENCODING_NULL = 9 FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero
} FlightLogFieldEncoding; } FlightLogFieldEncoding;
typedef enum FlightLogFieldSign { typedef enum FlightLogFieldSign {