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

Add Blackbox 0.1.0

This commit is contained in:
Nicholas Sherlock 2014-12-08 22:06:57 +13:00
parent 5794105dff
commit 8c41772584
20 changed files with 6797 additions and 14604 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@ -212,7 +212,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 \

86
README.md
View File

@ -1,66 +1,54 @@
# Cleanflight
# Blackbox flight data recorder port for Cleanflight
Clean-code version of baseflight flight-controller - flight controllers are used to fly multi-rotor craft and fixed wing craft.
![Rendered flight log frame](http://i.imgur.com/FBphB8c.jpg)
This fork differs from baseflight in that it attempts to use modern software development practices which result in:
WARNING - This firmware is experimental, and may cause your craft to suddenly fall out of the sky. This port to
Cleanflight has only had 3 test flights! No warranty is offered: if your craft breaks, you get to keep both pieces.
1. greater reliability through code robustness and automated testing.
2. easier maintainance through code cleanliness.
3. easier to develop new features.
4. easier to re-use code though code de-coupling and modularisation.
## Introduction
This is a modified version of Cleanflight which adds a flight data recorder function ("Blackbox"). Flight data
information is transmitted over the flight controller's serial port on every control loop iteration to an external
logging device to be recorded.
The MultiWii software, from which baseflight originated, violates many good software development best-practices. Hopefully this fork will go some way to address them. If you see any bad code in this fork please immediately raise an issue so it can be fixed, or better yet submit a pull request.
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.
## Additional Features
This is a port of my Blackbox feature for Baseflight, so please follow the instructions there for usage details (just
use the Cleanflight firmware from this repository instead of the Baseflight firmware):
Cleanflight also has additional features not found in baseflight. Since the primary maintainer of baseflight also sells hardware there is no incentive for baseflight to support the target platforms and features that Cleanflight now provides.
https://github.com/thenickdude/blackbox
For a list of features, changes and some discussion please review the thread on MultiWii forums and consult the documenation.
Instructions which are specific to Cleanflight are included here.
http://www.multiwii.com/forum/viewtopic.php?f=23&t=5149
## Installation of firmware
Before installing the new firmware onto your Naze32, back up your configuration: Connect to your flight controller
using the [Cleanflight Configurator][] , open up the CLI tab and enter "dump" into the box at the bottom and press enter.
Copy all of the text that results and paste it into a text document somewhere for safe-keeping.
Click the disconnect button, then on the main page choose the Firmware Flasher option. Tick the box for "Full Chip
Erase" (warning, this will erase all your settings!). Click the "Load firmware (local)" button, and select the file `cleanflight_NAZE.hex`
from the `obj/` directory. Click the "Flash Firmware" button and wait for it to complete.
## Documentation
Now you need to reload your configuration: Go to the CLI tab and paste in the dump that you saved earlier and press
enter, it should execute and restore your settings.
There is some documentation here: https://github.com/hydra/cleanflight/tree/master/docs
Before you leave 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][]. Because it requires a 115200 baud port, the best choice on the
Naze32 to use is serial_port_1, which is the two-pin Tx/Rx header in the centre of the board.
If what you need is not covered then refer to the baseflight documentation. If you still can't find what you need then visit the #cleanflight on the Freenode IRC network
Use `set serial_port_1_scenario=10` to switch the port to Blackbox-only, or `set serial_port_1_scenario=11` to switch it
to MSP, CLI, Blackbox and GPS Passthrough (probably the most useful configuration, since this is the port connected to
USB and you'll still want to access the CLI over it).
## IRC Support and Developers Channel
Enter `save`. Your configuration should be saved and the flight controller will reboot. You're ready to go!
There's a dedicated IRC channel here:
If you ever need to disable the Blackbox (say, for example, to switch to using the serial port for an OSD instead), you
can either reflash the stock firmware using the Configurator, or you can just turn off the Blackbox feature
by entering `feature -BLACKBOX` on the CLI tab.
irc://irc.freenode.net/#cleanflight
[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
If you are using windows and don't have an IRC client installed then take a look at HydraIRC - here: http://hydrairc.com/
## License
## Videos
There is a dedicated Cleanflight youtube channel which has progress update videos, flight demonstrations, instrutions and other related videos.
https://www.youtube.com/playlist?list=PL6H1fAj_XUNVBEcp8vbMH2DrllZAGWkt8
Please subscribe and '+1' the videos if you find them useful.
## Configuration Tool
To configure Cleanflight you should use the Cleanlight-configurator GUI tool (Windows/OSX/Linux) that can be found here:
https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb
The source for it is here:
https://github.com/hydra/cleanflight-configurator
## Contributing
Before making any contributions, take a note of the https://github.com/multiwii/baseflight/wiki/CodingStyle
For this fork it is also advised to read about clean code, here are some useful links:
* http://cleancoders.com/
* http://en.wikipedia.org/wiki/SOLID_%28object-oriented_design%29
* http://en.wikipedia.org/wiki/Code_smell
* http://en.wikipedia.org/wiki/Code_refactoring
* http://www.amazon.co.uk/Working-Effectively-Legacy-Robert-Martin/dp/0131177052
This project is licensed under GPLv3.

BIN
obj/cleanflight_CC3D.bin
View File

Binary file not shown.

5002
obj/cleanflight_CC3D.hex
View File

File diff suppressed because it is too large Load Diff

3927
obj/cleanflight_CJMCU.hex
View File

File diff suppressed because it is too large Load Diff

11460
obj/cleanflight_NAZE.hex
View File

File diff suppressed because it is too large Load Diff

756
src/main/blackbox/blackbox.c Normal file
View File

@ -0,0 +1,756 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <string.h>
#include <stdarg.h>
#include "platform.h"
#include "version.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/color.h"
#include "drivers/accgyro.h"
#include "drivers/light_led.h"
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.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
static const char blackboxHeader[] =
"H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
"H Blackbox version:1\n"
"H Data version:1\n";
// Some macros to make writing FLIGHT_LOG_FIELD_PREDICTOR_* 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) STR(CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x))
#define ENCODING(x) STR(CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x))
/* These headers have info for all 8 motors on them, we'll trim the final fields off to match the number of motors in the mixer: */
static const char * const blackboxHeaderFields[] = {
"H Field I name:"
"loopIteration,time,"
"axisP[0],axisP[1],axisP[2],"
"axisI[0],axisI[1],axisI[2],"
"axisD[0],axisD[1],axisD[2],"
"rcCommand[0],rcCommand[1],rcCommand[2],rcCommand[3],"
"gyroData[0],gyroData[1],gyroData[2],"
"accSmooth[0],accSmooth[1],accSmooth[2],"
"motor[0],motor[1],motor[2],motor[3],"
"motor[4],motor[5],motor[6],motor[7]",
"H Field I signed:"
/* loopIteration, time: */
"0,0,"
/* PIDs: */
"1,1,1,1,1,1,1,1,1,"
/* rcCommand[0..2] */
"1,1,1,"
/* rcCommand[3] (Throttle): */
"0,"
/* gyroData[0..2]: */
"1,1,1,"
/* accSmooth[0..2]: */
"1,1,1,"
/* Motor[0..7]: */
"0,0,0,0,0,0,0,0",
"H Field I predictor:"
/* loopIteration, time: */
PREDICT(0) "," PREDICT(0) ","
/* PIDs: */
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
/* rcCommand[0..2] */
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
/* rcCommand[3] (Throttle): */
PREDICT(MINTHROTTLE) ","
/* gyroData[0..2]: */
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
/* accSmooth[0..2]: */
PREDICT(0) "," PREDICT(0) "," PREDICT(0) ","
/* Motor[0]: */
PREDICT(MINTHROTTLE) ","
/* Motor[1..7]: */
PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) ","
PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) ","
PREDICT(MOTOR_0),
"H Field I encoding:"
/* loopIteration, time: */
ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) ","
/* PIDs: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* rcCommand[0..2] */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* rcCommand[3] (Throttle): */
ENCODING(UNSIGNED_VB) ","
/* gyroData[0..2]: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* accSmooth[0..2]: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* Motor[0]: */
ENCODING(UNSIGNED_VB) ","
/* Motor[1..7]: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB),
//Motors and gyros predict an average of the last two measurements (to reduce the impact of noise):
"H Field P predictor:"
/* loopIteration, time: */
PREDICT(INC) "," PREDICT(STRAIGHT_LINE) ","
/* PIDs: */
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
/* rcCommand[0..2] */
PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) ","
/* rcCommand[3] (Throttle): */
PREDICT(PREVIOUS) ","
/* gyroData[0..2]: */
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
/* accSmooth[0..2]: */
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
/* Motor[0]: */
PREDICT(AVERAGE_2) ","
/* Motor[1..7]: */
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) ","
PREDICT(AVERAGE_2),
/* RC fields are encoded together as a group, everything else is signed since they're diffs: */
"H Field P encoding:"
/* loopIteration, time: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* PIDs: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* rcCommand[0..3] */
ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) ","
ENCODING(TAG8_4S16) ","
/* gyroData[0..2]: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* accSmooth[0..2]: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
/* Motor[0]: */
ENCODING(SIGNED_VB) ","
/* Motor[1..7]: */
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(SIGNED_VB)
};
static const char blackboxGpsHeader[] =
"H Field G name:"
"GPS_numSat,GPS_coord[0],GPS_coord[1],GPS_altitude,GPS_speed\n"
"H Field G signed:"
"0,1,1,0,0\n"
"H Field G predictor:"
PREDICT(0) "," PREDICT(HOME_COORD) "," PREDICT(HOME_COORD) "," PREDICT(0) "," PREDICT(0) "\n"
"H Field G encoding:"
ENCODING(UNSIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) ","
ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) "\n"
"H Field H name:"
"GPS_home[0],GPS_home[1]\n"
"H Field H signed:"
"1,1\n"
"H Field H predictor:"
PREDICT(0) "," PREDICT(0) "\n"
"H Field H encoding:"
ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "\n";
typedef enum BlackboxState {
BLACKBOX_STATE_DISABLED = 0,
BLACKBOX_STATE_STOPPED,
BLACKBOX_STATE_SEND_HEADER,
BLACKBOX_STATE_SEND_FIELDINFO,
BLACKBOX_STATE_SEND_GPS_HEADERS,
BLACKBOX_STATE_SEND_SYSINFO,
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 BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t startTime;
static unsigned int headerXmitIndex;
static uint32_t blackboxIteration;
static serialPort_t *blackboxPort;
static portMode_t previousPortMode;
static uint32_t previousBaudRate;
static gpsState_t gpsHistory;
// Keep a history of length 2, plus a buffer for MW to store the new values into
static blackbox_values_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 blackbox_values_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);
}
/**
* 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)));
}
#define FIELD_ZERO 0
#define FIELD_4BIT 1
#define FIELD_8BIT 2
#define FIELD_16BIT 3
/**
* 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) {
uint8_t selector;
int x;
/*
* 4-bit fields can only be combined with their paired neighbor (there are two pairs), so choose a
* larger encoding if that's not possible.
*/
const uint8_t rcSelectorCleanup[16] = {
// Output selectors <- Input selectors
FIELD_ZERO << 2 | FIELD_ZERO, // zero, zero
FIELD_ZERO << 2 | FIELD_8BIT, // zero, 4-bit
FIELD_ZERO << 2 | FIELD_8BIT, // zero, 8-bit
FIELD_ZERO << 2 | FIELD_16BIT, // zero, 16-bit
FIELD_8BIT << 2 | FIELD_ZERO, // 4-bit, zero
FIELD_4BIT << 2 | FIELD_4BIT, // 4-bit, 4-bit
FIELD_8BIT << 2 | FIELD_8BIT, // 4-bit, 8-bit
FIELD_8BIT << 2 | FIELD_16BIT, // 4-bit, 16-bit
FIELD_8BIT << 2 | FIELD_ZERO, // 8-bit, zero
FIELD_8BIT << 2 | FIELD_8BIT, // 8-bit, 4-bit
FIELD_8BIT << 2 | FIELD_8BIT, // 8-bit, 8-bit
FIELD_8BIT << 2 | FIELD_16BIT, // 8-bit, 16-bit
FIELD_16BIT << 2 | FIELD_ZERO, // 16-bit, zero
FIELD_16BIT << 2 | FIELD_8BIT, // 16-bit, 4-bit
FIELD_16BIT << 2 | FIELD_8BIT, // 16-bit, 8-bit
FIELD_16BIT << 2 | FIELD_16BIT, // 16-bit, 16-bit
};
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] <= 7 && values[x] >= -8)
selector |= FIELD_4BIT;
else if (values[x] <= 127 && values[x] >= -128)
selector |= FIELD_8BIT;
else
selector |= FIELD_16BIT;
}
selector = rcSelectorCleanup[selector & 0x0F] | (rcSelectorCleanup[selector >> 4] << 4);
blackboxWrite(selector);
for (x = 0; x < 4; x++, selector >>= 2) {
switch (selector & 0x03) {
case FIELD_4BIT:
blackboxWrite((values[x] & 0x0F) | (values[x + 1] << 4));
//We write two selector fields:
x++;
selector >>= 2;
break;
case FIELD_8BIT:
blackboxWrite(values[x]);
break;
case FIELD_16BIT:
blackboxWrite(values[x]);
blackboxWrite(values[x] >> 8);
break;
}
}
}
static void writeIntraframe(void)
{
blackbox_values_t *blackboxCurrent = blackboxHistory[0];
int x;
blackboxWrite('I');
writeUnsignedVB(blackboxIteration);
writeUnsignedVB(blackboxCurrent->time);
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->axisP[x]);
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->axisI[x]);
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->axisD[x]);
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->rcCommand[x]);
writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle]
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->gyroData[x]);
for (x = 0; x < 3; 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]);
//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 rcDeltas[4];
blackbox_values_t *blackboxCurrent = blackboxHistory[0];
blackbox_values_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 < 3; x++)
writeSignedVB(blackboxCurrent->axisP[x] - blackboxLast->axisP[x]);
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->axisI[x] - blackboxLast->axisI[x]);
for (x = 0; x < 3; x++)
writeSignedVB(blackboxCurrent->axisD[x] - blackboxLast->axisD[x]);
for (x = 0; x < 4; x++)
rcDeltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
/*
* RC tends to stay the same or very small for many frames at a time, so use an encoding that
* can pack multiple values per byte:
*/
writeTag8_4S16(rcDeltas);
//Since gyros, accs and motors are noisy, base the prediction on the average of the history:
for (x = 0; x < 3; x++)
writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2);
for (x = 0; x < 3; 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);
//Rotate our history buffers
blackboxHistory[2] = blackboxHistory[1];
blackboxHistory[1] = blackboxHistory[0];
blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
}
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) {
configureBlackboxPort();
if (!blackboxPort) {
blackboxState = BLACKBOX_STATE_DISABLED;
return;
}
startTime = millis();
headerXmitIndex = 0;
blackboxIteration = 0;
blackboxState = BLACKBOX_STATE_SEND_HEADER;
memset(&gpsHistory, 0, sizeof(gpsHistory));
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
blackboxHistory[0] = &blackboxHistoryRing[0];
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
}
}
void finishBlackbox(void)
{
if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) {
blackboxState = 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];
}
void handleBlackbox(void)
{
int i;
blackbox_values_t *blackboxCurrent;
const int SERIAL_CHUNK_SIZE = 16;
static int charXmitIndex = 0;
int motorsToRemove, endIndex;
union floatConvert_t {
float f;
uint32_t u;
} floatConvert;
switch (blackboxState) {
case BLACKBOX_STATE_SEND_HEADER:
/*
* 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') {
blackboxState = BLACKBOX_STATE_SEND_FIELDINFO;
headerXmitIndex = 0;
charXmitIndex = 0;
}
}
break;
case BLACKBOX_STATE_SEND_FIELDINFO:
/*
* Once again, chunking up the data so we don't exceed our datarate. This time we're removing the excess field defs
* for motors we don't have.
*/
motorsToRemove = 8 - motorCount;
if (headerXmitIndex < sizeof(blackboxHeaderFields) / sizeof(blackboxHeaderFields[0])){
endIndex = strlen(blackboxHeaderFields[headerXmitIndex]) - (headerXmitIndex == 0 ? strlen(",motor[x]") : strlen(",x")) * motorsToRemove;
for (i = charXmitIndex; i < charXmitIndex + SERIAL_CHUNK_SIZE && i < endIndex; i++)
blackboxWrite(blackboxHeaderFields[headerXmitIndex][i]);
if (i == endIndex) {
blackboxWrite('\n');
headerXmitIndex++;
charXmitIndex = 0;
} else {
charXmitIndex = i;
}
} else {
if (feature(FEATURE_GPS)) {
blackboxState = BLACKBOX_STATE_SEND_GPS_HEADERS;
} else {
blackboxState = BLACKBOX_STATE_SEND_SYSINFO;
}
headerXmitIndex = 0;
}
break;
case BLACKBOX_STATE_SEND_GPS_HEADERS:
for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxGpsHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++)
blackboxWrite(blackboxGpsHeader[headerXmitIndex]);
if (blackboxGpsHeader[headerXmitIndex] == '\0') {
blackboxState = BLACKBOX_STATE_SEND_SYSINFO;
headerXmitIndex = 0;
}
break;
case BLACKBOX_STATE_SEND_SYSINFO:
switch (headerXmitIndex) {
case 0:
blackboxPrintf("H Firmware type:Cleanflight\n");
break;
case 1:
blackboxPrintf("H Firmware revision:%s\n", shortGitRevision);
break;
case 2:
blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime);
break;
case 3:
blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
break;
case 4:
blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle);
break;
case 5:
blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle);
break;
case 6:
floatConvert.f = gyro.scale;
blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
break;
case 7:
blackboxPrintf("H acc_1G:%u\n", acc_1G);
break;
default:
blackboxState = BLACKBOX_STATE_RUNNING;
}
headerXmitIndex++;
break;
case BLACKBOX_STATE_RUNNING:
// Copy current system values into the blackbox to be written out
blackboxCurrent = blackboxHistory[0];
blackboxCurrent->time = currentTime;
for (i = 0; i < motorCount; i++)
blackboxCurrent->motor[i] = motor[i];
for (i = 0; i < 3; i++)
blackboxCurrent->axisP[i] = axisP[i];
for (i = 0; i < 3; i++)
blackboxCurrent->axisI[i] = axisI[i];
for (i = 0; i < 3; i++)
blackboxCurrent->axisD[i] = axisD[i];
for (i = 0; i < 4; i++)
blackboxCurrent->rcCommand[i] = rcCommand[i];
for (i = 0; i < 3; i++)
blackboxCurrent->gyroData[i] = gyroData[i];
for (i = 0; i < 3; i++)
blackboxCurrent->accSmooth[i] = accSmooth[i];
// Write a keyframe every 32 frames so we can resynchronise upon missing frames
int blackboxIntercycleIndex = blackboxIteration % 32;
int blackboxIntracycleIndex = blackboxIteration / 32;
if (blackboxIntercycleIndex == 0)
writeIntraframe();
else {
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]
|| (blackboxIntercycleIndex == 15 && blackboxIntracycleIndex % 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++;
break;
default:
break;
}
}
bool canUseBlackboxWithCurrentConfiguration(void)
{
if (!feature(FEATURE_BLACKBOX))
return false;
return true;
}
void initBlackbox(void)
{
if (canUseBlackboxWithCurrentConfiguration())
blackboxState = BLACKBOX_STATE_STOPPED;
else
blackboxState = BLACKBOX_STATE_DISABLED;
}

