Skip to content

Commit

Permalink
clean up algorithms
Browse files Browse the repository at this point in the history
  • Loading branch information
@PonomarevDA
PonomarevDA committed Jun 4, 2024
1 parent 6b507fb commit b1a2339
Show file tree
Hide file tree
Showing 3 changed files with 124 additions and 33 deletions.
31 changes: 11 additions & 20 deletions Src/common/algorithms.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2022-2023 Dmitry Ponomarev <[email protected]>
* Copyright (C) 2022-2024 Dmitry Ponomarev <[email protected]>
* 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/.
Expand Down Expand Up @@ -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;
Expand All @@ -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;
}
39 changes: 26 additions & 13 deletions Src/common/algorithms.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2022-2023 Dmitry Ponomarev <[email protected]>
* Copyright (C) 2022-2024 Dmitry Ponomarev <[email protected]>
* 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/.
Expand All @@ -17,37 +17,50 @@ 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,
PwmDurationUs max_pwm,
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,
PwmDurationUs max_pwm,
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
Expand Down
87 changes: 87 additions & 0 deletions Tests/common/test_algorithms.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
/**
* This program is free software under the GNU General Public License v3.
* See <https://www.gnu.org/licenses/> for details.
* Author: Dmitry Ponomarev <[email protected]>
*/

#include <gtest/gtest.h>
#include <cmath> // For std::fabs
#include <algorithm> // 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();
}

0 comments on commit b1a2339

Please sign in to comment.