diff --git a/Makefile b/Makefile index 1b4f18ba8..3ec9736e2 100644 --- a/Makefile +++ b/Makefile @@ -219,7 +219,8 @@ HIGHEND_SRC = flight/autotune.c \ telemetry/msp.c \ telemetry/smartport.c \ sensors/sonar.c \ - sensors/barometer.c + sensors/barometer.c \ + blackbox/blackbox.c NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/accgyro_adxl345.c \ diff --git a/docs/Blackbox.md b/docs/Blackbox.md new file mode 100644 index 000000000..71c11dafb --- /dev/null +++ b/docs/Blackbox.md @@ -0,0 +1,233 @@ +# Blackbox flight data recorder + +![Rendered flight log frame](http://i.imgur.com/FBphB8c.jpg) + +## Introduction +This feature transmits your flight data information on every control loop iteration over a serial port to an external +logging device to be recorded. + +After your flight, you can process the resulting logs on your computer to either turn them into CSV (comma-separated +values) or render your flight log as a video using the tools `blackbox_decode` and `blackbox_render` . Those tools can be +found in this repository: + +https://github.com/thenickdude/blackbox + +## Logged data +The blackbox records flight data on every iteration of the flight control loop. It records the current time in +microseconds, P, I and D corrections for each axis, your RC command stick positions (after applying expo curves), +gyroscope data, accelerometer data (after your configured low-pass filtering), and the command being sent to each motor +speed controller. This is all stored without any approximation or loss of precision, so even quite subtle problems +should be detectable from the fight data log. + +Currently, the blackbox attempts to log GPS data whenever new GPS data is available, but this has not been tested yet. +The CSV decoder and video renderer do not yet show any of the GPS data (though this will be added). If you have a working +GPS, please send in your logs so I can get the decoding implemented. + +The data rate for my quadcopter using a looptime of 2400 is about 10.25kB/s. This allows about 18 days of flight logs +to fit on a 16GB MicroSD card, which ought to be enough for anybody :). + +## Supported configurations +The maximum data rate for the flight log is fairly restricted, so anything that increases the load can cause the flight +log to drop frames and contain errors. + +The Blackbox was developed and tested on a quadcopter. It has also been tested on a tricopter. It should work on +hexacopters or octocopters, but as they transmit more information to the flight log (due to having more motors), the +number of dropped frames may increase. The `blackbox_render` tool only supports tri and quadcopters (please send me +flight logs from other craft, and I can add support for them!) + +Cleanflight's `looptime` setting will decide how many times per second an update is saved to the flight log. The +software was developed on a craft with a looptime of 2400. Any looptime smaller than this will put more strain on the +data rate. The default looptime on Cleanflight is 3500. If you're using a looptime of 2000 or smaller, you will probably +need to reduce the sampling rate in the Blackbox settings, see the later section on configuring the Blackbox feature for +details. + +The Blackbox feature is currently available on the Naze32 and Naze32Pro targets. It may work on other targets, but you +will need to enable those manually in `/src/main/target/xxx/target.h` by adding a `#define BLACKBOX` statement and +recompile Cleanflight. + +## Hardware +The blackbox software is designed to be used with an [OpenLog serial data logger][] and a microSDHC card. You need a +little prep to get the OpenLog ready for use, so here are the details: + +### Firmware +The OpenLog should be flashed with the [OpenLog Lite firmware][] or the special [OpenLog Blackbox variant][] using the Arduino IDE +in order to minimise dropped frames (target the "Arduino Uno"). The Blackbox variant of the firmware ensures that the +OpenLog is running at the correct baud-rate and does away for the need for a `CONFIG.TXT` file to set up the OpenLog. + +If you decide to use the OpenLog Lite firmware instead, note that the .hex file for OpenLog Lite currently in the +OpenLog repository is out of date with respect to their .ino source file, so you'll need to build it yourself after +adding the [required libraries][] to your Arduino libraries directory. + +To flash the firmware, you'll need to use an FTDI programmer like the [FTDI Basic Breakout][] along with some way of +switching the Tx and Rx pins over (since the OpenLog has them switched) like the [FTDI crossover][]. + +[OpenLog serial data logger]: https://www.sparkfun.com/products/9530 +[OpenLog Lite firmware]: https://github.com/sparkfun/OpenLog/tree/master/firmware/OpenLog_v3_Light +[OpenLog Blackbox variant]: https://github.com/thenickdude/blackbox/tree/blackbox/tools/blackbox/OpenLog_v3_Blackbox +[Required libraries]: https://code.google.com/p/beta-lib/downloads/detail?name=SerialLoggerBeta20120108.zip&can=4&q= +[FTDI Basic Breakout]: https://www.sparkfun.com/products/9716 +[FTDI crossover]: https://www.sparkfun.com/products/10660 + +### Serial port +Connect the "TX" pin of the serial port you've chosen to the OpenLog's "RXI" pin. Don't connect the serial port's RX +pin to the OpenLog. + +### microSDHC +Your choice of microSDHC card is very important to the performance of the system. The OpenLog relies on being able to +make many small writes to the card with minimal delay, which not every card is good at. A faster SD-card speed rating is +not a guarantee of better performance. + +#### microSDHC cards known to have poor performance + - Generic 4GB Class 4 microSDHC card - the rate of missing frames is about 1%, and is concentrated around the most + interesting parts of the log! + +#### microSDHC cards known to have good performance + - Transcend 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%) + - Sandisk Extreme 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%) + +You should format any card you use with the [SD Association's special formatting tool][] , as it will give the OpenLog +the best chance of writing at high speed. You must format it with either FAT, or with FAT32 (recommended). + +[SD Association's special formatting tool]: https://www.sdcard.org/downloads/formatter_4/ + +### OpenLog configuration +This section applies only if you are using the OpenLog or OpenLog Lite original firmware on the OpenLog. If you flashed +it with the special OpenLog Blackbox firmware, you don't need to configure it further. + +Power up the OpenLog with a microSD card inside, wait 10 seconds or so, then power it down and plug the microSD card +into your computer. You should find a "CONFIG.TXT" file on the card. Edit it in a text editor to set the first number +(baud) to 115200. Set esc# to 0, mode to 0, and echo to 0. Save the file and put the card back into your OpenLog, it +should use those settings from now on. + +If your OpenLog didn't write a CONFIG.TXT file, you can [use this one instead][]. + +[use this one instead]: https://raw.githubusercontent.com/thenickdude/blackbox/blackbox/tools/blackbox/OpenLog_v3_Blackbox/CONFIG.TXT + +### Protection +The OpenLog can be wrapped in black electrical tape in order to insulate it from conductive frames (like carbon fiber), +but this makes its status LEDs impossible to see. I recommend wrapping it with some clear heatshrink tubing instead. + +![OpenLog installed](http://i.imgur.com/jYyZ0oC.jpg "OpenLog installed with double-sided tape, SDCard slot pointing outward") + +## Enabling this feature +In the [Cleanflight Configurator][], open up the CLI tab. Enable the Blackbox feature by typing in `feature BLACKBOX` +and pressing enter. You also need to assign the Blackbox to one of [your serial ports][] . A 115200 baud port is +required (such as serial_port_1 on the Naze32, the two-pin Tx/Rx header in the center of the board). + +For example, use `set serial_port_1_scenario=11` to switch the main serial port to MSP, CLI, Blackbox and GPS Passthrough. + +Enter `save`. Your configuration should be saved and the flight controller will reboot. You're ready to go! + +[your serial ports]: https://github.com/cleanflight/cleanflight/blob/master/docs/Serial.md +[Cleanflight Configurator]: https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb?hl=en + +## Configuring this feature +The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control +the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which +decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs +every iteration. + +If you are using a short looptime like 2000 or faster, or you're using a slower MicroSD card, you will need to reduce +this rate to reduce the number of corrupted logged frames. A rate of 1/2 is likely to work for most craft. + +You can change these settings by entering the CLI tab in the Cleanflight Configurator and using the `set` command, like so: + +``` +set blackbox_rate_num = 1 +set blackbox_rate_denom = 2 +``` + +## Usage +The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. Each time the OpenLog is +power-cycled, it begins a fresh new log file. If you arm and disarm several times without cycling the power (recording +several flights), those logs will be combined together into one file. The command line tools will ask you to pick which +one of these flights you want to display/decode. + +If your craft has a buzzer attached, a short beep will be played when you arm. You can later use this beep to +synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line in the flight +data log, which you can sync against the beep in your recorded audio track). + +The OpenLog requires a couple of seconds of delay after powerup before it's ready to record, so don't arm your craft +immediately after connecting the battery (you'll probably be waiting for the flight controller to become ready during +that time anyway!) + +You should also wait a few seconds after disarming the quad to allow the OpenLog to finish saving its data. + +Don't insert or remove the SD card while the OpenLog is powered up. + +After your flights, you'll have a series of files labeled "LOG00001.TXT" etc. on the microSD card. You'll need to +decode these with the `blackbox_decode` tool to create a CSV (comma-separated values) file, or render it into a series of +PNG frames with `blackbox_render` which you could convert into a video using another software package. + +### Using the blackbox_decode tool +This tool converts a flight log binary file into CSV format. Typical usage (from the command line) would be like: + +```bash +blackbox_decode LOG00001.TXT +``` + +That'll decode the log to `LOG00001.01.csv` and print out some statistics about the log. If you're using Windows, you +can drag and drop your log files onto `blackbox_decode` and they'll all be decoded. + +Use the `--help` option to show more details: + +```text +Blackbox flight log decoder by Nicholas Sherlock + +Usage: + blackbox_decode [options] + +Options: + --help This page + --index Choose the log from the file that should be decoded (or omit to decode all) + --limits Print the limits and range of each field + --stdout Write log to stdout instead of to a file + --debug Show extra debugging information + --raw Don't apply predictions to fields (show raw field deltas) +``` + +### Using the blackbox_render tool +This tool converts a flight log binary file into a series of transparent PNG images that you could overlay onto your +flight video using a video editor (like [DaVinci Resolve][]). Typical usage (from the command line) would be like: + +```bash +blackbox_render LOG00001.TXT +``` + +This will create PNG files at 30 fps into a new directory called `LOG00001.01` next to the log file. + +Use the `--help` option to show more details: + +```text +Blackbox flight log renderer by Nicholas Sherlock + +Usage: + blackbox_render [options] + +Options: + --help This page + --index Choose which log from the file should be rendered + --width Choose the width of the image (default 1920) + --height Choose the height of the image (default 1080) + --fps FPS of the resulting video (default 30) + --prefix Set the prefix of the output frame filenames + --start Begin the log at this time offset (default 0:00) + --end End the log at this time offset + --[no-]draw-pid-table Show table with PIDs and gyros (default on) + --[no-]draw-craft Show craft drawing (default on) + --[no-]draw-sticks Show RC command sticks (default on) + --[no-]draw-time Show frame number and time in bottom right (default on) + --[no-]plot-motor Draw motors on the upper graph (default on) + --[no-]plot-pid Draw PIDs on the lower graph (default off) + --[no-]plot-gyro Draw gyroscopes on the lower graph (default on) + --smoothing-pid Smoothing window for the PIDs (default 4) + --smoothing-gyro Smoothing window for the gyroscopes (default 2) + --smoothing-motor Smoothing window for the motors (default 2) + --prop-style Style of propeller display (pie/blades, default pie) + --gapless Fill in gaps in the log with straight lines +``` + +(At least on Windows) if you just want to render a log file using the defaults, you can drag and drop a log onto the +blackbox_render program and it'll start generating the PNGs immediately. + +[DaVinci Resolve]: https://www.blackmagicdesign.com/products/davinciresolve \ No newline at end of file diff --git a/docs/Serial.md b/docs/Serial.md index 7031fe3c3..025821d0d 100644 --- a/docs/Serial.md +++ b/docs/Serial.md @@ -46,6 +46,8 @@ To make configuration easier, common usage scenarios are listed below. 7 GPS-PASSTHROUGH ONLY 8 MSP ONLY 9 SMARTPORT TELEMETRY ONLY +10 BLACKBOX ONLY +11 MSP, CLI, BLACKBOX, GPS-PASSTHROUGH ``` ### Constraints diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c new file mode 100644 index 000000000..cbce7b57d --- /dev/null +++ b/src/main/blackbox/blackbox.c @@ -0,0 +1,1262 @@ +/* + * 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 . + */ + +#include +#include +#include + +#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 "common/printf.h" + +#include "flight/flight.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" +#include "sensors/sonar.h" +#include "sensors/compass.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 "config/runtime_config.h" +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#include "blackbox_fielddefs.h" +#include "blackbox.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])) + +// Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter: +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +#define CONCAT_HELPER(x,y) x ## y +#define CONCAT(x,y) CONCAT_HELPER(x, y) + +#define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x) +#define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x) +#define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x) +#define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED +#define SIGNED FLIGHT_LOG_FIELD_SIGNED + +static const char blackboxHeader[] = + "H Product:Blackbox flight data recorder by Nicholas Sherlock\n" + "H Blackbox version:1\n" + "H Data version:2\n" + "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n"; + +static const char* const blackboxMainHeaderNames[] = { + "I name", + "I signed", + "I predictor", + "I encoding", + "P predictor", + "P encoding" +}; + +static const char* const blackboxGPSGHeaderNames[] = { + "G name", + "G signed", + "G predictor", + "G encoding" +}; + +static const char* const blackboxGPSHHeaderNames[] = { + "H name", + "H signed", + "H predictor", + "H encoding" +}; + +/* All field definition structs should look like this (but with longer arrs): */ +typedef struct blackboxFieldDefinition_t { + const char *name; + uint8_t arr[1]; +} blackboxFieldDefinition_t; + +typedef struct blackboxMainFieldDefinition_t { + const char *name; + uint8_t isSigned; + uint8_t Ipredict; + uint8_t Iencode; + uint8_t Ppredict; + uint8_t Pencode; + uint8_t condition; // Decide whether this field should appear in the log +} blackboxMainFieldDefinition_t; + +typedef struct blackboxGPSFieldDefinition_t { + const char *name; + uint8_t isSigned; + uint8_t predict; + uint8_t encode; +} blackboxGPSFieldDefinition_t; + +/** + * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is + * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause + * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches + * the encoding we've promised here). + */ +static const blackboxMainFieldDefinition_t blackboxMainFields[] = { + /* loopIteration doesn't appear in P frames since it always increments */ + {"loopIteration", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)}, + /* Time advances pretty steadily so the P-frame prediction is a straight line */ + {"time", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"axisP[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"axisP[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"axisP[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + /* I terms get special packed encoding in P frames: */ + {"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[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(NONZERO_PID_D_0)}, + {"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(NONZERO_PID_D_2)}, + /* 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[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)}, + /* 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)}, + + {"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT}, +#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 */ + {"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[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"accSmooth[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"accSmooth[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"accSmooth[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + /* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */ + {"motor[0]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)}, + /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */ + {"motor[1]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)}, + {"motor[2]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)}, + {"motor[3]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)}, + {"motor[4]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)}, + {"motor[5]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)}, + {"motor[6]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)}, + {"motor[7]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)}, + {"servo[5]", UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)} +}; + +// GPS position/vel frame +static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { + {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, + {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, + {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)} +}; + +// GPS home frame +static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = { + {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)} +}; + +typedef enum BlackboxState { + BLACKBOX_STATE_DISABLED = 0, + BLACKBOX_STATE_STOPPED, + BLACKBOX_STATE_SEND_HEADER, + BLACKBOX_STATE_SEND_FIELDINFO, + BLACKBOX_STATE_SEND_GPS_H_HEADERS, + BLACKBOX_STATE_SEND_GPS_G_HEADERS, + BLACKBOX_STATE_SEND_SYSINFO, + BLACKBOX_STATE_PRERUN, + BLACKBOX_STATE_RUNNING +} BlackboxState; + +typedef struct gpsState_t { + int32_t GPS_home[2], GPS_coord[2]; + uint8_t GPS_numSat; +} gpsState_t; + +//From mixer.c: +extern uint8_t motorCount; + +//From mw.c: +extern uint32_t currentTime; + +static const int SERIAL_CHUNK_SIZE = 16; + +static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; + +static uint32_t startTime; +static unsigned int headerXmitIndex; +static int fieldXmitIndex; + +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 + * to encode: + */ +static uint16_t vbatReference; +static gpsState_t gpsHistory; + +// Keep a history of length 2, plus a buffer for MW to store the new values into +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))); +} + +/** + * 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 testBlackboxCondition(FlightLogFieldCondition condition) +{ + switch (condition) { + case FLIGHT_LOG_FIELD_CONDITION_ALWAYS: + return true; + + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: + return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; + case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: + 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_VBAT: + return feature(FEATURE_VBAT); + + case FLIGHT_LOG_FIELD_CONDITION_NEVER: + return false; + default: + return false; + } +} + +static void blackboxSetState(BlackboxState newState) +{ + //Perform initial setup required for the new state + switch (newState) { + case BLACKBOX_STATE_SEND_HEADER: + startTime = millis(); + headerXmitIndex = 0; + break; + case BLACKBOX_STATE_SEND_FIELDINFO: + case BLACKBOX_STATE_SEND_GPS_G_HEADERS: + case BLACKBOX_STATE_SEND_GPS_H_HEADERS: + headerXmitIndex = 0; + fieldXmitIndex = -1; + break; + case BLACKBOX_STATE_SEND_SYSINFO: + headerXmitIndex = 0; + break; + case BLACKBOX_STATE_RUNNING: + blackboxIteration = 0; + blackboxPFrameIndex = 0; + blackboxIFrameIndex = 0; + break; + default: + ; + } + blackboxState = newState; +} + +static void writeIntraframe(void) +{ + blackboxValues_t *blackboxCurrent = blackboxHistory[0]; + int x; + + blackboxWrite('I'); + + writeUnsignedVB(blackboxIteration); + writeUnsignedVB(blackboxCurrent->time); + + for (x = 0; x < XYZ_AXIS_COUNT; x++) + writeSignedVB(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++) + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) + writeSignedVB(blackboxCurrent->axisPID_D[x]); + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->rcCommand[x]); + + writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { + /* + * 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]); + + for (x = 0; x < XYZ_AXIS_COUNT; x++) + writeSignedVB(blackboxCurrent->accSmooth[x]); + + //Motors can be below minthrottle when disarmed, but that doesn't happen much + writeUnsignedVB(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]); + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) + writeSignedVB(blackboxHistory[0]->servo[5] - 1500); + + //Rotate our history buffers: + + //The current state becomes the new "before" state + blackboxHistory[1] = blackboxHistory[0]; + //And since we have no other history, we also use it for the "before, before" state + blackboxHistory[2] = blackboxHistory[0]; + //And advance the current state over to a blank space ready to be filled + blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing; +} + +static void writeInterframe(void) +{ + int x; + int32_t deltas[5]; + + blackboxValues_t *blackboxCurrent = blackboxHistory[0]; + blackboxValues_t *blackboxLast = blackboxHistory[1]; + + blackboxWrite('P'); + + //No need to store iteration count since its delta is always 1 + + /* + * Since the difference between the difference between successive times will be nearly zero, use + * second-order differences. + */ + writeSignedVB((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++) + 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); + + /* + * 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]); + + /* + * 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++) + deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; + + writeTag8_4S16(deltas); + + //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO + int optionalFieldCount = 0; + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { + 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: + 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++) + writeSignedVB(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); + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) + writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + + //Rotate our history buffers + blackboxHistory[2] = blackboxHistory[1]; + blackboxHistory[1] = blackboxHistory[0]; + blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing; +} + +static int gcd(int num, int denom) +{ + if (denom == 0) + return num; + return gcd(denom, num % denom); +} + +static void validateBlackboxConfig() +{ + int div; + + if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0 + || masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) { + masterConfig.blackbox_rate_num = 1; + masterConfig.blackbox_rate_denom = 1; + } else { + div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + + masterConfig.blackbox_rate_num /= div; + masterConfig.blackbox_rate_denom /= div; + } +} + +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; + } + } +} + +static void releaseBlackboxPort(void) +{ + serialSetMode(blackboxPort, previousPortMode); + serialSetBaudRate(blackboxPort, previousBaudRate); + + endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); +} + +void startBlackbox(void) +{ + if (blackboxState == BLACKBOX_STATE_STOPPED) { + validateBlackboxConfig(); + + configureBlackboxPort(); + + if (!blackboxPort) { + blackboxSetState(BLACKBOX_STATE_DISABLED); + return; + } + + memset(&gpsHistory, 0, sizeof(gpsHistory)); + + blackboxHistory[0] = &blackboxHistoryRing[0]; + blackboxHistory[1] = &blackboxHistoryRing[1]; + 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 + + blackboxSetState(BLACKBOX_STATE_SEND_HEADER); + } +} + +void finishBlackbox(void) +{ + if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) { + blackboxSetState(BLACKBOX_STATE_STOPPED); + + releaseBlackboxPort(); + } +} + +static void writeGPSHomeFrame() +{ + blackboxWrite('H'); + + writeSignedVB(GPS_home[0]); + writeSignedVB(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]; + gpsHistory.GPS_home[1] = GPS_home[1]; +} + +static void writeGPSFrame() +{ + blackboxWrite('G'); + + 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); + + gpsHistory.GPS_numSat = GPS_numSat; + gpsHistory.GPS_coord[0] = GPS_coord[0]; + gpsHistory.GPS_coord[1] = GPS_coord[1]; +} + +/** + * Fill the current state of the blackbox using values read from the flight controller + */ +static void loadBlackboxState(void) +{ + blackboxValues_t *blackboxCurrent = blackboxHistory[0]; + int i; + + blackboxCurrent->time = currentTime; + + for (i = 0; i < XYZ_AXIS_COUNT; i++) + blackboxCurrent->axisPID_P[i] = axisPID_P[i]; + for (i = 0; i < XYZ_AXIS_COUNT; i++) + blackboxCurrent->axisPID_I[i] = axisPID_I[i]; + for (i = 0; i < XYZ_AXIS_COUNT; i++) + blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + + for (i = 0; i < 4; i++) + blackboxCurrent->rcCommand[i] = rcCommand[i]; + + for (i = 0; i < XYZ_AXIS_COUNT; i++) + blackboxCurrent->gyroData[i] = gyroData[i]; + + for (i = 0; i < XYZ_AXIS_COUNT; i++) + blackboxCurrent->accSmooth[i] = accSmooth[i]; + + for (i = 0; i < motorCount; 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 + blackboxCurrent->servo[5] = servo[5]; +} + +/** + * Transmit the header information for the given field definitions. Transmitted header lines look like: + * + * H Field I name:a,b,c + * H Field I predictor:0,1,2 + * + * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field + * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition. + * + * Set headerXmitIndex to 0 and fieldXmitIndex to -1 before calling for the first time. + * + * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the + * fieldDefinition and secondCondition arrays. + * + * Returns true if there is still header left to transmit (so call again to continue transmission). + */ +static bool sendFieldDefinition(const char * const *headerNames, unsigned int headerCount, const void *fieldDefinitions, + const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition) +{ + const blackboxFieldDefinition_t *def; + int charsWritten; + static bool needComma = false; + size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions; + size_t conditionsStride = (char*) secondCondition - (char*) conditions; + + /* + * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit + * the whole header. + */ + if (fieldXmitIndex == -1) { + if (headerXmitIndex >= headerCount) + return false; //Someone probably called us again after we had already completed transmission + + charsWritten = blackboxPrint("H Field "); + charsWritten += blackboxPrint(headerNames[headerXmitIndex]); + charsWritten += blackboxPrint(":"); + + fieldXmitIndex++; + needComma = false; + } else + charsWritten = 0; + + for (; fieldXmitIndex < fieldCount && charsWritten < SERIAL_CHUNK_SIZE; fieldXmitIndex++) { + def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * fieldXmitIndex); + + if (!conditions || testBlackboxCondition(conditions[conditionsStride * fieldXmitIndex])) { + if (needComma) { + blackboxWrite(','); + charsWritten++; + } else + needComma = true; + + // The first header is a field name + if (headerXmitIndex == 0) { + charsWritten += blackboxPrint(def->name); + } else { + //The other headers are integers + if (def->arr[headerXmitIndex - 1] >= 10) { + blackboxWrite(def->arr[headerXmitIndex - 1] / 10 + '0'); + blackboxWrite(def->arr[headerXmitIndex - 1] % 10 + '0'); + charsWritten += 2; + } else { + blackboxWrite(def->arr[headerXmitIndex - 1] + '0'); + charsWritten++; + } + } + } + } + + // Did we complete this line? + if (fieldXmitIndex == fieldCount) { + blackboxWrite('\n'); + headerXmitIndex++; + fieldXmitIndex = -1; + } + + return headerXmitIndex < headerCount; +} + +/** + * Transmit a portion of the system information headers. Begin with a xmitIndex of 0. Returns the next xmitIndex to + * call with, or -1 if transmission is complete. + */ +static int blackboxWriteSysinfo(int xmitIndex) +{ + union floatConvert_t { + float f; + uint32_t u; + } 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 P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + break; + case 7: + blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); + break; + case 8: + blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle); + break; + case 9: + blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle); + break; + case 10: + floatConvert.f = gyro.scale; + blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); + break; + case 11: + blackboxPrintf("H acc_1G:%u\n", acc_1G); + break; + case 12: + blackboxPrintf("H vbatscale:%u\n", masterConfig.batteryConfig.vbatscale); + break; + case 13: + blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig.batteryConfig.vbatmincellvoltage, + masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage); + break; + case 14: + //Pause + break; + case 15: + blackboxPrintf("H vbatref:%u\n", vbatReference); + break; + case 16: + // One more pause for good luck + break; + default: + return -1; + } + + return xmitIndex + 1; +} + +// Beep the buzzer and write the current time to the log as a synchronization point +static void blackboxPlaySyncBeep() +{ + uint32_t now = micros(); + + /* + * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later. + * Our beep is timing sensitive, so start beeping now without setting the beeperIsOn flag. + */ + BEEP_ON; + + // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive + queueConfirmationBeep(1); + + blackboxWrite('E'); + blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP); + + writeUnsignedVB(now); +} + +void handleBlackbox(void) +{ + int i, result; + + switch (blackboxState) { + case BLACKBOX_STATE_SEND_HEADER: + //On entry of this state, headerXmitIndex is 0 and startTime is intialised + + /* + * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit + * buffer. + */ + if (millis() > startTime + 100) { + for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++) + blackboxWrite(blackboxHeader[headerXmitIndex]); + + if (blackboxHeader[headerXmitIndex] == '\0') + blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO); + } + break; + case BLACKBOX_STATE_SEND_FIELDINFO: + //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1 + if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1, + ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { + if (feature(FEATURE_GPS)) + blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS); + else + blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); + } + break; + case BLACKBOX_STATE_SEND_GPS_H_HEADERS: + //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1 + if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1, + ARRAY_LENGTH(blackboxGpsHFields), NULL, 0)) { + blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS); + } + break; + case BLACKBOX_STATE_SEND_GPS_G_HEADERS: + //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1 + if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1, + ARRAY_LENGTH(blackboxGpsGFields), NULL, 0)) { + blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); + } + break; + case BLACKBOX_STATE_SEND_SYSINFO: + //On entry of this state, headerXmitIndex is 0 + result = blackboxWriteSysinfo(headerXmitIndex); + + if (result == -1) + blackboxSetState(BLACKBOX_STATE_PRERUN); + else + headerXmitIndex = result; + break; + case BLACKBOX_STATE_PRERUN: + blackboxPlaySyncBeep(); + + blackboxSetState(BLACKBOX_STATE_RUNNING); + break; + case BLACKBOX_STATE_RUNNING: + // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 + + // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames + if (blackboxPFrameIndex == 0) { + // Copy current system values into the blackbox + loadBlackboxState(); + writeIntraframe(); + } else { + if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) { + loadBlackboxState(); + writeInterframe(); + } + + if (feature(FEATURE_GPS)) { + /* + * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the + * GPS home position. + * + * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can + * still be interpreted correctly. + */ + if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1] + || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) { + + writeGPSHomeFrame(); + writeGPSFrame(); + } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] + || GPS_coord[1] != gpsHistory.GPS_coord[1]) { + //We could check for velocity changes as well but I doubt it changes independent of position + writeGPSFrame(); + } + } + } + + blackboxIteration++; + blackboxPFrameIndex++; + + if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) { + blackboxPFrameIndex = 0; + blackboxIFrameIndex++; + } + break; + default: + break; + } +} + +static bool canUseBlackboxWithCurrentConfiguration(void) +{ + if (!feature(FEATURE_BLACKBOX)) + return false; + + return true; +} + +void initBlackbox(void) +{ + if (canUseBlackboxWithCurrentConfiguration()) + blackboxSetState(BLACKBOX_STATE_STOPPED); + else + blackboxSetState(BLACKBOX_STATE_DISABLED); +} diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h new file mode 100644 index 000000000..36b006c0a --- /dev/null +++ b/src/main/blackbox/blackbox.h @@ -0,0 +1,47 @@ +/* + * 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 "common/axis.h" +#include + +typedef struct blackboxValues_t { + uint32_t time; + + int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT]; + + int16_t rcCommand[4]; + int16_t gyroData[XYZ_AXIS_COUNT]; + int16_t accSmooth[XYZ_AXIS_COUNT]; + int16_t motor[MAX_SUPPORTED_MOTORS]; + 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; + +void initBlackbox(void); +void handleBlackbox(void); +void startBlackbox(void); +void finishBlackbox(void); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h new file mode 100644 index 000000000..68a75439f --- /dev/null +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -0,0 +1,100 @@ +/* + * 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 + +typedef enum FlightLogFieldCondition { + FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8, + FLIGHT_LOG_FIELD_CONDITION_TRICOPTER, + + FLIGHT_LOG_FIELD_CONDITION_MAG = 20, + FLIGHT_LOG_FIELD_CONDITION_BARO, + FLIGHT_LOG_FIELD_CONDITION_VBAT, + + 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, +} FlightLogFieldCondition; + +typedef enum FlightLogFieldPredictor { + //No prediction: + FLIGHT_LOG_FIELD_PREDICTOR_0 = 0, + + //Predict that the field is the same as last frame: + FLIGHT_LOG_FIELD_PREDICTOR_PREVIOUS = 1, + + //Predict that the slope between this field and the previous item is the same as that between the past two history items: + FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE = 2, + + //Predict that this field is the same as the average of the last two history items: + FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 = 3, + + //Predict that this field is minthrottle + FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE = 4, + + //Predict that this field is the same as motor 0 + FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 = 5, + + //This field always increments + FLIGHT_LOG_FIELD_PREDICTOR_INC = 6, + + //Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set) + FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7, + + //Predict 1500 + FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8, + + //Predict vbatref, the reference ADC level stored in the header + FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9 + +} FlightLogFieldPredictor; + +typedef enum FlightLogFieldEncoding { + FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0, // Signed variable-byte + 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_TAG8_4S16 = 8, + FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero +} FlightLogFieldEncoding; + +typedef enum FlightLogFieldSign { + FLIGHT_LOG_FIELD_UNSIGNED = 0, + FLIGHT_LOG_FIELD_SIGNED = 1 +} FlightLogFieldSign; + +typedef enum FlightLogEvent { + FLIGHT_LOG_EVENT_SYNC_BEEP = 0, + FLIGHT_LOG_EVENT_LOG_END = 255 +} FlightLogEvent; diff --git a/src/main/config/config.c b/src/main/config/config.c index 938613caa..f4b0dca42 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -445,6 +445,11 @@ static void resetConf(void) applyDefaultLedStripConfig(masterConfig.ledConfigs); #endif +#ifdef BLACKBOX + masterConfig.blackbox_rate_num = 1; + masterConfig.blackbox_rate_denom = 1; +#endif + // alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target) #ifdef ALIENWII32 featureSet(FEATURE_RX_SERIAL); diff --git a/src/main/config/config.h b/src/main/config/config.h index bbbc9e794..351fc1e84 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -40,7 +40,8 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18 + FEATURE_ONESHOT125 = 1 << 18, + FEATURE_BLACKBOX = 1 << 19 } features_e; bool feature(uint32_t mask); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index f5575be0c..bacb7b5a7 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -85,6 +85,8 @@ typedef struct master_t { uint8_t current_profile_index; controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT]; + uint8_t blackbox_rate_num; + uint8_t blackbox_rate_denom; uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 22ee35fc9..c5942b05c 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -45,6 +45,11 @@ extern uint16_t cycleTime; int16_t heading, magHold; int16_t axisPID[3]; + +#ifdef BLACKBOX +int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; +#endif + uint8_t dynP8[3], dynI8[3], dynD8[3]; static int32_t errorGyroI[3] = { 0, 0, 0 }; @@ -247,6 +252,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa delta1[axis] = delta; DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif } } @@ -330,6 +341,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } } diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index a6fadf55f..158ac23a7 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -127,6 +127,8 @@ extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AX extern int32_t accSum[XYZ_AXIS_COUNT]; extern int16_t axisPID[XYZ_AXIS_COUNT]; +extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; + extern int16_t heading, magHold; extern int32_t AltHold; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 16037d165..324bdf12f 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -47,7 +47,7 @@ #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 -static uint8_t motorCount = 0; +uint8_t motorCount = 0; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index a05894189..185ecf51c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -62,7 +62,10 @@ const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUN SCENARIO_CLI_ONLY, SCENARIO_GPS_PASSTHROUGH_ONLY, SCENARIO_MSP_ONLY, - SCENARIO_SMARTPORT_TELEMETRY_ONLY + SCENARIO_SMARTPORT_TELEMETRY_ONLY, + + SCENARIO_BLACKBOX_ONLY, + SCENARIO_MSP_CLI_BLACKBOX_GPS_PASTHROUGH }; static serialConfig_t *serialConfig; @@ -136,7 +139,8 @@ const functionConstraint_t functionConstraints[] = { { FUNCTION_MSP, 9600, 115200, NO_AUTOBAUD, SPF_NONE }, { FUNCTION_SERIAL_RX, 9600, 115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK }, { FUNCTION_TELEMETRY, 9600, 19200, NO_AUTOBAUD, SPF_NONE }, - { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600, NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE } + { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600, NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE }, + { FUNCTION_BLACKBOX, 115200,115200, NO_AUTOBAUD, SPF_NONE } }; #define FUNCTION_CONSTRAINT_COUNT (sizeof(functionConstraints) / sizeof(functionConstraint_t)) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 854044421..db685a327 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -25,7 +25,8 @@ typedef enum { FUNCTION_SMARTPORT_TELEMETRY = (1 << 3), FUNCTION_SERIAL_RX = (1 << 4), FUNCTION_GPS = (1 << 5), - FUNCTION_GPS_PASSTHROUGH = (1 << 6) + FUNCTION_GPS_PASSTHROUGH = (1 << 6), + FUNCTION_BLACKBOX = (1 << 7) } serialPortFunction_e; typedef enum { @@ -52,10 +53,12 @@ typedef enum { SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_SMARTPORT_TELEMETRY | FUNCTION_GPS_PASSTHROUGH, SCENARIO_SERIAL_RX_ONLY = FUNCTION_SERIAL_RX, SCENARIO_TELEMETRY_ONLY = FUNCTION_TELEMETRY, - SCENARIO_SMARTPORT_TELEMETRY_ONLY = FUNCTION_SMARTPORT_TELEMETRY + SCENARIO_SMARTPORT_TELEMETRY_ONLY = FUNCTION_SMARTPORT_TELEMETRY, + SCENARIO_BLACKBOX_ONLY = FUNCTION_BLACKBOX, + SCENARIO_MSP_CLI_BLACKBOX_GPS_PASTHROUGH = FUNCTION_CLI | FUNCTION_MSP | FUNCTION_BLACKBOX | FUNCTION_GPS_PASSTHROUGH } serialPortFunctionScenario_e; -#define SERIAL_PORT_SCENARIO_COUNT 10 +#define SERIAL_PORT_SCENARIO_COUNT 12 #define SERIAL_PORT_SCENARIO_MAX (SERIAL_PORT_SCENARIO_COUNT - 1) extern const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUNT]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2628ba93a..428496c15 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -135,7 +135,8 @@ static const char * const featureNames[] = { "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", NULL + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", + "BLACKBOX", NULL }; // sync this with sensors_e @@ -404,6 +405,9 @@ const clivalue_t valueTable[] = { { "p_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDVEL], 0, 200 }, { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, + + { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, + { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) diff --git a/src/main/main.c b/src/main/main.c index 9b06fd39f..a34a71520 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -66,6 +66,7 @@ #include "sensors/acceleration.h" #include "sensors/gyro.h" #include "telemetry/telemetry.h" +#include "blackbox/blackbox.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" #include "config/runtime_config.h" @@ -344,6 +345,10 @@ void init(void) initTelemetry(); #endif +#ifdef BLACKBOX + initBlackbox(); +#endif + previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { diff --git a/src/main/mw.c b/src/main/mw.c index 54defd191..a8e6b6d72 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -67,6 +67,7 @@ #include "io/statusindicator.h" #include "rx/msp.h" #include "telemetry/telemetry.h" +#include "blackbox/blackbox.h" #include "config/runtime_config.h" #include "config/config.h" @@ -306,6 +307,15 @@ void mwDisarm(void) } } #endif + +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + finishBlackbox(); + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } + } +#endif } } @@ -327,6 +337,16 @@ void mwArm(void) } } #endif + +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); + if (sharedBlackboxAndMspPort) { + mspReleasePortIfAllocated(sharedBlackboxAndMspPort); + } + startBlackbox(); + } +#endif disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero return; @@ -698,6 +718,12 @@ void loop(void) mixTable(); writeServos(); writeMotors(); + +#ifdef BLACKBOX + if (!cliMode && feature(FEATURE_BLACKBOX)) { + handleBlackbox(); + } +#endif } #ifdef TELEMETRY diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 186c6804e..9766d4013 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -31,6 +31,7 @@ uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; uint8_t vbat = 0; // battery voltage in 0.1V steps +uint16_t vbatLatest = 0; // most recent unsmoothed raw reading from vbat adc int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -54,7 +55,7 @@ void updateBatteryVoltage(void) uint16_t vbatSampleTotal = 0; // store the battery voltage with some other recent battery voltage readings - vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = adcGetChannel(ADC_BATTERY); + vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatest = adcGetChannel(ADC_BATTERY); // calculate vbat based on the average of recent readings for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 6288f9f10..f90395f5f 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -42,6 +42,7 @@ typedef enum { } batteryState_e; extern uint8_t vbat; +extern uint16_t vbatLatest; extern uint8_t batteryCellCount; extern uint16_t batteryWarningVoltage; extern int32_t amperage; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 453cb18e9..f5c4c1c3d 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -124,6 +124,7 @@ #define LED_STRIP #define LED_STRIP_TIMER TIM3 +#define BLACKBOX #define TELEMETRY #define SERIAL_RX #define AUTOTUNE diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 5376ef9aa..19f806d71 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -43,6 +43,7 @@ #define SENSORS_SET (SENSOR_ACC) #define GPS +#define BLACKBOX #define TELEMETRY #define SERIAL_RX #define AUTOTUNE