36
src/main/blackbox/blackbox.h Normal file
View File

@ -0,0 +1,36 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
typedef struct blackbox_values_t {
uint32_t time;
int32_t axisP[3], axisI[3], axisD[3];
int16_t rcCommand[4];
int16_t gyroData[3];
int16_t accSmooth[3];
int16_t motor[8];
} blackbox_values_t;
void initBlackbox(void);
void handleBlackbox(void);
void startBlackbox(void);
void finishBlackbox(void);

54
src/main/blackbox/blackbox_fielddefs.h Normal file
View File

@ -0,0 +1,54 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
//No prediction:
#define FLIGHT_LOG_FIELD_PREDICTOR_0 0
//Predict that the field is the same as last frame:
#define 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:
#define FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE 2
//Predict that this field is the same as the average of the last two history items:
#define FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 3
//Predict that this field is minthrottle
#define FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE 4
//Predict that this field is the same as motor 0
#define FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 5
//This field always increments
#define 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)
#define FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD 7
#define FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB 0
#define FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB 1
#define FLIGHT_LOG_FIELD_ENCODING_U8 2
#define FLIGHT_LOG_FIELD_ENCODING_U16 3
#define FLIGHT_LOG_FIELD_ENCODING_U32 4
#define FLIGHT_LOG_FIELD_ENCODING_S8 5
#define FLIGHT_LOG_FIELD_ENCODING_S16 6
#define FLIGHT_LOG_FIELD_ENCODING_S32 7
#define FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 8
#define FLIGHT_LOG_FIELD_ENCODING_NULL 9

