mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 12:43:33 +08:00
Rename math::gradual() to math::interpolate() and add unit tests to cover additional corner cases.
This commit is contained in:
mcsauder
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
021b23826d
commit
87a5705960
@@ -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) {
|
||||
|
||||
@@ -149,7 +149,7 @@ const T expo_deadzone(const T &value, const T &e, const T &dz)
|
||||
* x_low x_high
|
||||
*/
|
||||
template<typename T>
|
||||
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<typename T>
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user