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

Better handling of I-frame and P-frame interations

Browse source
This commit is contained in:
Martin Budden 2017-04-30 22:42:33 +01:00
parent fb0429597f
commit fa1514c083
7 changed files with 395 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -28,6 +28,7 @@
#include "blackbox_encoding.h"
#include "blackbox_io.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "build/version.h"
@ -81,16 +82,13 @@
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.p_denom = 32,
.device = DEFAULT_BLACKBOX_DEVICE,
.rate_num = 1,
.rate_denom = 1,
.on_motor_test = 0, // default off
.record_acc = 1
);
#define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define SLOW_FRAME_INTERVAL 4096
#define STATIC_ASSERT(condition, name ) \
typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
@ -106,7 +104,7 @@ PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
static const char blackboxHeader[] =
"H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
"H Data version:2\n"
"H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
"H I interval:";
static const char* const blackboxFieldHeaderNames[] = {
"name",
@ -353,9 +351,16 @@ static uint32_t blackboxConditionCache;
STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
static uint32_t blackboxIteration;
static uint16_t blackboxLoopIndex;
static uint16_t blackboxPFrameIndex;
static uint16_t blackboxIFrameIndex;
static uint16_t blackboxSlowFrameIterationTimer;
// number of flight loop iterations before logging I-frame
// typically 32 for 1kHz loop, 64 for 2kHz loop etc
STATIC_UNIT_TESTED int16_t blackboxIInterval = 0;
// number of flight loop iterations before logging P-frame
STATIC_UNIT_TESTED int16_t blackboxPInterval = 0;
STATIC_UNIT_TESTED int32_t blackboxSInterval = 0;
STATIC_UNIT_TESTED int32_t blackboxSlowFrameIterationTimer;
static bool blackboxLoggedAnyFrames;
/*
@ -386,7 +391,7 @@ bool blackboxMayEditConfig(void)
static bool blackboxIsOnlyLoggingIntraframes(void)
{
return blackboxConfig()->rate_num == 1 && blackboxConfig()->rate_denom == 32;
return blackboxConfig()->p_denom == 0;
}
static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
@ -444,7 +449,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
return blackboxConfig()->rate_num < blackboxConfig()->rate_denom;
return blackboxConfig()->p_denom != 1;
case FLIGHT_LOG_FIELD_CONDITION_ACC:
return sensors(SENSOR_ACC) && blackboxConfig()->record_acc;
@ -498,7 +503,7 @@ static void blackboxSetState(BlackboxState newState)
xmitState.headerIndex = 0;
break;
case BLACKBOX_STATE_RUNNING:
blackboxSlowFrameIterationTimer = SLOW_FRAME_INTERVAL; //Force a slow frame to be written on the first iteration
blackboxSlowFrameIterationTimer = blackboxSInterval; //Force a slow frame to be written on the first iteration
break;
case BLACKBOX_STATE_SHUTTING_DOWN:
xmitState.u.startTime = millis();
@ -765,13 +770,13 @@ static void loadSlowState(blackboxSlowState_t *slow)
/**
* If the data in the slow frame has changed, log a slow frame.
*
* If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
* If allowPeriodicWrite is true, the frame is also logged if it has been more than blackboxSInterval logging iterations
* since the field was last logged.
*/
static bool writeSlowFrameIfNeeded(void)
STATIC_UNIT_TESTED bool writeSlowFrameIfNeeded(void)
{
// Write the slow frame peridocially so it can be recovered if we ever lose sync
bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
bool shouldWrite = blackboxSlowFrameIterationTimer >= blackboxSInterval;
if (shouldWrite) {
loadSlowState(&slowHistory);
@ -796,20 +801,6 @@ static bool writeSlowFrameIfNeeded(void)
void blackboxValidateConfig(void)
{
if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
|| blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
blackboxConfigMutable()->rate_num = 1;
blackboxConfigMutable()->rate_denom = 1;
} else {
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
* itself more frequently)
*/
const int div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
blackboxConfigMutable()->rate_num /= div;
blackboxConfigMutable()->rate_denom /= div;
}
// If we've chosen an unsupported device, change the device to serial
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
@ -830,8 +821,10 @@ void blackboxValidateConfig(void)
static void blackboxResetIterationTimers(void)
{
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxLoopIndex = 0;
blackboxIFrameIndex = 0;
blackboxPFrameIndex = 0;
blackboxSlowFrameIterationTimer = 0;
}
/**
@ -1010,6 +1003,7 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
*/
static void loadMainState(timeUs_t currentTimeUs)
{
#ifndef UNIT_TEST
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
blackboxCurrent->time = currentTimeUs;
@ -1056,6 +1050,9 @@ static void loadMainState(timeUs_t currentTimeUs)
//Tail servo for tricopters
blackboxCurrent->servo[5] = servo[5];
#endif
#else
UNUSED(currentTimeUs);
#endif // UNIT_TEST
}
/**
@ -1189,6 +1186,7 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v
*/
static bool blackboxWriteSysinfo(void)
{
#ifndef UNIT_TEST
const uint16_t motorOutputLowInt = lrintf(motorOutputLow);
const uint16_t motorOutputHighInt = lrintf(motorOutputHigh);
@ -1203,11 +1201,11 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("P denom", "%d", blackboxConfig()->p_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt, motorOutputHighInt);
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
@ -1321,6 +1319,7 @@ static bool blackboxWriteSysinfo(void)
}
xmitState.headerIndex++;
#endif // UNIT_TEST
return false;
}
@ -1392,21 +1391,14 @@ static void blackboxCheckAndLogFlightMode(void)
}
}
/*
* Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
* the portion of logged loop iterations.
*/
static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void)
{
/* Adding a magic shift of "blackboxConfig()->rate_num - 1" in here creates a better spread of
* recorded / skipped frames when the I frame's position is considered:
*/
return (pFrameIndex + blackboxConfig()->rate_num - 1) % blackboxConfig()->rate_denom < blackboxConfig()->rate_num;
return blackboxPFrameIndex == 0 && blackboxConfig()->p_denom != 0;
}
static bool blackboxShouldLogIFrame(void)
STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void)
{
return blackboxPFrameIndex == 0;
return blackboxLoopIndex == 0;
}
/*
@ -1420,7 +1412,7 @@ static bool blackboxShouldLogIFrame(void)
STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
{
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
|| (blackboxPFrameIndex == blackboxIInterval / 2 && blackboxIFrameIndex % 128 == 0)) {
return true;
}
return false;
@ -1428,22 +1420,24 @@ STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
#endif
// Called once every FC loop in order to keep track of how many FC loop iterations have passed
static void blackboxAdvanceIterationTimers(void)
STATIC_UNIT_TESTED void blackboxAdvanceIterationTimers(void)
{
blackboxSlowFrameIterationTimer++;
blackboxIteration++;
blackboxPFrameIndex++;
++blackboxSlowFrameIterationTimer;
++blackboxIteration;
if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
blackboxPFrameIndex = 0;
if (++blackboxLoopIndex >= blackboxIInterval) {
blackboxLoopIndex = 0;
blackboxIFrameIndex++;
blackboxPFrameIndex = 0;
} else if (++blackboxPFrameIndex >= blackboxPInterval) {
blackboxPFrameIndex = 0;
}
}
// Called once every FC loop in order to log the current state
static void blackboxLogIteration(timeUs_t currentTimeUs)
STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs)
{
// Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
// Write a keyframe every blackboxIInterval frames so we can resynchronise upon missing frames
if (blackboxShouldLogIFrame()) {
/*
* Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
@ -1459,7 +1453,7 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
blackboxCheckAndLogArmingBeep();
blackboxCheckAndLogFlightMode(); // Check for FlightMode status change event
if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
if (blackboxShouldLogPFrame()) {
/*
* We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
* So only log slow frames during loop iterations where we log a main frame.
@ -1526,6 +1520,7 @@ void blackboxUpdate(timeUs_t currentTimeUs)
}
if (blackboxHeader[xmitState.headerIndex] == '\0') {
blackboxPrintf("%d\n", blackboxIInterval);
blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
}
}
@ -1681,10 +1676,25 @@ void blackboxUpdate(timeUs_t currentTimeUs)
*/
void blackboxInit(void)
{
blackboxResetIterationTimers();
// an I-frame is written every 32ms
// gyro.targetLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, gyro.targetLooptime is rounded for short looptimes
if (gyro.targetLooptime == 31) { // rounded from 31.25us
blackboxIInterval = 1024;
} else if (gyro.targetLooptime == 63) { // rounded from 62.5us
blackboxIInterval = 512;
} else {
blackboxIInterval = (uint16_t)(32 * 1000 / gyro.targetLooptime);
}
// by default p_denom is 32 and a P-frame is written every 1ms
// if p_denom is zero then no P-frames are logged
blackboxPInterval = blackboxConfig()->p_denom == 0 ? 0 : blackboxIInterval / blackboxConfig()->p_denom;
if (blackboxConfig()->device) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
} else {
blackboxSetState(BLACKBOX_STATE_DISABLED);
}
blackboxSInterval = blackboxIInterval * 256; // S-frame is written every 256*32 = 8192ms, approx every 8 seconds
}
#endif