5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

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
This commit is contained in:
Andy Piper 2022-02-10 14:41:44 +00:00 committed by Andrew Tridgell
parent a867e04116
commit ea1af70f2b
4 changed files with 100 additions and 0 deletions

View File

@ -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;
}

View File

@ -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

View File

@ -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; }

View File

@ -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()