diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 049634e22f31e5b255a000d2dc9e929886e668f0..4e2fb498f21ca8b36481664e4aebac321a1bc8a4 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -113,62 +113,63 @@ int FlightTasks::switchTask(int new_task_index) void FlightTasks::_updateCommand() { - /* 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->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); - - } +// 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->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); +// +// } } diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index c9e5ff5cf6010c9d4e0235f6c18a7398605919a6..b5abb21e3e845d81ead3466119efa352abacd57a 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -152,6 +152,6 @@ 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 _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 */ };