diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 990e101a0e..6ed6cbf041 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -188,6 +188,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; + case MAVLINK_MSG_ID_COMMAND_ACK: + handle_message_command_ack(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: handle_message_optical_flow_rad(msg); break; @@ -613,6 +617,19 @@ out: } } +void +MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) +{ + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(msg, &ack); + + 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); + } + } +} + void MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 51fa8189d0..2322037cc3 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -118,6 +118,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); + void handle_message_command_ack(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg);