1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-22 15:55:48 +03:00

Merge pull request #9803 from fgiudice98/blackbox-freq-cms

This commit is contained in:
Michael Keller 2020-06-30 00:55:16 +12:00 committed by GitHub
commit a0f6a91033
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 106 additions and 55 deletions

View file

@ -91,7 +91,7 @@
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.p_ratio = 32, .sample_rate = BLACKBOX_RATE_QUARTER,
.device = DEFAULT_BLACKBOX_DEVICE, .device = DEFAULT_BLACKBOX_DEVICE,
.record_acc = 1, .record_acc = 1,
.mode = BLACKBOX_MODE_NORMAL .mode = BLACKBOX_MODE_NORMAL
@ -371,7 +371,7 @@ static uint16_t blackboxIFrameIndex;
// typically 32 for 1kHz loop, 64 for 2kHz loop etc // typically 32 for 1kHz loop, 64 for 2kHz loop etc
STATIC_UNIT_TESTED int16_t blackboxIInterval = 0; STATIC_UNIT_TESTED int16_t blackboxIInterval = 0;
// number of flight loop iterations before logging P-frame // number of flight loop iterations before logging P-frame
STATIC_UNIT_TESTED int16_t blackboxPInterval = 0; STATIC_UNIT_TESTED int8_t blackboxPInterval = 0;
STATIC_UNIT_TESTED int32_t blackboxSInterval = 0; STATIC_UNIT_TESTED int32_t blackboxSInterval = 0;
STATIC_UNIT_TESTED int32_t blackboxSlowFrameIterationTimer; STATIC_UNIT_TESTED int32_t blackboxSlowFrameIterationTimer;
static bool blackboxLoggedAnyFrames; static bool blackboxLoggedAnyFrames;
@ -404,7 +404,7 @@ bool blackboxMayEditConfig(void)
static bool blackboxIsOnlyLoggingIntraframes(void) static bool blackboxIsOnlyLoggingIntraframes(void)
{ {
return blackboxConfig()->p_ratio == 0; return blackboxPInterval == 0;
} }
static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
@ -462,7 +462,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return isRssiConfigured(); return isRssiConfigured();
case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
return blackboxConfig()->p_ratio != 1; return blackboxPInterval != blackboxIInterval;
case FLIGHT_LOG_FIELD_CONDITION_ACC: case FLIGHT_LOG_FIELD_CONDITION_ACC:
return sensors(SENSOR_ACC) && blackboxConfig()->record_acc; return sensors(SENSOR_ACC) && blackboxConfig()->record_acc;
@ -1246,7 +1246,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", pilotConfig()->name); BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", pilotConfig()->name);
BLACKBOX_PRINT_HEADER_LINE("I interval", "%d", blackboxIInterval); BLACKBOX_PRINT_HEADER_LINE("I interval", "%d", blackboxIInterval);
BLACKBOX_PRINT_HEADER_LINE("P interval", "%d", blackboxPInterval); BLACKBOX_PRINT_HEADER_LINE("P interval", "%d", blackboxPInterval);
BLACKBOX_PRINT_HEADER_LINE("P ratio", "%d", blackboxConfig()->p_ratio); BLACKBOX_PRINT_HEADER_LINE("P ratio", "%d", (uint16_t)(blackboxIInterval / blackboxPInterval));
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
@ -1515,7 +1515,7 @@ static void blackboxCheckAndLogFlightMode(void)
STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void) STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void)
{ {
return blackboxPFrameIndex == 0 && blackboxConfig()->p_ratio != 0; return blackboxPFrameIndex == 0 && blackboxPInterval != 0;
} }
STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void) STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void)
@ -1826,6 +1826,14 @@ uint8_t blackboxGetRateDenom(void)
} }
uint16_t blackboxGetPRatio(void) {
return blackboxIInterval / blackboxPInterval;
}
uint8_t blackboxCalculateSampleRate(uint16_t pRatio) {
return LOG2(32000 / (targetPidLooptime * pRatio));
}
/** /**
* Call during system startup to initialize the blackbox. * Call during system startup to initialize the blackbox.
*/ */
@ -1838,15 +1846,11 @@ void blackboxInit(void)
// targetPidLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, targetPidLooptime is rounded for short looptimes // targetPidLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, targetPidLooptime is rounded for short looptimes
blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime); blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime);
// by default p_ratio is 32 and a P-frame is written every 1ms blackboxPInterval = 1 << blackboxConfig()->sample_rate;
// if p_ratio is zero then no P-frames are logged if (blackboxPInterval > blackboxIInterval) {
if (blackboxConfig()->p_ratio == 0) { blackboxPInterval = 0; // log only I frames if logging frequency is too low
blackboxPInterval = 0; // blackboxPInterval not used when p_ratio is zero, so just set it to zero
} else if (blackboxConfig()->p_ratio > blackboxIInterval && blackboxIInterval >= 32) {
blackboxPInterval = 1;
} else {
blackboxPInterval = blackboxIInterval / blackboxConfig()->p_ratio;
} }
if (blackboxConfig()->device) { if (blackboxConfig()->device) {
blackboxSetState(BLACKBOX_STATE_STOPPED); blackboxSetState(BLACKBOX_STATE_STOPPED);
} else { } else {

View file

@ -38,6 +38,14 @@ typedef enum BlackboxMode {
BLACKBOX_MODE_ALWAYS_ON BLACKBOX_MODE_ALWAYS_ON
} BlackboxMode; } BlackboxMode;
typedef enum BlackboxSampleRate { // Sample rate is 1/(2^BlackboxSampleRate)
BLACKBOX_RATE_ONE = 0,
BLACKBOX_RATE_HALF,
BLACKBOX_RATE_QUARTER,
BLACKBOX_RATE_8TH,
BLACKBOX_RATE_16TH
} BlackboxSampleRate_e;
typedef enum FlightLogEvent { typedef enum FlightLogEvent {
FLIGHT_LOG_EVENT_SYNC_BEEP = 0, FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, // UNUSED FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, // UNUSED
@ -51,7 +59,7 @@ typedef enum FlightLogEvent {
} FlightLogEvent; } FlightLogEvent;
typedef struct blackboxConfig_s { typedef struct blackboxConfig_s {
uint16_t p_ratio; // I-frame interval / P-frame interval uint8_t sample_rate; // sample rate
uint8_t device; uint8_t device;
uint8_t record_acc; uint8_t record_acc;
uint8_t mode; uint8_t mode;
@ -67,6 +75,8 @@ void blackboxUpdate(timeUs_t currentTimeUs);
void blackboxSetStartDateTime(const char *dateTime, timeMs_t timeNowMs); void blackboxSetStartDateTime(const char *dateTime, timeMs_t timeNowMs);
int blackboxCalculatePDenom(int rateNum, int rateDenom); int blackboxCalculatePDenom(int rateNum, int rateDenom);
uint8_t blackboxGetRateDenom(void); uint8_t blackboxGetRateDenom(void);
uint16_t blackboxGetPRatio(void);
uint8_t blackboxCalculateSampleRate(uint16_t pRatio);
void blackboxValidateConfig(void); void blackboxValidateConfig(void);
void blackboxFinish(void); void blackboxFinish(void);
bool blackboxMayEditConfig(void); bool blackboxMayEditConfig(void);

View file

@ -216,6 +216,10 @@ static const char * const lookupTableBlackboxDevice[] = {
static const char * const lookupTableBlackboxMode[] = { static const char * const lookupTableBlackboxMode[] = {
"NORMAL", "MOTOR_TEST", "ALWAYS" "NORMAL", "MOTOR_TEST", "ALWAYS"
}; };
static const char * const lookupTableBlackboxSampleRate[] = {
"1/1", "1/2", "1/4", "1/8", "1/16"
};
#endif #endif
#ifdef USE_SERIAL_RX #ifdef USE_SERIAL_RX
@ -511,6 +515,7 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
LOOKUP_TABLE_ENTRY(lookupTableBlackboxDevice), LOOKUP_TABLE_ENTRY(lookupTableBlackboxDevice),
LOOKUP_TABLE_ENTRY(lookupTableBlackboxMode), LOOKUP_TABLE_ENTRY(lookupTableBlackboxMode),
LOOKUP_TABLE_ENTRY(lookupTableBlackboxSampleRate),
#endif #endif
LOOKUP_TABLE_ENTRY(currentMeterSourceNames), LOOKUP_TABLE_ENTRY(currentMeterSourceNames),
LOOKUP_TABLE_ENTRY(voltageMeterSourceNames), LOOKUP_TABLE_ENTRY(voltageMeterSourceNames),
@ -770,7 +775,7 @@ const clivalue_t valueTable[] = {
// PG_BLACKBOX_CONFIG // PG_BLACKBOX_CONFIG
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
{ "blackbox_p_ratio", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_ratio) }, { "blackbox_sample_rate", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_SAMPLE_RATE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, sample_rate) },
{ "blackbox_device", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_DEVICE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, device) }, { "blackbox_device", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_DEVICE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, device) },
{ "blackbox_record_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, record_acc) }, { "blackbox_record_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, record_acc) },
{ "blackbox_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_MODE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, mode) }, { "blackbox_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_MODE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, mode) },

