fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 14:40:35 +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-24 13:42:35 -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/flight_mode_manager/FlightModeManager.cpp
+30 -29
View File
@@ -440,43 +440,44 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
void FlightModeManager::handleCommand()
{
// get command
vehicle_command_s command{};
_vehicle_command_sub.copy(&command);
vehicle_command_s command;
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
while (_vehicle_command_sub.update(&command)) {
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
// ignore all unkown commands
if (desired_task != FlightTaskIndex::None) {
// switch to the commanded task
FlightTaskError switch_result = switchTask(desired_task);
uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// ignore all unkown commands
if (desired_task != FlightTaskIndex::None) {
// switch to the commanded task
FlightTaskError switch_result = switchTask(desired_task);
uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// if we are in/switched to the desired task
if (switch_result >= FlightTaskError::NoError) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
// if we are in/switched to the desired task
if (switch_result >= FlightTaskError::NoError) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
// if the task is running apply parameters to it and see if it rejects
if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// if the task is running apply parameters to it and see if it rejects
if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// if we just switched and parameters are not accepted, go to failsafe
if (switch_result >= FlightTaskError::NoError) {
switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// if we just switched and parameters are not accepted, go to failsafe
if (switch_result >= FlightTaskError::NoError) {
switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
}
}
}
// send back acknowledgment
vehicle_command_ack_s command_ack{};
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = static_cast<int>(switch_result);
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
// send back acknowledgment
vehicle_command_ack_s command_ack{};
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = static_cast<int>(switch_result);
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
}
}