yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00: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-06-26 15:03:32 +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

122
src/lib/FlightTasks/FlightTasks.cpp
View File

@@ -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);
}
}

4
src/lib/FlightTasks/FlightTasks.hpp
View File

@@ -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 */
};