Skip to content

Commit

Permalink
AP_Math: correct description of linear_interpolate
Browse files Browse the repository at this point in the history
the return-value comment was just flat-out wrong.

Fix the parameter naming to make it clearer what is going on
  • Loading branch information
peterbarker committed Jan 10, 2025
1 parent 6efe210 commit 00a35b2
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 17 deletions.
24 changes: 12 additions & 12 deletions libraries/AP_Math/AP_Math.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -98,23 +98,23 @@ static void swap_float(float &f1, float &f2)
/*
* linear interpolation based on a variable in a range
*/
float linear_interpolate(float low_output, float high_output,
float var_value,
float var_low, float var_high)
float linear_interpolate(float output_low, float output_high,
float input_value,
float input_low, float input_high)
{
if (var_low > var_high) {
if (input_low > input_high) {
// support either polarity
swap_float(var_low, var_high);
swap_float(low_output, high_output);
swap_float(input_low, input_high);
swap_float(output_low, output_high);
}
if (var_value <= var_low) {
return low_output;
if (input_value <= input_low) {
return output_low;
}
if (var_value >= var_high) {
return high_output;
if (input_value >= input_high) {
return output_high;
}
float p = (var_value - var_low) / (var_high - var_low);
return low_output + p * (high_output - low_output);
float p = (input_value - input_low) / (input_high - input_low);
return output_low + p * (output_high - output_low);
}

/* cubic "expo" curve generator
Expand Down
15 changes: 10 additions & 5 deletions libraries/AP_Math/AP_Math.h
Original file line number Diff line number Diff line change
Expand Up @@ -300,13 +300,18 @@ 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]
return value will be in the range [output_low,output_high]
Either polarity is supported, so var_low can be higher than var_high
Either polarity is supported, so input_low can be higher than input_high
Read this as, "Take the value 'input_value' as a value between
'input_low' and 'input_high'. Return a value between 'output_low'
and 'output_high' proportonal to the position of 'input_value'
between 'input_low' and 'input_high'"
*/
float linear_interpolate(float low_output, float high_output,
float var_value,
float var_low, float var_high);
float linear_interpolate(float output_low, float output_high,
float input_value,
float input_low, float input_high);

/* cubic "expo" curve generator
* alpha range: [0,1] min to max expo
Expand Down

0 comments on commit 00a35b2

Please sign in to comment.