fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: comment out mavlink command processing

Browse Source 
because upstream there needs to be a
common mavlink command definition first
and then it can be easily reenabled again
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent cf48ceb428
commit 8b4471d842
2 changed files with 61 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/FlightTasks.cpp
+59 -58
View File
@@ -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);
//
// }
}
src/lib/FlightTasks/FlightTasks.hpp
+2 -2
View File
@@ -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 */
};