3
src/main/config/config.h
View File

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

17
src/main/flight/flight.c
View File

@ -42,6 +42,11 @@ extern uint16_t cycleTime;
int16_t heading, magHold;
int16_t axisPID[3];
#ifdef BLACKBOX
int32_t axisP[3], axisI[3], axisD[3];
#endif
uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
@ -243,6 +248,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
axisP[axis] = PTerm;
axisI[axis] = ITerm;
axisD[axis] = -DTerm;
#endif
}
}
@ -326,6 +337,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
#ifdef BLACKBOX
axisP[axis] = PTerm;
axisI[axis] = ITerm;
axisD[axis] = DTerm;
#endif
}
}

2
src/main/flight/flight.h
View File

@ -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 axisP[3], axisI[3], axisD[3];
extern int16_t heading, magHold;
extern int32_t AltHold;

2
src/main/flight/mixer.c
View File

@ -42,7 +42,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 };

8
src/main/io/serial.c
View File

@ -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;
@ -132,7 +135,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))

9
src/main/io/serial.h
View File

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

3
src/main/io/serial_cli.c
View File

@ -123,7 +123,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

5
src/main/main.c
View File

@ -64,6 +64,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"
@ -323,6 +324,10 @@ void init(void)
initTelemetry();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {

26
src/main/mw.c
View File

@ -65,6 +65,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"
@ -303,6 +304,15 @@ void mwDisarm(void)
}
}
#endif
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
finishBlackbox();
if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) {
mspAllocateSerialPorts(&masterConfig.serialConfig);
}
}
#endif
}
}
@ -324,6 +334,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
return;
}
}
@ -681,6 +701,12 @@ void loop(void)
mixTable();
writeServos();
writeMotors();
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox();
}
#endif
}
#ifdef TELEMETRY

1
src/main/target/NAZE/target.h
View File

@ -109,6 +109,7 @@
#define LED_STRIP
#define LED_STRIP_TIMER TIM3
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE

1
src/main/target/NAZE32PRO/target.h
View File

@ -43,6 +43,7 @@
#define SENSORS_SET (SENSOR_ACC)
#define GPS
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE