fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: switch order of setpoint amendment

Browse Source 
to make sure the takeoff limitation is always done last.
This commit is contained in:
Matthias Grob 2020-03-09 09:16:34 +01:00
parent a8063ac948
commit 1f98ebdb47
1 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -593,6 +593,16 @@ MulticopterPositionControl::Run()
constraints.speed_up = _param_mpc_z_vel_max_up.get();
}
// limit tilt during takeoff ramupup
if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}
// limit altitude only if local position is valid
if (PX4_ISFINITE(_states.position(2))) {
limit_altitude(setpoint);
}
// handle smooth takeoff
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff,
constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
@ -612,15 +622,6 @@ MulticopterPositionControl::Run()
_flight_tasks.reActivate();
}
if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}
// limit altitude only if local position is valid
if (PX4_ISFINITE(_states.position(2))) {
limit_altitude(setpoint);
}
// Run position control
_control.setState(_states);
_control.setConstraints(constraints);