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

consume all available queued vehicle_commands

Browse Source
This commit is contained in:
Daniel Agar
2021-01-26 12:00:18 -05:00
parent d5c7e243a9
commit d1dfa26903
11 changed files with 167 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+13 -17
View File
@@ -162,9 +162,19 @@ FixedwingPositionControl::vehicle_control_mode_poll()
void
FixedwingPositionControl::vehicle_command_poll()
{
if (_vehicle_command_sub.updated()) {
_vehicle_command_sub.copy(&_vehicle_command);
handle_command();
vehicle_command_s vehicle_command;
while (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// only abort landing before point of no return (horizontal and vertical)
if (_control_mode.flag_control_auto_enabled &&
_pos_sp_triplet.current.valid &&
(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
abort_landing(true);
}
}
}
}
@@ -1597,20 +1607,6 @@ FixedwingPositionControl::get_tecs_thrust()
return 0.0f;
}
void
FixedwingPositionControl::handle_command()
{
if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// only abort landing before point of no return (horizontal and vertical)
if (_control_mode.flag_control_auto_enabled &&
_pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
abort_landing(true);
}
}
}
void
FixedwingPositionControl::Run()
{