Better handling of I-frame and P-frame interations

src/main/blackbox/blackbox.c

@@ -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)
     }
 }
 
-/*
+STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(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)
 {
-    /* Adding a magic shift of "blackboxConfig()->rate_num - 1" in here creates a better spread of
+    return blackboxPFrameIndex == 0 && blackboxConfig()->p_denom != 0;
-     * recorded / skipped frames when the I frame's position is considered:
-     */
-    return (pFrameIndex + blackboxConfig()->rate_num - 1) % blackboxConfig()->rate_denom < blackboxConfig()->rate_num;
 }
 
-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++;
+    ++blackboxSlowFrameIterationTimer;
-    blackboxIteration++;
+    ++blackboxIteration;
-    blackboxPFrameIndex++;
 
-    if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
+    if (++blackboxLoopIndex >= blackboxIInterval) {
-        blackboxPFrameIndex = 0;
+        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

src/main/blackbox/blackbox.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "platform.h"
+#include "build/build_config.h"
 #include "blackbox/blackbox_fielddefs.h"
 #include "common/time.h"
 #include "config/parameter_group.h"
@@ -33,8 +35,7 @@ typedef enum BlackboxDevice {
 } BlackboxDevice_e;
 
 typedef struct blackboxConfig_s {
-    uint8_t rate_num;
+    uint16_t p_denom; // I-frame interval / P-frame interval
-    uint8_t rate_denom;
     uint8_t device;
     uint8_t on_motor_test;
     uint8_t record_acc;
@@ -49,3 +50,14 @@ void blackboxUpdate(timeUs_t currentTimeUs);
 void blackboxValidateConfig(void);
 void blackboxFinish(void);
 bool blackboxMayEditConfig(void);
+#ifdef UNIT_TEST
+STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs);
+STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void);
+STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void);
+STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void);
+STATIC_UNIT_TESTED bool writeSlowFrameIfNeeded(void);
+// Called once every FC loop in order to keep track of how many FC loop iterations have passed
+STATIC_UNIT_TESTED void blackboxAdvanceIterationTimers(void);
+extern int32_t blackboxSInterval;
+extern int32_t blackboxSlowFrameIterationTimer;
+#endif

src/main/cms/cms_menu_blackbox.c

@@ -82,7 +82,7 @@ static const char * const cmsx_BlackboxDeviceNames[] = {
     "SERIAL"
 };
 
-static uint8_t blackboxConfig_rate_denom;
+static uint16_t blackboxConfig_p_denom;
 
 static uint8_t cmsx_BlackboxDevice;
 static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames };
@@ -168,7 +168,7 @@ static long cmsx_Blackbox_onEnter(void)
     cmsx_Blackbox_GetDeviceStatus();
     cmsx_BlackboxDevice = blackboxConfig()->device;
 
-    blackboxConfig_rate_denom = blackboxConfig()->rate_denom;
+    blackboxConfig_p_denom = blackboxConfig()->p_denom;
     return 0;
 }
 
@@ -180,7 +180,7 @@ static long cmsx_Blackbox_onExit(const OSD_Entry *self)
         blackboxConfigMutable()->device = cmsx_BlackboxDevice;
         blackboxValidateConfig();
     }
-    blackboxConfigMutable()->rate_denom = blackboxConfig_rate_denom;
+    blackboxConfigMutable()->p_denom = blackboxConfig_p_denom;
     return 0;
 }
 
@@ -196,7 +196,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
     { "(STATUS)",    OME_String,  NULL,            &cmsx_BlackboxStatus,                                      0 },
     { "(USED)",      OME_String,  NULL,            &cmsx_BlackboxDeviceStorageUsed,                           0 },
     { "(FREE)",      OME_String,  NULL,            &cmsx_BlackboxDeviceStorageFree,                           0 },
-    { "RATE DENOM",  OME_UINT8,   NULL,            &(OSD_UINT8_t){ &blackboxConfig_rate_denom, 1, 32, 1 },    0 },
+    { "P DENOM",     OME_UINT16,  NULL,            &(OSD_UINT16_t){ &blackboxConfig_p_denom, 1, INT16_MAX, 1 },0 },
 
 #ifdef USE_FLASHFS
     { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL,                                                      0 },

src/main/fc/fc_msp.c

@@ -1263,13 +1263,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 #ifdef BLACKBOX
         sbufWriteU8(dst, 1); //Blackbox supported
         sbufWriteU8(dst, blackboxConfig()->device);
