From 3b7d31de754b58f8a8b5a074860ec57c8af8dc3f Mon Sep 17 00:00:00 2001 From: MaEtUgR Date: Tue, 26 Jun 2018 15:03:32 +0200 Subject: [PATCH] FlightTasks: reintroduce vehicle command handling The handling for vehicle commands inside the FlightTasks library was already there but commented out because the previously used MAVLink messages were tied to the specific application we used and there was no clean common message definition. Because there is now a well defined message for the orbit task I'm uncommenting and using the working message handling again here. --- src/lib/FlightTasks/FlightTasks.cpp | 122 ++++++++++++++-------------- src/lib/FlightTasks/FlightTasks.hpp | 4 +- 2 files changed, 65 insertions(+), 61 deletions(-) diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index b295c4f37a..ef6ba69b50 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -184,63 +184,67 @@ const char *FlightTasks::errorToString(const int error) void FlightTasks::_updateCommand() { -// TODO: port flight task mavlink command to have support for this functionality -// /* lazy subscription to command topic */ -// if (_sub_vehicle_command < 0) { -// _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); -// } -// -// /* check if there's any new command */ -// bool updated = false; -// orb_check(_sub_vehicle_command, &updated); -// -// if (!updated) { -// return; -// } -// -// /* check if command is for flight task library */ -// struct vehicle_command_s command; -// orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); -// -// if (command.command != vehicle_command_s::VEHICLE_CMD_FLIGHT_TASK) { -// return; -// } -// -// /* evaluate command */ -// // TODO: remapping of the Mavlink definition to the internal tasks necessary -// int switch_result = switchTask(int(command.param1 + 0.5f)); -// uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; -// -// /* if we are in the desired task */ -// if (switch_result >= 0) { -// 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 we just switched and parameters are not accepted, go to failsafe */ -// if (switch_result == 1) { -// switchTask(-1); -// 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 = switch_result; -// command_ack.target_system = command.source_system; -// command_ack.target_component = command.source_component; -// -// if (_pub_vehicle_command_ack == nullptr) { -// _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, -// vehicle_command_ack_s::ORB_QUEUE_LENGTH); -// -// } else { -// orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); -// -// } + // lazy subscription to command topic + if (_sub_vehicle_command < 0) { + _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); + } + + // check if there's any new command + bool updated = false; + orb_check(_sub_vehicle_command, &updated); + + if (!updated) { + return; + } + + // get command + struct vehicle_command_s command; + orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); + + // check what command it is + FlightTaskIndex desired_task = FlightTaskIndex::None; + switch(command.command) { + case vehicle_command_s::VEHICLE_CMD_DO_ORBIT : + desired_task = FlightTaskIndex::Orbit; + break; + // ignore all unkown commands + default : return; + } + + // switch to the commanded task + int 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 >= 0) { + 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 we just switched and parameters are not accepted, go to failsafe + if (switch_result == 1) { + switchTask(FlightTaskIndex::Position); + 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 = switch_result; + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + + if (_pub_vehicle_command_ack == nullptr) { + _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); + + } } diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index c7d57a7367..6cea623953 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -188,8 +188,8 @@ private: * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them */ void _updateCommand(); + int _sub_vehicle_command = -1; /**< topic handle on which commands are received */ + orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */ int _initTask(FlightTaskIndex task_index); -// int _sub_vehicle_command = -1; /**< topic handle on which commands are received */ -// orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */ };