From ea1af70f2be0c353bafbad6cf61c72e5178c0fce Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 10 Feb 2022 14:41:44 +0000
Subject: [PATCH] AP_HAL: add support for @SYS/timers.txt

move prescaler calculation here and add unit test
add ability to find closest matching frequency in prescaler calculation
account for bit widths in prescaler tests
---
 libraries/AP_HAL/RCOutput.cpp             | 32 +++++++++++++
 libraries/AP_HAL/RCOutput.h               |  9 ++++
 libraries/AP_HAL/Util.h                   |  2 +
 libraries/AP_HAL/tests/test_prescaler.cpp | 57 +++++++++++++++++++++++
 4 files changed, 100 insertions(+)
 create mode 100644 libraries/AP_HAL/tests/test_prescaler.cpp

diff --git a/libraries/AP_HAL/RCOutput.cpp b/libraries/AP_HAL/RCOutput.cpp
index a3891cffef..0b36cf1a2c 100644
--- a/libraries/AP_HAL/RCOutput.cpp
+++ b/libraries/AP_HAL/RCOutput.cpp
@@ -68,3 +68,35 @@ bool AP_HAL::RCOutput::is_dshot_protocol(const enum output_mode mode)
         return false;
     }
 }
+
+/*
+ * calculate the prescaler required to achieve the desire bitrate
+ */
+uint32_t AP_HAL::RCOutput::calculate_bitrate_prescaler(uint32_t timer_clock, uint32_t target_frequency, bool is_dshot)
+{
+    uint32_t prescaler;
+    if (is_dshot) {
+        // original prescaler calculation from betaflight. bi-dir dshot is incredibly sensitive to the bitrate
+        prescaler = uint32_t(lrintf((float) timer_clock / target_frequency + 0.01f) - 1);
+    } else {
+        // adjust frequency to give an allowed value given the clock, erring on the high side
+        prescaler = timer_clock / target_frequency;
+        while ((timer_clock / prescaler) < target_frequency && prescaler > 1) {
+            prescaler--;
+        }
+    }
+
+    // find the closest value
+    const uint32_t freq = timer_clock / prescaler;
+    const float delta = fabsf(float(freq) - target_frequency);
+    if (freq > target_frequency
+        && delta > fabsf(float(timer_clock / (prescaler+1)) - target_frequency)) {
+        prescaler++;
+    } else if (freq < target_frequency
+        && delta > fabsf(float(timer_clock / (prescaler-1)) - target_frequency)) {
+        prescaler--;
+    }
+
+    return prescaler;
+}
+
diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h
index aa3110bfa6..8e90bd670d 100644
--- a/libraries/AP_HAL/RCOutput.h
+++ b/libraries/AP_HAL/RCOutput.h
@@ -31,6 +31,8 @@
 
 class ByteBuffer;
 
+class ExpandingString;
+
 class AP_HAL::RCOutput {
 public:
     virtual void init() = 0;
@@ -310,6 +312,13 @@ public:
      */
     virtual void serial_led_send(const uint16_t chan) {}
 
+    virtual void timer_info(ExpandingString &str) {}
+
+    /*
+     * calculate the prescaler required to achieve the desire bitrate
+     */
+    static uint32_t calculate_bitrate_prescaler(uint32_t timer_clock, uint32_t target_frequency, bool is_dshot);
+
 protected:
 
     // helper functions for implementation of get_output_mode_banner
diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h
index 8ebf397df1..b730acb1d5 100644
--- a/libraries/AP_HAL/Util.h
+++ b/libraries/AP_HAL/Util.h
@@ -184,6 +184,8 @@ public:
     // request information on uart I/O
     virtual void uart_info(ExpandingString &str) {}
 #endif
+    // request information on timer frequencies
+    virtual void timer_info(ExpandingString &str) {}
 
     // generate Random values
     virtual bool get_random_vals(uint8_t* data, size_t size) { return false; }
diff --git a/libraries/AP_HAL/tests/test_prescaler.cpp b/libraries/AP_HAL/tests/test_prescaler.cpp
new file mode 100644
index 0000000000..f741fb1c37
--- /dev/null
+++ b/libraries/AP_HAL/tests/test_prescaler.cpp
@@ -0,0 +1,57 @@
+#include <AP_gtest.h>
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/RCOutput.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+class PrescalerParameterizedTestFixture :public ::testing::TestWithParam<uint32_t> {
+protected:
+    void test_prescaler(uint32_t clock, uint32_t target_rate, bool is_dshot)
+    {
+        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);
+    }
+};
+
+TEST_P(PrescalerParameterizedTestFixture, DShot150) {
+    test_prescaler(GetParam(), 150000 * 20, true);
+}
+
+TEST_P(PrescalerParameterizedTestFixture, DShot300) {
+    test_prescaler(GetParam(), 300000 * 20, true);
+}
+
+TEST_P(PrescalerParameterizedTestFixture, DShot600) {
+    test_prescaler(GetParam(), 600000 * 20, true);
+}
+
+TEST_P(PrescalerParameterizedTestFixture, DShot1200) {
+    test_prescaler(GetParam(), 1200000 * 20, true);
+}
+
+TEST_P(PrescalerParameterizedTestFixture, Passthrough) {
+    test_prescaler(GetParam(), 19200 * 10, false);
+}
+
+TEST_P(PrescalerParameterizedTestFixture, NeoPixel) {
+    test_prescaler(GetParam(), 800000 * 20, false);
+}
+
+TEST_P(PrescalerParameterizedTestFixture, ProfiLED) {
+    test_prescaler(GetParam(), 1500000 * 20, false);
+}
+
+INSTANTIATE_TEST_CASE_P(
+        prescaler_Test,
+        PrescalerParameterizedTestFixture,
+        ::testing::Values(
+                200000000,  // H743
+                216000000,  // F745
+                108000000,  // F745
+                 84000000,  // F405
+                168000000   // F405
+        ));
+
+AP_GTEST_MAIN()
\ No newline at end of file