Calculate blackbox rate_num and rate_denom for MSP

src/main/blackbox/blackbox.c

@@ -1669,6 +1669,21 @@ void blackboxUpdate(timeUs_t currentTimeUs)
     }
 }
 
+int blackboxCalculatePDenom(int rateNum, int rateDenom)
+{
+    return blackboxIInterval * rateNum / rateDenom;
+}
+
+uint8_t blackboxGetRateNum(void)
+{
+    return blackboxGetRateDenom() * blackboxConfig()->p_denom / blackboxIInterval;
+}
+
+uint8_t blackboxGetRateDenom(void)
+{
+    return gcd(blackboxIInterval, blackboxPInterval);
+}
+
 /**
  * Call during system startup to initialize the blackbox.
  */

src/main/blackbox/blackbox.h

@@ -47,6 +47,9 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
 
 void blackboxInit(void);
 void blackboxUpdate(timeUs_t currentTimeUs);
+int blackboxCalculatePDenom(int rateNum, int rateDenom);
+uint8_t blackboxGetRateNum(void);
+uint8_t blackboxGetRateDenom(void);
 void blackboxValidateConfig(void);
 void blackboxFinish(void);
 bool blackboxMayEditConfig(void);

src/main/fc/fc_msp.c

@@ -1263,8 +1263,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 #ifdef BLACKBOX
         sbufWriteU8(dst, 1); //Blackbox supported
         sbufWriteU8(dst, blackboxConfig()->device);
-        sbufWriteU8(dst, 0); // was rate_num
+        sbufWriteU8(dst, blackboxGetRateNum());
-        sbufWriteU8(dst, 0); // was rate_denom
+        sbufWriteU8(dst, blackboxGetRateDenom());
         sbufWriteU8(dst, blackboxConfig()->p_denom);
 #else
         sbufWriteU8(dst, 0); // Blackbox not supported
@@ -1789,9 +1789,15 @@ 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);
-            sbufReadU8(src); // was rate_nuk
+            const int rateNum = sbufReadU8(src); // was rate_num
-            sbufReadU8(src); // was rate_denom
+            const int rateDenom = sbufReadU8(src); // was rate_denom
+            if (sbufBytesRemaining(src) >= 1) {
+                // p_denom specified, so use it directly
                 blackboxConfigMutable()->p_denom = sbufReadU8(src);
+            } else {
+                // p_denom not specified in MSP, so calculate it from old rateNum and rateDenom
+                blackboxConfigMutable()->p_denom = blackboxCalculatePDenom(rateNum, rateDenom);
+            }
         }
         break;
 #endif

src/test/unit/blackbox_unittest.cc