-        sbufWriteU8(dst, blackboxConfig()->rate_num);
+        sbufWriteU8(dst, 0); // was rate_num
-        sbufWriteU8(dst, blackboxConfig()->rate_denom);
+        sbufWriteU8(dst, 0); // was rate_denom
+        sbufWriteU8(dst, blackboxConfig()->p_denom);
 #else
         sbufWriteU8(dst, 0); // Blackbox not supported
         sbufWriteU8(dst, 0);
         sbufWriteU8(dst, 0);
         sbufWriteU8(dst, 0);
+        sbufWriteU8(dst, 0);
 #endif
         break;
 
@@ -1787,8 +1789,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         // Don't allow config to be updated while Blackbox is logging
         if (blackboxMayEditConfig()) {
             blackboxConfigMutable()->device = sbufReadU8(src);
-            blackboxConfigMutable()->rate_num = sbufReadU8(src);
+            sbufReadU8(src); // was rate_nuk
-            blackboxConfigMutable()->rate_denom = sbufReadU8(src);
+            sbufReadU8(src); // was rate_denom
+            blackboxConfigMutable()->p_denom = sbufReadU8(src);
         }
         break;
 #endif

src/main/fc/settings.c

@@ -376,8 +376,7 @@ const clivalue_t valueTable[] = {
 
 // PG_BLACKBOX_CONFIG
 #ifdef BLACKBOX
-    { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, rate_num) },
+    { "blackbox_p_demom",           VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) },
-    { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, rate_denom) },
     { "blackbox_device",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_DEVICE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, device) },
     { "blackbox_on_motor_test",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, on_motor_test) },
     { "blackbox_record_acc",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, record_acc) },

src/test/Makefile

@@ -46,6 +46,16 @@ battery_unittest_SRC := \
 		$(USER_DIR)/common/maths.c
 
 
+blackbox_unittest_SRC :=  \
+		$(USER_DIR)/blackbox/blackbox.c \
+		$(USER_DIR)/blackbox/blackbox_encoding.c \
+		$(USER_DIR)/blackbox/blackbox_io.c \
+		$(USER_DIR)/common/encoding.c \
+		$(USER_DIR)/common/printf.c \
+		$(USER_DIR)/common/maths.c \
+		$(USER_DIR)/common/typeconversion.c \
+		$(USER_DIR)/drivers/gyro_sync.c
+
 blackbox_encoding_unittest_SRC :=  \
 		$(USER_DIR)/blackbox/blackbox_encoding.c \
 		$(USER_DIR)/common/encoding.c \

src/test/unit/blackbox_unittest.cc