View file

@ -41,6 +41,7 @@ typedef enum {
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
TABLE_BLACKBOX_DEVICE, TABLE_BLACKBOX_DEVICE,
TABLE_BLACKBOX_MODE, TABLE_BLACKBOX_MODE,
TABLE_BLACKBOX_SAMPLE_RATE,
#endif #endif
TABLE_CURRENT_METER, TABLE_CURRENT_METER,
TABLE_VOLTAGE_METER, TABLE_VOLTAGE_METER,

View file

@ -58,6 +58,8 @@
#include "pg/pg.h" #include "pg/pg.h"
#include "flight/pid.h"
static const char * const cmsx_BlackboxDeviceNames[] = { static const char * const cmsx_BlackboxDeviceNames[] = {
"NONE", "NONE",
"FLASH", "FLASH",
@ -65,16 +67,25 @@ static const char * const cmsx_BlackboxDeviceNames[] = {
"SERIAL" "SERIAL"
}; };
static uint16_t blackboxConfig_p_ratio; static const char * const cmsx_BlackboxRateNames[] = {
"1/1",
"1/2",
"1/4",
"1/8",
"1/16"
};
static uint8_t cmsx_BlackboxDevice; static uint8_t cmsx_BlackboxDevice;
static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames }; static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 3, cmsx_BlackboxDeviceNames };
static uint8_t cmsx_BlackboxRate;
static OSD_TAB_t cmsx_BlackboxRateTable = { &cmsx_BlackboxRate, 4, cmsx_BlackboxRateNames };
static debugType_e systemConfig_debug_mode; static debugType_e systemConfig_debug_mode;
#define CMS_BLACKBOX_STRING_LENGTH 8 #define CMS_BLACKBOX_STRING_LENGTH 8
static char cmsx_BlackboxStatus[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxStatus[CMS_BLACKBOX_STRING_LENGTH];
static char cmsx_BlackboxDeviceStorageUsed[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxDeviceStorageUsed[CMS_BLACKBOX_STRING_LENGTH];
static char cmsx_BlackboxDeviceStorageFree[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxDeviceStorageFree[CMS_BLACKBOX_STRING_LENGTH];
static char cmsx_pidFreq[CMS_BLACKBOX_STRING_LENGTH];
static void cmsx_Blackbox_GetDeviceStatus(void) static void cmsx_Blackbox_GetDeviceStatus(void)
{ {
@ -184,9 +195,15 @@ static const void *cmsx_Blackbox_onEnter(displayPort_t *pDisp)
cmsx_Blackbox_GetDeviceStatus(); cmsx_Blackbox_GetDeviceStatus();
cmsx_BlackboxDevice = blackboxConfig()->device; cmsx_BlackboxDevice = blackboxConfig()->device;
cmsx_BlackboxRate = blackboxConfig()->sample_rate;
blackboxConfig_p_ratio = blackboxConfig()->p_ratio;
systemConfig_debug_mode = systemConfig()->debug_mode; systemConfig_debug_mode = systemConfig()->debug_mode;
const uint16_t pidFreq = (uint16_t)pidGetPidFrequency();
if (pidFreq > 1000) {
tfp_sprintf(cmsx_pidFreq, "%1d.%02dKHZ", (pidFreq / 10) / 100, (pidFreq / 10) % 100);
} else {
tfp_sprintf(cmsx_pidFreq, "%3dHZ", pidFreq);
}
return NULL; return NULL;
} }
@ -199,7 +216,7 @@ static const void *cmsx_Blackbox_onExit(displayPort_t *pDisp, const OSD_Entry *s
blackboxConfigMutable()->device = cmsx_BlackboxDevice; blackboxConfigMutable()->device = cmsx_BlackboxDevice;
blackboxValidateConfig(); blackboxValidateConfig();
} }
blackboxConfigMutable()->p_ratio = blackboxConfig_p_ratio; blackboxConfigMutable()->sample_rate = cmsx_BlackboxRate;
systemConfigMutable()->debug_mode = systemConfig_debug_mode; systemConfigMutable()->debug_mode = systemConfig_debug_mode;
return NULL; return NULL;
@ -230,11 +247,12 @@ static CMS_Menu cmsx_menuEraseFlashCheck = {
static const OSD_Entry cmsx_menuBlackboxEntries[] = static const OSD_Entry cmsx_menuBlackboxEntries[] =
{ {
{ "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, { "-- BLACKBOX --", OME_Label, NULL, NULL, 0},
{ "(PID FREQ)", OME_String, NULL, &cmsx_pidFreq, 0 },
{ "SAMPLERATE", OME_TAB, NULL, &cmsx_BlackboxRateTable, REBOOT_REQUIRED },
{ "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, REBOOT_REQUIRED }, { "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, REBOOT_REQUIRED },
{ "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 }, { "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 },
{ "(USED)", OME_String, NULL, &cmsx_BlackboxDeviceStorageUsed, 0 }, { "(USED)", OME_String, NULL, &cmsx_BlackboxDeviceStorageUsed, 0 },
{ "(FREE)", OME_String, NULL, &cmsx_BlackboxDeviceStorageFree, 0 }, { "(FREE)", OME_String, NULL, &cmsx_BlackboxDeviceStorageFree, 0 },
{ "P RATIO", OME_UINT16, NULL, &(OSD_UINT16_t){ &blackboxConfig_p_ratio, 1, INT16_MAX, 1 }, REBOOT_REQUIRED },
{ "DEBUG MODE", OME_TAB, NULL, &(OSD_TAB_t) { &systemConfig_debug_mode, DEBUG_COUNT - 1, debugModeNames }, REBOOT_REQUIRED }, { "DEBUG MODE", OME_TAB, NULL, &(OSD_TAB_t) { &systemConfig_debug_mode, DEBUG_COUNT - 1, debugModeNames }, REBOOT_REQUIRED },
#ifdef USE_FLASHFS #ifdef USE_FLASHFS

View file

@ -1590,13 +1590,15 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, blackboxConfig()->device); sbufWriteU8(dst, blackboxConfig()->device);
sbufWriteU8(dst, 1); // Rate numerator, not used anymore sbufWriteU8(dst, 1); // Rate numerator, not used anymore
sbufWriteU8(dst, blackboxGetRateDenom()); sbufWriteU8(dst, blackboxGetRateDenom());
sbufWriteU16(dst, blackboxConfig()->p_ratio); sbufWriteU16(dst, blackboxGetPRatio());
sbufWriteU8(dst, blackboxConfig()->sample_rate);
#else #else
sbufWriteU8(dst, 0); // Blackbox not supported sbufWriteU8(dst, 0); // Blackbox not supported
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU8(dst, 0);
#endif #endif
break; break;
@ -2763,12 +2765,21 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
blackboxConfigMutable()->device = sbufReadU8(src); blackboxConfigMutable()->device = sbufReadU8(src);
const int rateNum = sbufReadU8(src); // was rate_num const int rateNum = sbufReadU8(src); // was rate_num
const int rateDenom = sbufReadU8(src); // was rate_denom const int rateDenom = sbufReadU8(src); // was rate_denom
uint16_t pRatio = 0;
if (sbufBytesRemaining(src) >= 2) { if (sbufBytesRemaining(src) >= 2) {
// p_ratio specified, so use it directly // p_ratio specified, so use it directly
blackboxConfigMutable()->p_ratio = sbufReadU16(src); pRatio = sbufReadU16(src);
} else { } else {
// p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom // p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom
blackboxConfigMutable()->p_ratio = blackboxCalculatePDenom(rateNum, rateDenom); pRatio = blackboxCalculatePDenom(rateNum, rateDenom);
}
if (sbufBytesRemaining(src) >= 1) {
// sample_rate specified, so use it directly
blackboxConfigMutable()->sample_rate = sbufReadU8(src);
} else {
// sample_rate not specified in MSP, so calculate it from old p_ratio
blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(pRatio);
} }
} }
break; break;

View file

@ -90,7 +90,7 @@ void targetConfiguration(void)
mixerConfigMutable()->yaw_motors_reversed = true; mixerConfigMutable()->yaw_motors_reversed = true;
imuConfigMutable()->small_angle = 180; imuConfigMutable()->small_angle = 180;
blackboxConfigMutable()->p_ratio = 128; blackboxConfigMutable()->sample_rate = 1; // sample_rate is half of PID loop frequency
/* Breadboard-specific settings for development purposes only /* Breadboard-specific settings for development purposes only
*/ */

View file

@ -60,7 +60,7 @@ gyroDev_t gyroDev;
TEST(BlackboxTest, TestInitIntervals) TEST(BlackboxTest, TestInitIntervals)
{ {
blackboxConfigMutable()->p_ratio = 32; blackboxConfigMutable()->sample_rate = 4; // sample_rate = PID loop frequency / 16
// 250Hz PIDloop // 250Hz PIDloop
targetPidLooptime = 4000; targetPidLooptime = 4000;
blackboxInit(); blackboxInit();
@ -72,9 +72,10 @@ TEST(BlackboxTest, TestInitIntervals)
targetPidLooptime = 2000; targetPidLooptime = 2000;
blackboxInit(); blackboxInit();
EXPECT_EQ(16, blackboxIInterval); EXPECT_EQ(16, blackboxIInterval);
EXPECT_EQ(0, blackboxPInterval); EXPECT_EQ(16, blackboxPInterval);
EXPECT_EQ(4096, blackboxSInterval); EXPECT_EQ(4096, blackboxSInterval);
blackboxConfigMutable()->sample_rate = 1; // sample_rate = PID loop frequency / 2
// 2kHz PIDloop // 2kHz PIDloop
targetPidLooptime = 500; targetPidLooptime = 500;
blackboxInit(); blackboxInit();
@ -82,6 +83,7 @@ TEST(BlackboxTest, TestInitIntervals)
EXPECT_EQ(2, blackboxPInterval); EXPECT_EQ(2, blackboxPInterval);
EXPECT_EQ(16384, blackboxSInterval); EXPECT_EQ(16384, blackboxSInterval);
blackboxConfigMutable()->sample_rate = 2;
// 4kHz PIDloop // 4kHz PIDloop
targetPidLooptime = 250; targetPidLooptime = 250;
blackboxInit(); blackboxInit();
@ -89,6 +91,7 @@ TEST(BlackboxTest, TestInitIntervals)
EXPECT_EQ(4, blackboxPInterval); EXPECT_EQ(4, blackboxPInterval);
EXPECT_EQ(32768, blackboxSInterval); EXPECT_EQ(32768, blackboxSInterval);
blackboxConfigMutable()->sample_rate = 3;
// 8kHz PIDloop // 8kHz PIDloop
targetPidLooptime = 125; targetPidLooptime = 125;
blackboxInit(); blackboxInit();
@ -99,7 +102,7 @@ TEST(BlackboxTest, TestInitIntervals)
TEST(BlackboxTest, Test_500Hz) TEST(BlackboxTest, Test_500Hz)
{ {
blackboxConfigMutable()->p_ratio = 32; blackboxConfigMutable()->sample_rate = 0;
// 500Hz PIDloop // 500Hz PIDloop
targetPidLooptime = 2000; targetPidLooptime = 2000;
blackboxInit(); blackboxInit();
@ -117,7 +120,7 @@ TEST(BlackboxTest, Test_500Hz)
TEST(BlackboxTest, Test_1kHz) TEST(BlackboxTest, Test_1kHz)
{ {
blackboxConfigMutable()->p_ratio = 32; blackboxConfigMutable()->sample_rate = 0;
// 1kHz PIDloop // 1kHz PIDloop
targetPidLooptime = 1000; targetPidLooptime = 1000;
blackboxInit(); blackboxInit();
@ -143,7 +146,7 @@ TEST(BlackboxTest, Test_1kHz)
TEST(BlackboxTest, Test_2kHz) TEST(BlackboxTest, Test_2kHz)
{ {
blackboxConfigMutable()->p_ratio = 32; blackboxConfigMutable()->sample_rate = 1;
// 2kHz PIDloop // 2kHz PIDloop
targetPidLooptime = 500; targetPidLooptime = 500;
blackboxInit(); blackboxInit();
@ -176,7 +179,7 @@ TEST(BlackboxTest, Test_2kHz)
TEST(BlackboxTest, Test_8kHz) TEST(BlackboxTest, Test_8kHz)
{ {
blackboxConfigMutable()->p_ratio = 32; blackboxConfigMutable()->sample_rate = 3;
// 8kHz PIDloop // 8kHz PIDloop
targetPidLooptime = 125; targetPidLooptime = 125;
blackboxInit(); blackboxInit();
@ -199,18 +202,18 @@ TEST(BlackboxTest, Test_8kHz)
EXPECT_TRUE(blackboxShouldLogPFrame()); EXPECT_TRUE(blackboxShouldLogPFrame());
} }
TEST(BlackboxTest, Test_zero_p_ratio) TEST(BlackboxTest, Test_zero_p_interval)
{ {
blackboxConfigMutable()->p_ratio = 0; blackboxConfigMutable()->sample_rate = 4;
// 1kHz PIDloop // 250Hz PIDloop
targetPidLooptime = 1000; targetPidLooptime = 4000;
blackboxInit(); blackboxInit();
EXPECT_EQ(32, blackboxIInterval); EXPECT_EQ(8, blackboxIInterval);
EXPECT_EQ(0, blackboxPInterval); EXPECT_EQ(0, blackboxPInterval);
EXPECT_TRUE(blackboxShouldLogIFrame()); EXPECT_TRUE(blackboxShouldLogIFrame());
EXPECT_FALSE(blackboxShouldLogPFrame()); EXPECT_FALSE(blackboxShouldLogPFrame());
for (int ii = 0; ii < 31; ++ii) { for (int ii = 0; ii < 7; ++ii) {
blackboxAdvanceIterationTimers(); blackboxAdvanceIterationTimers();
EXPECT_FALSE(blackboxShouldLogIFrame()); EXPECT_FALSE(blackboxShouldLogIFrame());
EXPECT_FALSE(blackboxShouldLogPFrame()); EXPECT_FALSE(blackboxShouldLogPFrame());
@ -222,9 +225,8 @@ TEST(BlackboxTest, Test_zero_p_ratio)
TEST(BlackboxTest, Test_CalculatePDenom) TEST(BlackboxTest, Test_CalculatePDenom)
{ {
blackboxConfigMutable()->p_ratio = 0; blackboxConfigMutable()->sample_rate = 0;
// note I-frame is logged every 32ms regardless of PIDloop rate // note I-frame is logged every 32ms regardless of PIDloop rate
// so p_ratio is 32 when blackbox logging rate is 1kHz
// 1kHz PIDloop // 1kHz PIDloop
targetPidLooptime = 1000; targetPidLooptime = 1000;
@ -266,19 +268,19 @@ TEST(BlackboxTest, Test_CalculateRates)
{ {
// 1kHz PIDloop // 1kHz PIDloop
targetPidLooptime = 1000; targetPidLooptime = 1000;
blackboxConfigMutable()->p_ratio = 32; blackboxConfigMutable()->sample_rate = 0;
blackboxInit(); blackboxInit();
EXPECT_EQ(32, blackboxIInterval); EXPECT_EQ(32, blackboxIInterval);
EXPECT_EQ(1, blackboxPInterval); EXPECT_EQ(1, blackboxPInterval);
EXPECT_EQ(1, blackboxGetRateDenom()); EXPECT_EQ(1, blackboxGetRateDenom());
blackboxConfigMutable()->p_ratio = 16; blackboxConfigMutable()->sample_rate = 1;
blackboxInit(); blackboxInit();
EXPECT_EQ(32, blackboxIInterval); EXPECT_EQ(32, blackboxIInterval);
EXPECT_EQ(2, blackboxPInterval); EXPECT_EQ(2, blackboxPInterval);
EXPECT_EQ(2, blackboxGetRateDenom()); EXPECT_EQ(2, blackboxGetRateDenom());
blackboxConfigMutable()->p_ratio = 8; blackboxConfigMutable()->sample_rate = 2;
blackboxInit(); blackboxInit();
EXPECT_EQ(32, blackboxIInterval); EXPECT_EQ(32, blackboxIInterval);
EXPECT_EQ(4, blackboxPInterval); EXPECT_EQ(4, blackboxPInterval);
@ -287,43 +289,43 @@ TEST(BlackboxTest, Test_CalculateRates)
// 8kHz PIDloop // 8kHz PIDloop
targetPidLooptime = 125; targetPidLooptime = 125;
blackboxConfigMutable()->p_ratio = 32; // 1kHz logging blackboxConfigMutable()->sample_rate = 3; // 1kHz logging
blackboxInit(); blackboxInit();
EXPECT_EQ(256, blackboxIInterval); EXPECT_EQ(256, blackboxIInterval);
EXPECT_EQ(8, blackboxPInterval); EXPECT_EQ(8, blackboxPInterval);
EXPECT_EQ(8, blackboxGetRateDenom()); EXPECT_EQ(8, blackboxGetRateDenom());
blackboxConfigMutable()->p_ratio = 48; // 1.5kHz logging blackboxConfigMutable()->sample_rate = 4; // 500Hz logging
blackboxInit(); blackboxInit();
EXPECT_EQ(256, blackboxIInterval); EXPECT_EQ(256, blackboxIInterval);
EXPECT_EQ(5, blackboxPInterval); EXPECT_EQ(16, blackboxPInterval);
EXPECT_EQ(5, blackboxGetRateDenom()); EXPECT_EQ(16, blackboxGetRateDenom());
blackboxConfigMutable()->p_ratio = 64; // 2kHz logging blackboxConfigMutable()->sample_rate = 2; // 2kHz logging
blackboxInit(); blackboxInit();
EXPECT_EQ(256, blackboxIInterval); EXPECT_EQ(256, blackboxIInterval);
EXPECT_EQ(4, blackboxPInterval); EXPECT_EQ(4, blackboxPInterval);
EXPECT_EQ(4, blackboxGetRateDenom()); EXPECT_EQ(4, blackboxGetRateDenom());
blackboxConfigMutable()->p_ratio = 128; // 4kHz logging blackboxConfigMutable()->sample_rate = 1; // 4kHz logging
blackboxInit(); blackboxInit();
EXPECT_EQ(256, blackboxIInterval); EXPECT_EQ(256, blackboxIInterval);
EXPECT_EQ(2, blackboxPInterval); EXPECT_EQ(2, blackboxPInterval);
EXPECT_EQ(2, blackboxGetRateDenom()); EXPECT_EQ(2, blackboxGetRateDenom());
blackboxConfigMutable()->p_ratio = 256; // 8kHz logging blackboxConfigMutable()->sample_rate = 0; // 8kHz logging
blackboxInit(); blackboxInit();
EXPECT_EQ(256, blackboxIInterval); EXPECT_EQ(256, blackboxIInterval);
EXPECT_EQ(1, blackboxPInterval); EXPECT_EQ(1, blackboxPInterval);
EXPECT_EQ(1, blackboxGetRateDenom()); EXPECT_EQ(1, blackboxGetRateDenom());
// 0.126 PIDloop // 0.126 PIDloop = 7.94kHz
targetPidLooptime = 126; targetPidLooptime = 126;
blackboxConfigMutable()->p_ratio = 32; // 1kHz logging blackboxConfigMutable()->sample_rate = 3; // 0.992kHz logging
blackboxInit(); blackboxInit();
EXPECT_EQ(253, blackboxIInterval); EXPECT_EQ(253, blackboxIInterval);
EXPECT_EQ(7, blackboxPInterval); EXPECT_EQ(8, blackboxPInterval);
EXPECT_EQ(7, blackboxGetRateDenom()); EXPECT_EQ(8, blackboxGetRateDenom());
} }