diff --git a/Makefile b/Makefile index f2d4c5951..a62cc1ab8 100644 --- a/Makefile +++ b/Makefile @@ -248,7 +248,8 @@ HIGHEND_SRC = flight/autotune.c \ telemetry/smartport.c \ sensors/sonar.c \ sensors/barometer.c \ - blackbox/blackbox.c + blackbox/blackbox.c \ + blackbox/blackbox_io.c VCP_SRC = \ vcp/hw_config.c \ @@ -395,6 +396,7 @@ CJMCU_SRC = \ drivers/timer.c \ drivers/timer_stm32f10x.c \ blackbox/blackbox.c \ + blackbox/blackbox_io.c \ hardware_revision.c \ $(COMMON_SRC) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49677e062..d88b27910 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" #include "version.h" @@ -37,8 +36,6 @@ #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "common/printf.h" - #include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" @@ -75,9 +72,8 @@ #include "config/config_master.h" #include "blackbox.h" +#include "blackbox_io.h" -#define BLACKBOX_BAUDRATE 115200 -#define BLACKBOX_INITIAL_PORT_MODE MODE_TX #define BLACKBOX_I_INTERVAL 32 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) @@ -255,9 +251,6 @@ extern uint8_t motorCount; //From mw.c: extern uint32_t currentTime; -// How many bytes should we transmit per loop iteration? -static uint8_t serialChunkSize = 16; - static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static struct { @@ -281,10 +274,6 @@ STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; -static serialPort_t *blackboxPort; -static portMode_t previousPortMode; -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 @@ -299,306 +288,6 @@ static blackboxValues_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 blackboxValues_t* blackboxHistory[3]; -static void blackboxWrite(uint8_t value) -{ - serialWrite(blackboxPort, value); -} - -static void _putc(void *p, char c) -{ - (void)p; - serialWrite(blackboxPort, c); -} - -//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!) -static void blackboxPrintf(char *fmt, ...) -{ - va_list va; - va_start(va, fmt); - tfp_format(NULL, _putc, fmt, va); - va_end(va); -} - -// Print the null-terminated string 's' to the serial port and return the number of bytes written -static int blackboxPrint(const char *s) -{ - const char *pos = s; - - while (*pos) { - serialWrite(blackboxPort, *pos); - pos++; - } - - return pos - s; -} - -/** - * Write an unsigned integer to the blackbox serial port using variable byte encoding. - */ -static void writeUnsignedVB(uint32_t value) -{ - //While this isn't the final byte (we can only write 7 bits at a time) - while (value > 127) { - blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" - value >>= 7; - } - blackboxWrite(value); -} - -/** - * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. - */ -static void writeSignedVB(int32_t value) -{ - //ZigZag encode to make the value always positive - writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); -} - -static void writeS16(int16_t value) -{ - blackboxWrite(value & 0xFF); - blackboxWrite((value >> 8) & 0xFF); -} - -/** - * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits - */ -static void writeTag2_3S32(int32_t *values) { - static const int NUM_FIELDS = 3; - - //Need to be enums rather than const ints if we want to switch on them (due to being C) - enum { - BITS_2 = 0, - BITS_4 = 1, - BITS_6 = 2, - BITS_32 = 3 - }; - - enum { - BYTES_1 = 0, - BYTES_2 = 1, - BYTES_3 = 2, - BYTES_4 = 3 - }; - - int x; - int selector = BITS_2, selector2; - - /* - * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes - * below: - * - * Selector possibilities - * - * 2 bits per field ss11 2233, - * 4 bits per field ss00 1111 2222 3333 - * 6 bits per field ss11 1111 0022 2222 0033 3333 - * 32 bits per field sstt tttt followed by fields of various byte counts - */ - for (x = 0; x < NUM_FIELDS; x++) { - //Require more than 6 bits? - if (values[x] >= 32 || values[x] < -32) { - selector = BITS_32; - break; - } - - //Require more than 4 bits? - if (values[x] >= 8 || values[x] < -8) { - if (selector < BITS_6) - selector = BITS_6; - } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits? - if (selector < BITS_4) - selector = BITS_4; - } - } - - switch (selector) { - case BITS_2: - blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); - break; - case BITS_4: - blackboxWrite((selector << 6) | (values[0] & 0x0F)); - blackboxWrite((values[1] << 4) | (values[2] & 0x0F)); - break; - case BITS_6: - blackboxWrite((selector << 6) | (values[0] & 0x3F)); - blackboxWrite((uint8_t)values[1]); - blackboxWrite((uint8_t)values[2]); - break; - case BITS_32: - /* - * Do another round to compute a selector for each field, assuming that they are at least 8 bits each - * - * Selector2 field possibilities - * 0 - 8 bits - * 1 - 16 bits - * 2 - 24 bits - * 3 - 32 bits - */ - selector2 = 0; - - //Encode in reverse order so the first field is in the low bits: - for (x = NUM_FIELDS - 1; x >= 0; x--) { - selector2 <<= 2; - - if (values[x] < 128 && values[x] >= -128) - selector2 |= BYTES_1; - else if (values[x] < 32768 && values[x] >= -32768) - selector2 |= BYTES_2; - else if (values[x] < 8388608 && values[x] >= -8388608) - selector2 |= BYTES_3; - else - selector2 |= BYTES_4; - } - - //Write the selectors - blackboxWrite((selector << 6) | selector2); - - //And now the values according to the selectors we picked for them - for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) { - switch (selector2 & 0x03) { - case BYTES_1: - blackboxWrite(values[x]); - break; - case BYTES_2: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - break; - case BYTES_3: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x] >> 16); - break; - case BYTES_4: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x] >> 16); - blackboxWrite(values[x] >> 24); - break; - } - } - break; - } -} - -/** - * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. - */ -static void writeTag8_4S16(int32_t *values) { - - //Need to be enums rather than const ints if we want to switch on them (due to being C) - enum { - FIELD_ZERO = 0, - FIELD_4BIT = 1, - FIELD_8BIT = 2, - FIELD_16BIT = 3 - }; - - uint8_t selector, buffer; - int nibbleIndex; - int x; - - selector = 0; - //Encode in reverse order so the first field is in the low bits: - for (x = 3; x >= 0; x--) { - selector <<= 2; - - if (values[x] == 0) - selector |= FIELD_ZERO; - else if (values[x] < 8 && values[x] >= -8) - selector |= FIELD_4BIT; - else if (values[x] < 128 && values[x] >= -128) - selector |= FIELD_8BIT; - else - selector |= FIELD_16BIT; - } - - blackboxWrite(selector); - - nibbleIndex = 0; - buffer = 0; - for (x = 0; x < 4; x++, selector >>= 2) { - switch (selector & 0x03) { - case FIELD_ZERO: - //No-op - break; - case FIELD_4BIT: - if (nibbleIndex == 0) { - //We fill high-bits first - buffer = values[x] << 4; - nibbleIndex = 1; - } else { - blackboxWrite(buffer | (values[x] & 0x0F)); - nibbleIndex = 0; - } - break; - case FIELD_8BIT: - if (nibbleIndex == 0) - blackboxWrite(values[x]); - else { - //Write the high bits of the value first (mask to avoid sign extension) - blackboxWrite(buffer | ((values[x] >> 4) & 0x0F)); - //Now put the leftover low bits into the top of the next buffer entry - buffer = values[x] << 4; - } - break; - case FIELD_16BIT: - if (nibbleIndex == 0) { - //Write high byte first - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x]); - } else { - //First write the highest 4 bits - blackboxWrite(buffer | ((values[x] >> 12) & 0x0F)); - // Then the middle 8 - blackboxWrite(values[x] >> 4); - //Only the smallest 4 bits are still left to write - buffer = values[x] << 4; - } - break; - } - } - //Anything left over to write? - if (nibbleIndex == 1) - 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 testBlackboxConditionUncached(FlightLogFieldCondition condition) { switch (condition) { @@ -660,8 +349,9 @@ static void blackboxBuildConditionCache() blackboxConditionCache = 0; for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { - if (testBlackboxConditionUncached(cond)) + if (testBlackboxConditionUncached(cond)) { blackboxConditionCache |= 1 << cond; + } } } @@ -694,6 +384,7 @@ static void blackboxSetState(BlackboxState newState) break; case BLACKBOX_STATE_SHUTTING_DOWN: xmitState.u.startTime = millis(); + blackboxDeviceFlush(); break; default: ; @@ -708,23 +399,28 @@ static void writeIntraframe(void) blackboxWrite('I'); - writeUnsignedVB(blackboxIteration); - writeUnsignedVB(blackboxCurrent->time); + blackboxWriteUnsignedVB(blackboxIteration); + blackboxWriteUnsignedVB(blackboxCurrent->time); - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_P[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_I[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_I[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]); + 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++) - writeSignedVB(blackboxCurrent->rcCommand[x]); + for (x = 0; x < 3; x++) { + blackboxWriteSignedVB(blackboxCurrent->rcCommand[x]); + } - writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -733,41 +429,47 @@ static void writeIntraframe(void) * * Write 14 bits even if the number is negative (which would otherwise result in 32 bits) */ - writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); + blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { // 12bit value directly from ADC - writeUnsignedVB(blackboxCurrent->amperageLatest); + blackboxWriteUnsignedVB(blackboxCurrent->amperageLatest); } #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->magADC[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->magADC[x]); + } } #endif #ifdef BARO - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) - writeSignedVB(blackboxCurrent->BaroAlt); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) { + blackboxWriteSignedVB(blackboxCurrent->BaroAlt); + } #endif - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->gyroData[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->gyroData[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->accSmooth[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->accSmooth[x]); + } //Motors can be below minthrottle when disarmed, but that doesn't happen much - writeUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); //Motors tend to be similar to each other - for (x = 1; x < motorCount; x++) - writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + for (x = 1; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) - writeSignedVB(blackboxHistory[0]->servo[5] - 1500); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { + blackboxWriteSignedVB(blackboxHistory[0]->servo[5] - 1500); + } //Rotate our history buffers: @@ -795,36 +497,41 @@ static void writeInterframe(void) * Since the difference between the difference between successive times will be nearly zero (due to consistent * looptime spacing), use second-order differences. */ - writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); + blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); + 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++) + for (x = 0; x < XYZ_AXIS_COUNT; x++) { deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x]; + } /* * 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. */ - writeTag2_3S32(deltas); + blackboxWriteTag2_3S32(deltas); /* * 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 < XYZ_AXIS_COUNT; x++) - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) - writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { + blackboxWriteSignedVB(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 * can pack multiple values per byte: */ - for (x = 0; x < 4; x++) + for (x = 0; x < 4; x++) { deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; + } - writeTag8_4S16(deltas); + blackboxWriteTag8_4S16(deltas); //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO int optionalFieldCount = 0; @@ -839,29 +546,35 @@ static void writeInterframe(void) #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) + 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)) + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) { deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt; + } #endif - writeTag8_8SVB(deltas, optionalFieldCount); + 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++) - writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[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++) - writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); + for (x = 0; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); + } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) - writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { + blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + } //Rotate our history buffers blackboxHistory[2] = blackboxHistory[1]; @@ -871,8 +584,10 @@ static void writeInterframe(void) static int gcd(int num, int denom) { - if (denom == 0) + if (denom == 0) { return num; + } + return gcd(denom, num % denom); } @@ -892,59 +607,12 @@ static void validateBlackboxConfig() } } -static void configureBlackboxPort(void) -{ - blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - - serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); - serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); - beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - } else { - blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); - - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - } - } - - /* - * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If - * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should - * transmit with each write. - * - * 9 / 1250 = 7200 / 1000000 - */ - serialChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); -} - -static void releaseBlackboxPort(void) -{ - serialSetMode(blackboxPort, previousPortMode); - serialSetBaudRate(blackboxPort, previousBaudRate); - - endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - - /* - * Normally this would be handled by mw.c, but since we take an unknown amount - * of time to shut down asynchronously, we're the only ones that know when to call it. - */ - if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { - mspAllocateSerialPorts(&masterConfig.serialConfig); - } -} - void startBlackbox(void) { if (blackboxState == BLACKBOX_STATE_STOPPED) { validateBlackboxConfig(); - configureBlackboxPort(); - - if (!blackboxPort) { + if (!blackboxDeviceOpen()) { blackboxSetState(BLACKBOX_STATE_DISABLED); return; } @@ -982,7 +650,7 @@ void finishBlackbox(void) * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event. * Just give the port back and stop immediately. */ - releaseBlackboxPort(); + blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } } @@ -992,8 +660,8 @@ static void writeGPSHomeFrame() { blackboxWrite('H'); - writeSignedVB(GPS_home[0]); - writeSignedVB(GPS_home[1]); + blackboxWriteSignedVB(GPS_home[0]); + blackboxWriteSignedVB(GPS_home[1]); //TODO it'd be great if we could grab the GPS current time and write that too gpsHistory.GPS_home[0] = GPS_home[0]; @@ -1012,15 +680,15 @@ static void writeGPSFrame() */ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) { // Predict the time of the last frame in the main log - writeUnsignedVB(currentTime - blackboxHistory[1]->time); + blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time); } - writeUnsignedVB(GPS_numSat); - writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); - writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); - writeUnsignedVB(GPS_altitude); - writeUnsignedVB(GPS_speed); - writeUnsignedVB(GPS_ground_course); + blackboxWriteUnsignedVB(GPS_numSat); + blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); + blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); + blackboxWriteUnsignedVB(GPS_altitude); + blackboxWriteUnsignedVB(GPS_speed); + blackboxWriteUnsignedVB(GPS_ground_course); gpsHistory.GPS_numSat = GPS_numSat; gpsHistory.GPS_coord[0] = GPS_coord[0]; @@ -1038,31 +706,39 @@ static void loadBlackboxState(void) blackboxCurrent->time = currentTime; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_P[i] = axisPID_P[i]; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + } + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_I[i] = axisPID_I[i]; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + } + for (i = 0; i < XYZ_AXIS_COUNT; 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]; + } - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->gyroData[i] = gyroData[i]; + } - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->accSmooth[i] = accSmooth[i]; + } - for (i = 0; i < motorCount; i++) + for (i = 0; i < motorCount; i++) { blackboxCurrent->motor[i] = motor[i]; + } blackboxCurrent->vbatLatest = vbatLatestADC; blackboxCurrent->amperageLatest = amperageLatestADC; #ifdef MAG - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->magADC[i] = magADC[i]; + } #endif #ifdef BARO @@ -1103,8 +779,9 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he * the whole header. */ if (xmitState.u.fieldIndex == -1) { - if (xmitState.headerIndex >= headerCount) + if (xmitState.headerIndex >= headerCount) { return false; //Someone probably called us again after we had already completed transmission + } charsWritten = blackboxPrint("H Field "); charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]); @@ -1115,15 +792,16 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he } else charsWritten = 0; - for (; xmitState.u.fieldIndex < fieldCount && charsWritten < serialChunkSize; xmitState.u.fieldIndex++) { + for (; xmitState.u.fieldIndex < fieldCount && charsWritten < blackboxWriteChunkSize; xmitState.u.fieldIndex++) { def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex); if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) { if (needComma) { blackboxWrite(','); charsWritten++; - } else + } else { needComma = true; + } // The first header is a field name if (xmitState.headerIndex == 0) { @@ -1169,7 +847,7 @@ static bool blackboxWriteSysinfo() } // How many bytes can we afford to transmit this loop? - xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + serialChunkSize, 64); + xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + blackboxWriteChunkSize, 64); // Most headers will consume at least 20 bytes so wait until we've built up that much link budget if (xmitState.u.serialBudget < 20) { @@ -1262,8 +940,9 @@ static bool blackboxWriteSysinfo() */ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) { - if (blackboxState != BLACKBOX_STATE_RUNNING) + if (blackboxState != BLACKBOX_STATE_RUNNING) { return; + } //Shared header for event frames blackboxWrite('E'); @@ -1272,7 +951,7 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) //Now serialize the data for this specific frame type switch (event) { case FLIGHT_LOG_EVENT_SYNC_BEEP: - writeUnsignedVB(data->syncBeep.time); + blackboxWriteUnsignedVB(data->syncBeep.time); break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: blackboxWrite(data->autotuneCycleStart.phase); @@ -1288,11 +967,11 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWrite(data->autotuneCycleStart.d); break; case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS: - writeS16(data->autotuneTargets.currentAngle); + blackboxWriteS16(data->autotuneTargets.currentAngle); blackboxWrite((uint8_t) data->autotuneTargets.targetAngle); blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak); - writeS16(data->autotuneTargets.firstPeakAngle); - writeS16(data->autotuneTargets.secondPeakAngle); + blackboxWriteS16(data->autotuneTargets.firstPeakAngle); + blackboxWriteS16(data->autotuneTargets.secondPeakAngle); break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); @@ -1333,11 +1012,13 @@ void handleBlackbox(void) * buffer. */ if (millis() > xmitState.u.startTime + 100) { - for (i = 0; i < serialChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) + for (i = 0; i < blackboxWriteChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { blackboxWrite(blackboxHeader[xmitState.headerIndex]); + } - if (blackboxHeader[xmitState.headerIndex] == '\0') + if (blackboxHeader[xmitState.headerIndex] == '\0') { blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO); + } } break; case BLACKBOX_STATE_SEND_FIELDINFO: @@ -1345,9 +1026,9 @@ void handleBlackbox(void) if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { #ifdef GPS - if (feature(FEATURE_GPS)) + if (feature(FEATURE_GPS)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS); - else + } else #endif blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } @@ -1372,8 +1053,9 @@ void handleBlackbox(void) //On entry of this state, xmitState.headerIndex is 0 //Keep writing chunks of the system info headers until it returns true to signal completion - if (blackboxWriteSysinfo()) + if (blackboxWriteSysinfo()) { blackboxSetState(BLACKBOX_STATE_PRERUN); + } break; case BLACKBOX_STATE_PRERUN: blackboxSetState(BLACKBOX_STATE_RUNNING); @@ -1425,7 +1107,7 @@ void handleBlackbox(void) } break; case BLACKBOX_STATE_SHUTTING_DOWN: - //On entry of this state, startTime is set + //On entry of this state, startTime is set and a flush is performed /* * Wait for the log we've transmitted to make its way to the logger before we release the serial port, @@ -1433,8 +1115,8 @@ void handleBlackbox(void) * * Don't wait longer than it could possibly take if something funky happens. */ - if (millis() > xmitState.u.startTime + 200 || isSerialTransmitBufferEmpty(blackboxPort)) { - releaseBlackboxPort(); + if (millis() > xmitState.u.startTime + 200 || isBlackboxDeviceIdle()) { + blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } break; @@ -1445,16 +1127,14 @@ void handleBlackbox(void) static bool canUseBlackboxWithCurrentConfiguration(void) { - if (!feature(FEATURE_BLACKBOX)) - return false; - - return true; + return feature(FEATURE_BLACKBOX); } void initBlackbox(void) { - if (canUseBlackboxWithCurrentConfiguration()) + if (canUseBlackboxWithCurrentConfiguration()) { blackboxSetState(BLACKBOX_STATE_STOPPED); - else + } else { blackboxSetState(BLACKBOX_STATE_DISABLED); + } } diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c new file mode 100644 index 000000000..194c03c40 --- /dev/null +++ b/src/main/blackbox/blackbox_io.c @@ -0,0 +1,437 @@ +#include +#include + +#include "blackbox_io.h" + +#include "platform.h" +#include "version.h" + +#include "common/maths.h" +#include "common/axis.h" +#include "common/color.h" + +#include "drivers/gpio.h" +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/serial.h" +#include "drivers/compass.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/accgyro.h" +#include "drivers/light_led.h" +#include "drivers/sound_beeper.h" + +#include "flight/flight.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "io/beeper.h" +#include "io/display.h" +#include "io/escservo.h" +#include "rx/rx.h" +#include "io/rc_controls.h" +#include "flight/mixer.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/autotune.h" +#include "flight/navigation.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/serial.h" +#include "io/serial_cli.h" +#include "io/serial_msp.h" +#include "io/statusindicator.h" +#include "rx/msp.h" +#include "telemetry/telemetry.h" +#include "common/printf.h" + +#include "config/runtime_config.h" +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#define BLACKBOX_BAUDRATE 115200 +#define BLACKBOX_INITIAL_PORT_MODE MODE_TX + +// How many bytes should we transmit per loop iteration? +uint8_t blackboxWriteChunkSize = 16; + +static serialPort_t *blackboxPort; +static portMode_t previousPortMode; +static uint32_t previousBaudRate; + +void blackboxWrite(uint8_t value) +{ + serialWrite(blackboxPort, value); +} + +static void _putc(void *p, char c) +{ + (void)p; + blackboxWrite(c); +} + +//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!) +void blackboxPrintf(char *fmt, ...) +{ + va_list va; + va_start(va, fmt); + tfp_format(NULL, _putc, fmt, va); + va_end(va); +} + +// Print the null-terminated string 's' to the serial port and return the number of bytes written +int blackboxPrint(const char *s) +{ + const char *pos = s; + + while (*pos) { + blackboxWrite(*pos); + pos++; + } + + return pos - s; +} + +/** + * Write an unsigned integer to the blackbox serial port using variable byte encoding. + */ +void blackboxWriteUnsignedVB(uint32_t value) +{ + //While this isn't the final byte (we can only write 7 bits at a time) + while (value > 127) { + blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" + value >>= 7; + } + blackboxWrite(value); +} + +/** + * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. + */ +void blackboxWriteSignedVB(int32_t value) +{ + //ZigZag encode to make the value always positive + blackboxWriteUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); +} + +void blackboxWriteS16(int16_t value) +{ + blackboxWrite(value & 0xFF); + blackboxWrite((value >> 8) & 0xFF); +} + +/** + * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits + */ +void blackboxWriteTag2_3S32(int32_t *values) { + static const int NUM_FIELDS = 3; + + //Need to be enums rather than const ints if we want to switch on them (due to being C) + enum { + BITS_2 = 0, + BITS_4 = 1, + BITS_6 = 2, + BITS_32 = 3 + }; + + enum { + BYTES_1 = 0, + BYTES_2 = 1, + BYTES_3 = 2, + BYTES_4 = 3 + }; + + int x; + int selector = BITS_2, selector2; + + /* + * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes + * below: + * + * Selector possibilities + * + * 2 bits per field ss11 2233, + * 4 bits per field ss00 1111 2222 3333 + * 6 bits per field ss11 1111 0022 2222 0033 3333 + * 32 bits per field sstt tttt followed by fields of various byte counts + */ + for (x = 0; x < NUM_FIELDS; x++) { + //Require more than 6 bits? + if (values[x] >= 32 || values[x] < -32) { + selector = BITS_32; + break; + } + + //Require more than 4 bits? + if (values[x] >= 8 || values[x] < -8) { + if (selector < BITS_6) { + selector = BITS_6; + } + } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits? + if (selector < BITS_4) { + selector = BITS_4; + } + } + } + + switch (selector) { + case BITS_2: + blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); + break; + case BITS_4: + blackboxWrite((selector << 6) | (values[0] & 0x0F)); + blackboxWrite((values[1] << 4) | (values[2] & 0x0F)); + break; + case BITS_6: + blackboxWrite((selector << 6) | (values[0] & 0x3F)); + blackboxWrite((uint8_t)values[1]); + blackboxWrite((uint8_t)values[2]); + break; + case BITS_32: + /* + * Do another round to compute a selector for each field, assuming that they are at least 8 bits each + * + * Selector2 field possibilities + * 0 - 8 bits + * 1 - 16 bits + * 2 - 24 bits + * 3 - 32 bits + */ + selector2 = 0; + + //Encode in reverse order so the first field is in the low bits: + for (x = NUM_FIELDS - 1; x >= 0; x--) { + selector2 <<= 2; + + if (values[x] < 128 && values[x] >= -128) { + selector2 |= BYTES_1; + } else if (values[x] < 32768 && values[x] >= -32768) { + selector2 |= BYTES_2; + } else if (values[x] < 8388608 && values[x] >= -8388608) { + selector2 |= BYTES_3; + } else { + selector2 |= BYTES_4; + } + } + + //Write the selectors + blackboxWrite((selector << 6) | selector2); + + //And now the values according to the selectors we picked for them + for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) { + switch (selector2 & 0x03) { + case BYTES_1: + blackboxWrite(values[x]); + break; + case BYTES_2: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + break; + case BYTES_3: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + break; + case BYTES_4: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + blackboxWrite(values[x] >> 24); + break; + } + } + break; + } +} + +/** + * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. + */ +void blackboxWriteTag8_4S16(int32_t *values) { + + //Need to be enums rather than const ints if we want to switch on them (due to being C) + enum { + FIELD_ZERO = 0, + FIELD_4BIT = 1, + FIELD_8BIT = 2, + FIELD_16BIT = 3 + }; + + uint8_t selector, buffer; + int nibbleIndex; + int x; + + selector = 0; + //Encode in reverse order so the first field is in the low bits: + for (x = 3; x >= 0; x--) { + selector <<= 2; + + if (values[x] == 0) { + selector |= FIELD_ZERO; + } else if (values[x] < 8 && values[x] >= -8) { + selector |= FIELD_4BIT; + } else if (values[x] < 128 && values[x] >= -128) { + selector |= FIELD_8BIT; + } else { + selector |= FIELD_16BIT; + } + } + + blackboxWrite(selector); + + nibbleIndex = 0; + buffer = 0; + for (x = 0; x < 4; x++, selector >>= 2) { + switch (selector & 0x03) { + case FIELD_ZERO: + //No-op + break; + case FIELD_4BIT: + if (nibbleIndex == 0) { + //We fill high-bits first + buffer = values[x] << 4; + nibbleIndex = 1; + } else { + blackboxWrite(buffer | (values[x] & 0x0F)); + nibbleIndex = 0; + } + break; + case FIELD_8BIT: + if (nibbleIndex == 0) { + blackboxWrite(values[x]); + } else { + //Write the high bits of the value first (mask to avoid sign extension) + blackboxWrite(buffer | ((values[x] >> 4) & 0x0F)); + //Now put the leftover low bits into the top of the next buffer entry + buffer = values[x] << 4; + } + break; + case FIELD_16BIT: + if (nibbleIndex == 0) { + //Write high byte first + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x]); + } else { + //First write the highest 4 bits + blackboxWrite(buffer | ((values[x] >> 12) & 0x0F)); + // Then the middle 8 + blackboxWrite(values[x] >> 4); + //Only the smallest 4 bits are still left to write + buffer = values[x] << 4; + } + break; + } + } + //Anything left over to write? + if (nibbleIndex == 1) { + 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. + */ +void blackboxWriteTag8_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) { + blackboxWriteSignedVB(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) { + blackboxWriteSignedVB(values[i]); + } + } + } + } +} + +/** + * If there is data waiting to be written to the blackbox device, attempt to write that now. + */ +void blackboxDeviceFlush(void) +{ + //Presently a no-op on serial +} + +/** + * Attempt to open the logging device. Returns true if successful. + */ +bool blackboxDeviceOpen(void) +{ + blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + + serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); + serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); + beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + } else { + blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + } + } + + /* + * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If + * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should + * transmit with each write. + * + * 9 / 1250 = 7200 / 1000000 + */ + blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); + + return blackboxPort != NULL; +} + +void blackboxDeviceClose(void) +{ + serialSetMode(blackboxPort, previousPortMode); + serialSetBaudRate(blackboxPort, previousBaudRate); + + endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + + /* + * Normally this would be handled by mw.c, but since we take an unknown amount + * of time to shut down asynchronously, we're the only ones that know when to call it. + */ + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } +} + +bool isBlackboxDeviceIdle(void) +{ + return isSerialTransmitBufferEmpty(blackboxPort); +} diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h new file mode 100644 index 000000000..5abacecfa --- /dev/null +++ b/src/main/blackbox/blackbox_io.h @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +uint8_t blackboxWriteChunkSize; + +void blackboxWrite(uint8_t value); + +void blackboxPrintf(char *fmt, ...); +int blackboxPrint(const char *s); + +void blackboxWriteUnsignedVB(uint32_t value); +void blackboxWriteSignedVB(int32_t value); +void blackboxWriteS16(int16_t value); +void blackboxWriteTag2_3S32(int32_t *values); +void blackboxWriteTag8_4S16(int32_t *values); +void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); + +void blackboxDeviceFlush(void); +bool blackboxDeviceOpen(void); +void blackboxDeviceClose(void); + +bool isBlackboxDeviceIdle(void);