fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 22:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rename math::gradual() to math::interpolate() and add unit tests to cover additional corner cases.

Browse Source
This commit is contained in:
mcsauder
2022-08-18 14:18:02 +02:00
committed by Mathieu Bresciani
parent 021b23826d
commit 87a5705960
8 changed files with 93 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+3 -3
View File
@@ -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);
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+7 -7
View File
@@ -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);
}
}
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+16 -13
View File
@@ -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 */
src/modules/mc_att_control/mc_att_control_main.cpp
+4 -4
View File
@@ -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());
}
}
src/modules/simulator/battery_simulator/BatterySimulator.cpp
+2 -1
View File
@@ -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);