fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 05:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: reintroduce vehicle command handling

Browse Source 
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.
This commit is contained in:
MaEtUgR
2018-07-24 15:03:53 +02:00
committed by ChristophTobler
parent 5dae404fb2
commit 3b7d31de75
2 changed files with 65 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/FlightTasks.cpp
+63 -59
View File
@@ -184,63 +184,67 @@ const char *FlightTasks::errorToString(const int error)
void FlightTasks::_updateCommand() void FlightTasks::_updateCommand()
{ {
// TODO: port flight task mavlink command to have support for this functionality // lazy subscription to command topic
// /* lazy subscription to command topic */ if (_sub_vehicle_command < 0) {
// if (_sub_vehicle_command < 0) { _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
// _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); }
// }
// // check if there's any new command
// /* check if there's any new command */ bool updated = false;
// bool updated = false; orb_check(_sub_vehicle_command, &updated);
// orb_check(_sub_vehicle_command, &updated);
// if (!updated) {
// if (!updated) { return;
// return; }
// }
// // get command
// /* check if command is for flight task library */ struct vehicle_command_s command;
// struct vehicle_command_s command; orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
// orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
// // check what command it is
// if (command.command != vehicle_command_s::VEHICLE_CMD_FLIGHT_TASK) { FlightTaskIndex desired_task = FlightTaskIndex::None;
// return; switch(command.command) {
// } case vehicle_command_s::VEHICLE_CMD_DO_ORBIT :
// desired_task = FlightTaskIndex::Orbit;
// /* evaluate command */ break;
// // TODO: remapping of the Mavlink definition to the internal tasks necessary // ignore all unkown commands
// int switch_result = switchTask(int(command.param1 + 0.5f)); default : return;
// uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; }
//
// /* if we are in the desired task */ // switch to the commanded task
// if (switch_result >= 0) { int switch_result = switchTask(desired_task);
// cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
//
// /* if the task is running apply parameters to it and see if it rejects */ // if we are in/switched to the desired task
// if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { if (switch_result >= 0) {
// cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
//
// /* if we just switched and parameters are not accepted, go to failsafe */ // if the task is running apply parameters to it and see if it rejects
// if (switch_result == 1) { if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
// switchTask(-1); cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// } // 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; // send back acknowledgment
// command_ack.target_system = command.source_system; vehicle_command_ack_s command_ack = {};
// command_ack.target_component = command.source_component; command_ack.command = command.command;
// command_ack.result = cmd_result;
// if (_pub_vehicle_command_ack == nullptr) { command_ack.result_param1 = switch_result;
// _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, command_ack.target_system = command.source_system;
// vehicle_command_ack_s::ORB_QUEUE_LENGTH); command_ack.target_component = command.source_component;
//
// } else { if (_pub_vehicle_command_ack == nullptr) {
// orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); _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);
}
} }
src/lib/FlightTasks/FlightTasks.hpp
+2 -2
View File
@@ -188,8 +188,8 @@ private:
* Check for vehicle commands (received via MAVLink), evaluate and acknowledge them * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
*/ */
void _updateCommand(); 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 _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 */
}; };