diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2b24b8afcf..ca6be9f3bf 100644 --- a/src/main/blackbox/blackbox.c +++ b/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. */ diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 58bfb3d27b..e7a256b62c 100644 --- a/src/main/blackbox/blackbox.h +++ b/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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 40ea8af722..c2cae2ccfe 100644 --- a/src/main/fc/fc_msp.c +++ b/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, 0); // was rate_denom + sbufWriteU8(dst, blackboxGetRateNum()); + 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 - sbufReadU8(src); // was rate_denom - blackboxConfigMutable()->p_denom = sbufReadU8(src); + const int rateNum = sbufReadU8(src); // was rate_num + 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 diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index 8ba0750b1c..2d0784f92a 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/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" {