yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: Allow vehicle to receive a command_ack message and publish it to listeners

Browse Source
This commit is contained in:
José Roberto de Souza
2017-05-08 16:40:15 -07:00
committed by Lorenz Meier
parent a8cfd6f36a
commit 7082cc13e0
1 changed files with 14 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -620,6 +620,20 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid);
vehicle_command_ack_s command_ack = {};
command_ack.command = ack.command;
command_ack.result = ack.result;
command_ack.timestamp = hrt_absolute_time();
if (_command_ack_pub == nullptr) {
_command_ack_pub = 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), _command_ack_pub, &command_ack);
}
// TODO: move it to the same place that sent the command
if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) {
if (msg->compid == MAV_COMP_ID_CAMERA) {
PX4_WARN("Got unsuccessful result %d from camera", ack.result);