1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00

Change p_ratio to sample_rate

Complete refactor logging rate selection in cli, msp and blackbox code
Retains backward compatibility

Fixed tests and requested changes

Fixed blackbox device "SERIAL" not showing in cms menu and whitespace of "FLASH"
This commit is contained in:
fgiudice98 2020-05-14 17:27:13 +02:00
parent 6aeac3caa6
commit b063cb4e90
8 changed files with 100 additions and 70 deletions

View file

@ -90,7 +90,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
@ -369,7 +369,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;
@ -402,7 +402,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)
@ -460,7 +460,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;
@ -1244,7 +1244,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));
@ -1513,7 +1513,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)
@ -1816,6 +1816,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.
*/ */
@ -1828,15 +1836,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),
@ -771,7 +776,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,23 +58,34 @@
#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",
"SDCARD", "SDCARD",
"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

@ -1601,13 +1601,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;
@ -2730,12 +2732,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,32 +202,10 @@ TEST(BlackboxTest, Test_8kHz)
EXPECT_TRUE(blackboxShouldLogPFrame()); EXPECT_TRUE(blackboxShouldLogPFrame());
} }
TEST(BlackboxTest, Test_zero_p_ratio)
{
blackboxConfigMutable()->p_ratio = 0;
// 1kHz PIDloop
targetPidLooptime = 1000;
blackboxInit();
EXPECT_EQ(32, blackboxIInterval);
EXPECT_EQ(0, blackboxPInterval);
EXPECT_TRUE(blackboxShouldLogIFrame());
EXPECT_FALSE(blackboxShouldLogPFrame());
for (int ii = 0; ii < 31; ++ii) {
blackboxAdvanceIterationTimers();
EXPECT_FALSE(blackboxShouldLogIFrame());
EXPECT_FALSE(blackboxShouldLogPFrame());
}
blackboxAdvanceIterationTimers();
EXPECT_TRUE(blackboxShouldLogIFrame());
EXPECT_FALSE(blackboxShouldLogPFrame());
}
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 +247,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 +268,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());
} }