Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: support either polarity in linear_interpolate()

Browse Source
This commit is contained in:
Andrew Tridgell 2021-05-19 09:19:52 +10:00
parent ddfc9e8300
commit 154ae64e63
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Math/AP_Math.cpp
View File

@ -82,6 +82,16 @@ template float safe_sqrt<short>(const short v);
template float safe_sqrt<float>(const float v);
template float safe_sqrt<double>(const double v);
/*
replacement for std::swap() needed for STM32
*/
static void swap_float(float &f1, float &f2)
{
float tmp = f1;
f1 = f2;
f2 = tmp;
}
/*
* linear interpolation based on a variable in a range
*/
@ -89,6 +99,11 @@ float linear_interpolate(float low_output, float high_output,
float var_value,
float var_low, float var_high)
{
if (var_low > var_high) {
// support either polarity
swap_float(var_low, var_high);
swap_float(low_output, high_output);
}
if (var_value <= var_low) {
return low_output;
}

3
libraries/AP_Math/AP_Math.h
View File

@ -248,6 +248,9 @@ inline constexpr uint32_t usec_to_hz(uint32_t usec)
/*
linear interpolation based on a variable in a range
return value will be in the range [var_low,var_high]
Either polarity is supported, so var_low can be higher than var_high
*/
float linear_interpolate(float low_output, float high_output,
float var_value,