@@ -0,0 +1,299 @@
+/*
+ *
+ * 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 <stdint.h>
+#include <string.h>
+
+extern "C" {
+    #include "platform.h"
+
+    #include "blackbox/blackbox.h"
+    #include "common/utils.h"
+
+    #include "config/parameter_group.h"
+    #include "config/parameter_group_ids.h"
+
+    #include "drivers/accgyro/accgyro.h"
+    #include "drivers/gyro_sync.h"
+    #include "drivers/serial.h"
+
+    #include "flight/failsafe.h"
+    #include "flight/mixer.h"
+
+    #include "fc/rc_controls.h"
+
+    #include "io/serial.h"
+
+    #include "rx/rx.h"
+
+    #include "sensors/battery.h"
+    #include "sensors/gyro.h"
+
+    extern int16_t blackboxIInterval;
+    extern int16_t blackboxPInterval;
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+gyroDev_t gyroDev;
+
+TEST(BlackboxTest, TestInitIntervals)
+{
+    blackboxConfigMutable()->p_denom = 32;
+    // 250Hz PIDloop
+    gyro.targetLooptime = 4000;
+    blackboxInit();
+    EXPECT_EQ(8, blackboxIInterval);
+    EXPECT_EQ(0, blackboxPInterval);
+    EXPECT_EQ(2048, blackboxSInterval);
+
+    // 500Hz PIDloop
+    gyro.targetLooptime = 2000;
+    blackboxInit();
+    EXPECT_EQ(16, blackboxIInterval);
+    EXPECT_EQ(0, blackboxPInterval);
+    EXPECT_EQ(4096, blackboxSInterval);
+
+    // 1kHz PIDloop
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_188HZ, 1, false);
+    blackboxInit();
+    EXPECT_EQ(32, blackboxIInterval);
+    EXPECT_EQ(1, blackboxPInterval);
+    EXPECT_EQ(8192, blackboxSInterval);
+
+    // 2kHz PIDloop
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 4, false);
+    EXPECT_EQ(500, gyro.targetLooptime);
+    blackboxInit();
+    EXPECT_EQ(64, blackboxIInterval);
+    EXPECT_EQ(2, blackboxPInterval);
+    EXPECT_EQ(16384, blackboxSInterval);
+
+    // 4kHz PIDloop
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 2, false);
+    EXPECT_EQ(250, gyro.targetLooptime);
+    blackboxInit();
+    EXPECT_EQ(128, blackboxIInterval);
+    EXPECT_EQ(4, blackboxPInterval);
+    EXPECT_EQ(32768, blackboxSInterval);
+
+    // 8kHz PIDloop
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 1, false);
+    EXPECT_EQ(125, gyro.targetLooptime);
+    gyro.targetLooptime = 125;
+    blackboxInit();
+    EXPECT_EQ(256, blackboxIInterval);
+    EXPECT_EQ(8, blackboxPInterval);
+    EXPECT_EQ(65536, blackboxSInterval);
+
+    // 16kHz PIDloop
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 2, true);
+    EXPECT_EQ(63, gyro.targetLooptime);
+    gyro.targetLooptime = 63; // rounded from 62.5
+    blackboxInit();
+    EXPECT_EQ(512, blackboxIInterval); // note rounding
+    EXPECT_EQ(16, blackboxPInterval);
+    EXPECT_EQ(131072, blackboxSInterval);
+
+    // 32kHz PIDloop
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 1, true);
+    EXPECT_EQ(31, gyro.targetLooptime);
+    gyro.targetLooptime = 31; // rounded from 31.25
+    blackboxInit();
+    EXPECT_EQ(1024, blackboxIInterval); // note rounding
+    EXPECT_EQ(32, blackboxPInterval);
+    EXPECT_EQ(262144, blackboxSInterval);
+}
+
+TEST(BlackboxTest, Test_500Hz)
+{
+    blackboxConfigMutable()->p_denom = 32;
+    // 500Hz PIDloop
+    gyro.targetLooptime = 2000;
+    blackboxInit();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+    for (int ii = 0; ii < 15; ++ii) {
+        blackboxAdvanceIterationTimers();
+        EXPECT_EQ(false, blackboxShouldLogIFrame());
+        EXPECT_EQ(true, blackboxShouldLogPFrame());
+    }
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+}
+
+TEST(BlackboxTest, Test_1kHz)
+{
+    blackboxConfigMutable()->p_denom = 32;
+    // 1kHz PIDloop
+    gyro.targetLooptime = 1000;
+    blackboxInit();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+    EXPECT_EQ(0, blackboxSlowFrameIterationTimer);
+    EXPECT_EQ(false, blackboxSlowFrameIterationTimer >= blackboxSInterval);
+    blackboxSlowFrameIterationTimer = blackboxSInterval;
+    EXPECT_EQ(true, writeSlowFrameIfNeeded());
+    EXPECT_EQ(0, blackboxSlowFrameIterationTimer);
+
+    for (int ii = 0; ii < 31; ++ii) {
+        blackboxAdvanceIterationTimers();
+        EXPECT_EQ(false, blackboxShouldLogIFrame());
+        EXPECT_EQ(true, blackboxShouldLogPFrame());
+        EXPECT_EQ(ii + 1, blackboxSlowFrameIterationTimer);
+        EXPECT_EQ(false, writeSlowFrameIfNeeded());
+    }
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+}
+
+TEST(BlackboxTest, Test_2kHz)
+{
+    blackboxConfigMutable()->p_denom = 32;
+    // 2kHz PIDloop
+    gyro.targetLooptime = 500;
+    blackboxInit();
+    EXPECT_EQ(64, blackboxIInterval);
+    EXPECT_EQ(2, blackboxPInterval);
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(false, blackboxShouldLogIFrame());
+    EXPECT_EQ(false, blackboxShouldLogPFrame());
+
+    for (int ii = 0; ii < 31; ++ii) {
+        blackboxAdvanceIterationTimers();
+        EXPECT_EQ(false, blackboxShouldLogIFrame());
+        EXPECT_EQ(true, blackboxShouldLogPFrame());
+
+        blackboxAdvanceIterationTimers();
+        EXPECT_EQ(false, blackboxShouldLogIFrame());
+        EXPECT_EQ(false, blackboxShouldLogPFrame());
+    }
+
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(false, blackboxShouldLogIFrame());
+    EXPECT_EQ(false, blackboxShouldLogPFrame());
+}
+
+TEST(BlackboxTest, Test_8kHz)
+{
+    blackboxConfigMutable()->p_denom = 32;
+    // 1kHz PIDloop
+    gyro.targetLooptime = 125;
+    blackboxInit();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+    EXPECT_EQ(0, blackboxSlowFrameIterationTimer);
+    EXPECT_EQ(false, blackboxSlowFrameIterationTimer >= blackboxSInterval);
+    blackboxSlowFrameIterationTimer = blackboxSInterval;
+    EXPECT_EQ(true, writeSlowFrameIfNeeded());
+    EXPECT_EQ(0, blackboxSlowFrameIterationTimer);
+
+    for (int ii = 0; ii < 255; ++ii) {
+        blackboxAdvanceIterationTimers();
+        EXPECT_EQ(false, blackboxShouldLogIFrame());
+        EXPECT_EQ(ii + 1, blackboxSlowFrameIterationTimer);
+        EXPECT_EQ(false, writeSlowFrameIfNeeded());
+    }
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(true, blackboxShouldLogPFrame());
+}
+
+TEST(BlackboxTest, Test_zero_p_denom)
+{
+    blackboxConfigMutable()->p_denom = 0;
+    // 2kHz PIDloop
+    gyro.targetLooptime = 1000;
+    blackboxInit();
+    EXPECT_EQ(32, blackboxIInterval);
+    EXPECT_EQ(0, blackboxPInterval);
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(false, blackboxShouldLogPFrame());
+
+    for (int ii = 0; ii < 31; ++ii) {
+        blackboxAdvanceIterationTimers();
+        EXPECT_EQ(false, blackboxShouldLogIFrame());
+        EXPECT_EQ(false, blackboxShouldLogPFrame());
+    }
+    blackboxAdvanceIterationTimers();
+    EXPECT_EQ(true, blackboxShouldLogIFrame());
+    EXPECT_EQ(false, blackboxShouldLogPFrame());
+}
+
+// STUBS
+extern "C" {
+
+PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
+PG_REGISTER(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
+PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
+PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
+PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
+PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
+
+uint8_t armingFlags;
+uint8_t stateFlags;
+const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000,
+        400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000}; // see baudRate_e
+uint8_t debugMode;
+int32_t blackboxHeaderBudget;
+int32_t GPS_coord[2];
+uint16_t GPS_altitude;
+uint16_t GPS_speed;
+uint8_t GPS_numSat;
+uint16_t GPS_ground_course;
+int32_t GPS_home[2];
+
+gyro_t gyro;
+
+uint16_t motorOutputHigh, motorOutputLow;
+int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
+struct pidProfile_s;
+struct pidProfile_s *currentPidProfile;
+uint32_t targetPidLooptime;
+
+uint32_t rcModeActivationMask;
+
+void mspSerialAllocatePorts(void) {}
+uint32_t getArmingBeepTimeMicros(void) {return 0;}
+uint16_t getBatteryVoltageLatest(void) {return 0;}
+uint8_t getMotorCount() {return 4;}
+bool isModeActivationConditionPresent(const modeActivationCondition_t *, boxId_e) {return false;}
+uint32_t millis(void) {return 0;}
+bool sensors(uint32_t) {return false;}
+void serialWrite(serialPort_t *, uint8_t) {}
+uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
+bool isSerialTransmitBufferEmpty(const serialPort_t *) {return false;}
+bool feature(uint32_t) {return false;}
+void mspSerialReleasePortIfAllocated(serialPort_t *) {}
+serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
+serialPort_t *findSharedSerialPort(uint16_t , serialPortFunction_e ) {return NULL;}
+serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) {return NULL;}
+void closeSerialPort(serialPort_t *) {}
+portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e ) {return PORTSHARING_UNUSED;}
+failsafePhase_e failsafePhase(void) {return FAILSAFE_IDLE;}
+bool rxAreFlightChannelsValid(void) {return false;}
+bool rxIsReceivingSignal(void) {return false;}
+
+}