From 87a5705960246f7d5e5b03e545c9e1603fc5478b Mon Sep 17 00:00:00 2001 From: mcsauder Date: Mon, 15 Aug 2022 14:25:02 -0600 Subject: [PATCH] Rename math::gradual() to math::interpolate() and add unit tests to cover additional corner cases. --- src/lib/battery/battery.cpp | 2 +- src/lib/mathlib/math/Functions.hpp | 12 +-- src/lib/mathlib/math/FunctionsTest.cpp | 99 ++++++++++--------- .../tasks/Auto/FlightTaskAuto.cpp | 6 +- .../FlightTaskManualAltitude.cpp | 14 +-- .../FixedwingAttitudeControl.cpp | 29 +++--- .../mc_att_control/mc_att_control_main.cpp | 8 +- .../battery_simulator/BatterySimulator.cpp | 3 +- 8 files changed, 93 insertions(+), 80 deletions(-) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index c01b018dc2..dbb50e7dd3 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -215,7 +215,7 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a cell_voltage += throttle * _params.v_load_drop; } - _state_of_charge_volt_based = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f); + _state_of_charge_volt_based = math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f); // choose which quantity we're using for final reporting if (_params.capacity > 0.f && _battery_initialized) { diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 13de8e8ad7..401f0d1a73 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -149,7 +149,7 @@ const T expo_deadzone(const T &value, const T &e, const T &dz) * x_low x_high */ template -const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high) +const T interpolate(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high) { if (value <= x_low) { return y_low; @@ -178,15 +178,15 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, * x_low x_middle x_high */ template -const T gradual3(const T &value, - const T &x_low, const T &x_middle, const T &x_high, - const T &y_low, const T &y_middle, const T &y_high) +const T interpolate3(const T &value, + const T &x_low, const T &x_middle, const T &x_high, + const T &y_low, const T &y_middle, const T &y_high) { if (value < x_middle) { - return gradual(value, x_low, x_middle, y_low, y_middle); + return interpolate(value, x_low, x_middle, y_low, y_middle); } else { - return gradual(value, x_middle, x_high, y_middle, y_high); + return interpolate(value, x_middle, x_high, y_middle, y_high); } } diff --git a/src/lib/mathlib/math/FunctionsTest.cpp b/src/lib/mathlib/math/FunctionsTest.cpp index 48ac63fcec..b6ed95382f 100644 --- a/src/lib/mathlib/math/FunctionsTest.cpp +++ b/src/lib/mathlib/math/FunctionsTest.cpp @@ -140,63 +140,72 @@ TEST(FunctionsTest, expo_deadzone) EXPECT_FLOAT_EQ(expo_deadzone(-.5f, .9f, .4f), -.020833336f); } -TEST(FunctionsTest, gradual) +TEST(FunctionsTest, interpolate) { // factor of *2, offset +1 - EXPECT_FLOAT_EQ(gradual(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f); - EXPECT_FLOAT_EQ(gradual(0.f, 0.f, 1.f, 1.f, 3.f), 1.f); - EXPECT_FLOAT_EQ(gradual(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f); - EXPECT_FLOAT_EQ(gradual(.5f, 0.f, 1.f, 1.f, 3.f), 2.f); - EXPECT_FLOAT_EQ(gradual(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f); - EXPECT_FLOAT_EQ(gradual(1.f, 0.f, 1.f, 1.f, 3.f), 3.f); - EXPECT_FLOAT_EQ(gradual(12.f, 0.f, 1.f, 1.f, 3.f), 3.f); + EXPECT_FLOAT_EQ(interpolate(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f); + EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 1.f, 1.f, 3.f), 1.f); + EXPECT_FLOAT_EQ(interpolate(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f); + EXPECT_FLOAT_EQ(interpolate(.5f, 0.f, 1.f, 1.f, 3.f), 2.f); + EXPECT_FLOAT_EQ(interpolate(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f); + EXPECT_FLOAT_EQ(interpolate(1.f, 0.f, 1.f, 1.f, 3.f), 3.f); + EXPECT_FLOAT_EQ(interpolate(12.f, 0.f, 1.f, 1.f, 3.f), 3.f); // factor of *2, offset +3 - EXPECT_FLOAT_EQ(gradual(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f); - EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 2.f, 4.f, 6.f), 4.f); - EXPECT_FLOAT_EQ(gradual(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f); - EXPECT_FLOAT_EQ(gradual(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f); - EXPECT_FLOAT_EQ(gradual(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f); - EXPECT_FLOAT_EQ(gradual(2.f, 1.f, 2.f, 4.f, 6.f), 6.f); - EXPECT_FLOAT_EQ(gradual(12.f, 1.f, 2.f, 4.f, 6.f), 6.f); + EXPECT_FLOAT_EQ(interpolate(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f); + EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 2.f, 4.f, 6.f), 4.f); + EXPECT_FLOAT_EQ(interpolate(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f); + EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f); + EXPECT_FLOAT_EQ(interpolate(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f); + EXPECT_FLOAT_EQ(interpolate(2.f, 1.f, 2.f, 4.f, 6.f), 6.f); + EXPECT_FLOAT_EQ(interpolate(12.f, 1.f, 2.f, 4.f, 6.f), 6.f); // corner case when x_low == x_high == value - EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 1.f, 4.f, 6.f), 4.f); + EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f); // corner case when x_low > x_high - EXPECT_FLOAT_EQ(gradual(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f); + EXPECT_FLOAT_EQ(interpolate(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f); + + // corner case when y_low == y_high == value + EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f); + + // corner case when y_low > y_high + EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 6.f, 4.f), 5.f); + + // corner case when x_low == x_high == y_low == y_high == 0.f + EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 0.f, 0.f, 0.f), 0.f); } -TEST(FunctionsTest, gradual3) +TEST(FunctionsTest, interpolate3) { // factor of *2, offset +1 - EXPECT_FLOAT_EQ(gradual3(-12.f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 1.f); - EXPECT_FLOAT_EQ(gradual3(0.f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 1.f); - EXPECT_FLOAT_EQ(gradual3(.25f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 1.5f); - EXPECT_FLOAT_EQ(gradual3(.5f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 2.f); - EXPECT_FLOAT_EQ(gradual3(.75f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 2.25f); - EXPECT_FLOAT_EQ(gradual3(1.f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 2.5f); - EXPECT_FLOAT_EQ(gradual3(1.25f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 2.75f); - EXPECT_FLOAT_EQ(gradual3(1.5f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 3.f); - EXPECT_FLOAT_EQ(gradual3(12.f, - 0.f, .5f, 1.5f, - 1.f, 2.f, 3.f), 3.f); + EXPECT_FLOAT_EQ(interpolate3(-12.f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 1.f); + EXPECT_FLOAT_EQ(interpolate3(0.f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 1.f); + EXPECT_FLOAT_EQ(interpolate3(.25f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 1.5f); + EXPECT_FLOAT_EQ(interpolate3(.5f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 2.f); + EXPECT_FLOAT_EQ(interpolate3(.75f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 2.25f); + EXPECT_FLOAT_EQ(interpolate3(1.f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 2.5f); + EXPECT_FLOAT_EQ(interpolate3(1.25f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 2.75f); + EXPECT_FLOAT_EQ(interpolate3(1.5f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 3.f); + EXPECT_FLOAT_EQ(interpolate3(12.f, + 0.f, .5f, 1.5f, + 1.f, 2.f, 3.f), 3.f); } TEST(FunctionsTest, sqrt_linear) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 09f8acf2fc..ec7da58067 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -223,9 +223,9 @@ void FlightTaskAuto::_prepareLandSetpoints() _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint // Slow down automatic descend close to ground - float vertical_speed = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()); + float vertical_speed = math::interpolate(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()); bool range_dist_available = PX4_ISFINITE(_dist_to_bottom); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 243b59dcce..afd729152c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -273,14 +273,14 @@ void FlightTaskManualAltitude::_respectMaxAltitude() void FlightTaskManualAltitude::_respectGroundSlowdown() { - // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 + // Interpolate descent rate between the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 if (PX4_ISFINITE(_dist_to_ground)) { - const float limit_down = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _constraints.speed_down); - const float limit_up = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_tko_speed.get(), _constraints.speed_up); + const float limit_down = math::interpolate(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _constraints.speed_down); + const float limit_up = math::interpolate(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_tko_speed.get(), _constraints.speed_up); _velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down); } } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 1feee6bbf7..9a2215d86d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -35,7 +35,7 @@ using namespace time_literals; using math::constrain; -using math::gradual; +using math::interpolate; using math::radians; FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : @@ -488,20 +488,23 @@ void FixedwingAttitudeControl::Run() float trim_yaw = _param_trim_yaw.get(); if (airspeed < _param_fw_airspd_trim.get()) { - trim_roll += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(), - 0.0f); - trim_pitch += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(), - 0.0f); - trim_yaw += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(), - 0.0f); + trim_roll += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim_pitch += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim_yaw += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_y_vmin.get(), + 0.0f); } else { - trim_roll += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_r_vmax.get()); - trim_pitch += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_p_vmax.get()); - trim_yaw += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_y_vmax.get()); + trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); } /* add trim increment if flaps are deployed */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index d783c21bba..4f6c782915 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -99,12 +99,12 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) // throttle_stick_input is in range [0, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle - return math::gradual(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get()); + return math::interpolate(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get()); default: // 0 or other: rescale to hover throttle at 0.5 stick - return math::gradual3(throttle_stick_input, - 0.f, .5f, 1.f, - _param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()); + return math::interpolate3(throttle_stick_input, + 0.f, .5f, 1.f, + _param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()); } } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index bb765e637f..54588eac94 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -97,7 +97,8 @@ void BatterySimulator::Run() float ibatt = -1.0f; // no current sensor in simulation _battery_percentage = math::max(_battery_percentage, _param_bat_min_pct.get() / 100.f); - float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); + float vbatt = math::interpolate(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), + _battery.full_cell_voltage()); vbatt *= _battery.cell_count(); _battery.setConnected(true);