@@ -199,7 +199,7 @@ TEST(BlackboxTest, Test_2kHz)
 TEST(BlackboxTest, Test_8kHz)
 {
     blackboxConfigMutable()->p_denom = 32;
-    // 1kHz PIDloop
+    // 8kHz PIDloop
     gyro.targetLooptime = 125;
     blackboxInit();
     EXPECT_EQ(true, blackboxShouldLogIFrame());
@@ -224,7 +224,7 @@ TEST(BlackboxTest, Test_8kHz)
 TEST(BlackboxTest, Test_zero_p_denom)
 {
     blackboxConfigMutable()->p_denom = 0;
-    // 2kHz PIDloop
+    // 1kHz PIDloop
     gyro.targetLooptime = 1000;
     blackboxInit();
     EXPECT_EQ(32, blackboxIInterval);
@@ -242,6 +242,106 @@ TEST(BlackboxTest, Test_zero_p_denom)
     EXPECT_EQ(false, blackboxShouldLogPFrame());
 }
 
+TEST(BlackboxTest, Test_CalculatePDenom)
+{
+    blackboxConfigMutable()->p_denom = 0;
+    // note I-frame is logged every 32ms regardless of PIDloop rate
+    // so p_denom is 32 when blackbox logging rate is 1kHz
+
+    // 1kHz PIDloop
+    gyro.targetLooptime = 1000;
+    blackboxInit();
+    EXPECT_EQ(32, blackboxIInterval);
+    EXPECT_EQ(32, blackboxCalculatePDenom(1, 1)); // 1kHz logging
+    EXPECT_EQ(16, blackboxCalculatePDenom(1, 2));
+    EXPECT_EQ(8, blackboxCalculatePDenom(1, 4));
+
+    // 2kHz PIDloop
+    gyro.targetLooptime = 500;
+    blackboxInit();
+    EXPECT_EQ(64, blackboxIInterval);
+    EXPECT_EQ(64, blackboxCalculatePDenom(1, 1));
+    EXPECT_EQ(32, blackboxCalculatePDenom(1, 2)); // 1kHz logging
+    EXPECT_EQ(16, blackboxCalculatePDenom(1, 4));
+
+    // 4kHz PIDloop
+    gyro.targetLooptime = 250;
+    blackboxInit();
+    EXPECT_EQ(128, blackboxIInterval);
+    EXPECT_EQ(128, blackboxCalculatePDenom(1, 1));
+    EXPECT_EQ(64, blackboxCalculatePDenom(1, 2));
+    EXPECT_EQ(32, blackboxCalculatePDenom(1, 4)); // 1kHz logging
+    EXPECT_EQ(16, blackboxCalculatePDenom(1, 8));
+
+    // 8kHz PIDloop
+    gyro.targetLooptime = 125;
+    blackboxInit();
+    EXPECT_EQ(256, blackboxIInterval);
+    EXPECT_EQ(256, blackboxCalculatePDenom(1, 1));
+    EXPECT_EQ(128, blackboxCalculatePDenom(1, 2));
+    EXPECT_EQ(64, blackboxCalculatePDenom(1, 4));
+    EXPECT_EQ(32, blackboxCalculatePDenom(1, 8)); // 1kHz logging
+    EXPECT_EQ(16, blackboxCalculatePDenom(1, 16));
+}
+
+TEST(BlackboxTest, Test_CalculateRates)
+{
+    // 1kHz PIDloop
+    gyro.targetLooptime = 1000;
+    blackboxConfigMutable()->p_denom = 32;
+    blackboxInit();
+    EXPECT_EQ(32, blackboxIInterval);
+    EXPECT_EQ(1, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(1, blackboxGetRateDenom());
+
+    blackboxConfigMutable()->p_denom = 16;
+    blackboxInit();
+    EXPECT_EQ(32, blackboxIInterval);
+    EXPECT_EQ(2, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(2, blackboxGetRateDenom());
+
+    blackboxConfigMutable()->p_denom = 8;
+    blackboxInit();
+    EXPECT_EQ(32, blackboxIInterval);
+    EXPECT_EQ(4, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(4, blackboxGetRateDenom());
+
+
+    // 8kHz PIDloop
+    gyro.targetLooptime = 125;
+    blackboxConfigMutable()->p_denom = 32; // 1kHz logging
+    blackboxInit();
+    EXPECT_EQ(256, blackboxIInterval);
+    EXPECT_EQ(8, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(8, blackboxGetRateDenom());
+
+    blackboxConfigMutable()->p_denom = 64; // 2kHz logging
+    blackboxInit();
+    EXPECT_EQ(256, blackboxIInterval);
+    EXPECT_EQ(4, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(4, blackboxGetRateDenom());
+
+    blackboxConfigMutable()->p_denom = 128; // 4kHz logging
+    blackboxInit();
+    EXPECT_EQ(256, blackboxIInterval);
+    EXPECT_EQ(2, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(2, blackboxGetRateDenom());
+
+    blackboxConfigMutable()->p_denom = 256; // 8kHz logging
+    blackboxInit();
+    EXPECT_EQ(256, blackboxIInterval);
+    EXPECT_EQ(1, blackboxPInterval);
+    EXPECT_EQ(1, blackboxGetRateNum());
+    EXPECT_EQ(1, blackboxGetRateDenom());
+}
+
+
 // STUBS
 extern "C" {