From b1a2339e3b43bb796daa60c6da9cb6b8853668ff Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 3 Jun 2024 00:07:15 +0300 Subject: [PATCH] clean up algorithms --- Src/common/algorithms.cpp | 31 ++++-------- Src/common/algorithms.hpp | 39 +++++++++----- Tests/common/test_algorithms.cpp | 87 ++++++++++++++++++++++++++++++++ 3 files changed, 124 insertions(+), 33 deletions(-) create mode 100644 Tests/common/test_algorithms.cpp diff --git a/Src/common/algorithms.cpp b/Src/common/algorithms.cpp index 7cc828d..0af75db 100644 --- a/Src/common/algorithms.cpp +++ b/Src/common/algorithms.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022-2023 Dmitry Ponomarev + * Copyright (C) 2022-2024 Dmitry Ponomarev * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. @@ -37,6 +37,16 @@ PwmDurationUs mapFloatCommandToPwm(float command, return pwm; } +float mapFloat(float value, float in_min, float in_max, float out_min, float out_max) { + if (std::fabs(in_min - in_max) < 1e-6f) { + return out_min; + } + + float output = out_min + (value - in_min) / (in_max - in_min) * (out_max - out_min); + + return std::clamp(output, std::min(out_min, out_max), std::max(out_min, out_max)); +} + float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max) { auto max = pwm_max; auto min = pwm_min; @@ -48,22 +58,3 @@ float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max) { auto val = std::clamp(scaled_val, 0.f, 100.f); return val; } - -float mapFloat(float value, float in_min, float in_max, float out_min, float out_max) { - float output; - if (value <= in_min && in_min <= in_max) { - output = out_min; - } else if (value >= in_max && in_min <= in_max) { - output = out_max; - } else if (fabs(out_min - out_max) < 0.001) { - output = out_min; - } else { - output = out_min + (value - in_min) / (in_max - in_min) * (out_max - out_min); - if (out_min <= out_max) { - output = std::clamp(output, out_min, out_max); - } else { - output = std::clamp(output, out_max, out_min); - } - } - return output; -} diff --git a/Src/common/algorithms.hpp b/Src/common/algorithms.hpp index 69610a4..cd19737 100644 --- a/Src/common/algorithms.hpp +++ b/Src/common/algorithms.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022-2023 Dmitry Ponomarev + * Copyright (C) 2022-2024 Dmitry Ponomarev * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. @@ -17,10 +17,13 @@ extern "C" { typedef uint16_t PwmDurationUs; /** - * @brief Map raw command value (in interval from 0 to 8191) - * to PWM duration in us (in interval from min to max) - * @return pwm_duration if input is correct, - * def_pwm if raw_command value is less than min or higher than max + * @brief Maps a 16-bit integer command to a PWM duration. + * + * This function takes a 16-bit integer command and maps it to a PWM duration in microseconds. + * If the command is outside the valid range [0, 8191], the default PWM duration is returned. + * Otherwise, the command is linearly mapped to the specified PWM range. + * + * @note Suitable for esc.RawCommand mapping */ PwmDurationUs mapInt16CommandToPwm(int16_t rc_value, PwmDurationUs min_pwm, @@ -28,10 +31,13 @@ PwmDurationUs mapInt16CommandToPwm(int16_t rc_value, PwmDurationUs def_pwm); /** - * @brief Map array command value (in interval from -1.0 to 1.0) - * to PWM duration in us (in interval from min to max) - * @return pwm_duration if input is correct, - * def_pwm if raw_command value is less than min or higher than max + * @brief Maps a float command to a PWM duration. + * + * This function takes a float command and maps it to a PWM duration in microseconds. + * If the command is outside the valid range [-1.0, 1.0], the default PWM duration is returned. + * Otherwise, the command is linearly mapped to the specified PWM range. + * + * @note Suitable for actuator.Command or ratiometric setpoint mapping */ PwmDurationUs mapFloatCommandToPwm(float ac_value, PwmDurationUs min_pwm, @@ -39,15 +45,22 @@ PwmDurationUs mapFloatCommandToPwm(float ac_value, PwmDurationUs def_pwm); /** - * @brief Map PWM duration in us (in interval from min to max) to pct - * @return pwm_duration in pct + * @brief Maps a value from one range to another. + * + * This function takes an input float value and linearly maps it from an input range + * [in_min, in_max] to an output range [out_min, out_max]. If the input value is outside + * the input range, the output value will be clamped to the nearest boundary of the output range. */ -float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max); - float mapFloat(float value, float in_min, float in_max, float out_min, float out_max); +/** + * @brief Map PWM duration in us (in interval from min to max) to pct + * @return pwm_duration in pct + */ +float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max); + #ifdef __cplusplus } #endif diff --git a/Tests/common/test_algorithms.cpp b/Tests/common/test_algorithms.cpp new file mode 100644 index 0000000..528d2b8 --- /dev/null +++ b/Tests/common/test_algorithms.cpp @@ -0,0 +1,87 @@ +/** + * This program is free software under the GNU General Public License v3. + * See for details. + * Author: Dmitry Ponomarev + */ + +#include +#include // For std::fabs +#include // For std::clamp + +static constexpr auto ABS_ERR = 0.1f; + +float mapFloat(float value, float in_min, float in_max, float out_min, float out_max) { + if (std::fabs(in_min - in_max) < 1e-6f) { + return out_min; + } + + float output = out_min + (value - in_min) / (in_max - in_min) * (out_max - out_min); + + return std::clamp(output, std::min(out_min, out_max), std::max(out_min, out_max)); +} + +TEST(MapFloatTest, raw_command_esc) { + static constexpr auto IN_MIN = 0; + static constexpr auto IN_MAX = 8191; + static constexpr auto OUT_MIN = 1000; + static constexpr auto OUT_MAX = 2000; + + EXPECT_NEAR(mapFloat(-1, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(0, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(4095, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), 1500.0f, ABS_ERR); + EXPECT_NEAR(mapFloat(8191, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); + EXPECT_NEAR(mapFloat(8192, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); +} + +TEST(MapFloatTest, raw_command_servo_inverted_with_ofsset) { + static constexpr auto IN_MIN = 0; + static constexpr auto IN_MAX = 8191; + static constexpr auto OUT_MIN = 1600; + static constexpr auto OUT_MAX = 1800; + + EXPECT_NEAR(mapFloat(-1, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(0, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(4095, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), 1700.0f, ABS_ERR); + EXPECT_NEAR(mapFloat(8191, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); + EXPECT_NEAR(mapFloat(8192, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); +} + +TEST(MapFloatTest, actuator_command_servo) { + static constexpr auto IN_MIN = -1.0f; + static constexpr auto IN_MAX = +1.0f; + static constexpr auto OUT_MIN = 1200; + static constexpr auto OUT_MAX = 1400; + + EXPECT_NEAR(mapFloat(-1.01f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(-1.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(0.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), 1300.0f, ABS_ERR); + EXPECT_NEAR(mapFloat(+1.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); + EXPECT_NEAR(mapFloat(+1.01f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); +} + +TEST(MapFloatTest, inveted_air_servo_mapping) { + static constexpr auto IN_MIN = 2000.0f; + static constexpr auto IN_MAX = 1000.0f; + static constexpr auto OUT_MIN = 1700; + static constexpr auto OUT_MAX = 1900; + + EXPECT_NEAR(mapFloat(2001.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(2000.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MIN, ABS_ERR); + EXPECT_NEAR(mapFloat(1750.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), 1750.0f, ABS_ERR); + EXPECT_NEAR(mapFloat(1250.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), 1850.0f, ABS_ERR); + EXPECT_NEAR(mapFloat(1000.0f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); + EXPECT_NEAR(mapFloat(999.00f, IN_MIN, IN_MAX, OUT_MIN, OUT_MAX), OUT_MAX, ABS_ERR); +} + +TEST(MapFloatTest, wrong_input) { + EXPECT_NEAR(mapFloat(1500, 1000, 1000, 1000, 2000), 1000, ABS_ERR); +} + +TEST(MapFloatTest, wrong_output) { + EXPECT_NEAR(mapFloat(0.5f, 0.0f, 1.0f, 1000, 1000), 1000, ABS_ERR); +} + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}