fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:23:33 +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);
}
}