diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 8e90bd670d..f10918659f 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -319,6 +319,21 @@ public: */ static uint32_t calculate_bitrate_prescaler(uint32_t timer_clock, uint32_t target_frequency, bool is_dshot); + /* + * bit width values for different protocols + */ + static constexpr uint32_t DSHOT_BIT_WIDTH_TICKS = 8; + static constexpr uint32_t DSHOT_BIT_0_TICKS = 3; + static constexpr uint32_t DSHOT_BIT_1_TICKS = 6; + + // See WS2812B spec for expected pulse widths + static constexpr uint32_t NEOP_BIT_WIDTH_TICKS = 20; + static constexpr uint32_t NEOP_BIT_0_TICKS = 7; + static constexpr uint32_t NEOP_BIT_1_TICKS = 14; + // neopixel does not use pulse widths at all + static constexpr uint32_t PROFI_BIT_0_TICKS = 7; + static constexpr uint32_t PROFI_BIT_1_TICKS = 14; + protected: // helper functions for implementation of get_output_mode_banner diff --git a/libraries/AP_HAL/tests/test_prescaler.cpp b/libraries/AP_HAL/tests/test_prescaler.cpp index 6e9bfb6070..68125a2a10 100644 --- a/libraries/AP_HAL/tests/test_prescaler.cpp +++ b/libraries/AP_HAL/tests/test_prescaler.cpp @@ -11,24 +11,41 @@ protected: const uint32_t prescaler = AP_HAL::RCOutput::calculate_bitrate_prescaler(clock, target_rate, is_dshot); // we would like at most a 1% discrepancy in target versus actual const float rate_delta = fabsf(float(clock / prescaler) - target_rate) / target_rate; - EXPECT_TRUE(rate_delta < 0.13f); + // hack because CUAV_GPS does not support dshot + const float expected_delta = clock > 50000000 ? 0.13f : 0.35f; + EXPECT_TRUE(rate_delta < expected_delta); + } + + void test_prescaler_neopixel(uint32_t clock) + { + const uint32_t target_rate = 800000 * AP_HAL::RCOutput::NEOP_BIT_WIDTH_TICKS; + const uint32_t prescaler = AP_HAL::RCOutput::calculate_bitrate_prescaler(clock, target_rate, false); + const uint32_t actual_rate = clock / prescaler; + + const float bit_1_width_us = 1000000.0f * AP_HAL::RCOutput::NEOP_BIT_1_TICKS / actual_rate; + const float bit_0_width_us = 1000000.0f * AP_HAL::RCOutput::NEOP_BIT_0_TICKS / actual_rate; + + // timing requirements from WS2812B spec + EXPECT_TRUE(bit_1_width_us < (0.85f + 0.15f) && bit_1_width_us > (0.85f - 0.15f)); + EXPECT_TRUE(bit_0_width_us < (0.4f + 0.15f) && bit_0_width_us > (0.4f - 0.15f)); + EXPECT_TRUE((bit_0_width_us + bit_1_width_us) < (1.25f + 0.6f) && (bit_0_width_us + bit_1_width_us) > (1.25f - 0.6f)); } }; TEST_P(PrescalerParameterizedTestFixture, DShot150) { - test_prescaler(GetParam(), 150000 * 8, true); + test_prescaler(GetParam(), 150000 * AP_HAL::RCOutput::DSHOT_BIT_WIDTH_TICKS, true); } TEST_P(PrescalerParameterizedTestFixture, DShot300) { - test_prescaler(GetParam(), 300000 * 8, true); + test_prescaler(GetParam(), 300000 * AP_HAL::RCOutput::DSHOT_BIT_WIDTH_TICKS, true); } TEST_P(PrescalerParameterizedTestFixture, DShot600) { - test_prescaler(GetParam(), 600000 * 8, true); + test_prescaler(GetParam(), 600000 * AP_HAL::RCOutput::DSHOT_BIT_WIDTH_TICKS, true); } TEST_P(PrescalerParameterizedTestFixture, DShot1200) { - test_prescaler(GetParam(), 1200000 * 8, true); + test_prescaler(GetParam(), 1200000 * AP_HAL::RCOutput::DSHOT_BIT_WIDTH_TICKS, true); } TEST_P(PrescalerParameterizedTestFixture, Passthrough) { @@ -36,11 +53,11 @@ TEST_P(PrescalerParameterizedTestFixture, Passthrough) { } TEST_P(PrescalerParameterizedTestFixture, NeoPixel) { - test_prescaler(GetParam(), 800000 * 11, false); + test_prescaler_neopixel(GetParam()); } TEST_P(PrescalerParameterizedTestFixture, ProfiLED) { - test_prescaler(GetParam(), 1500000 * 11, false); + test_prescaler(GetParam(), 1500000 * AP_HAL::RCOutput::NEOP_BIT_WIDTH_TICKS, false); } INSTANTIATE_TEST_CASE_P( @@ -51,7 +68,8 @@ INSTANTIATE_TEST_CASE_P( 216000000, // F745 108000000, // F745 84000000, // F405 - 168000000 // F405 + 168000000, // F405 + 50000000 // CUAV_GPS )); AP_GTEST_MAIN() \ No newline at end of file