Browse Source

AP_Mount: fix where status_msg() is sent

send to the channel where the trigger msg was sent from
mission-4.1.18
Michael Oborne 11 years ago committed by Andrew Tridgell
parent
commit
30a3927ea8
  1. 6
      libraries/AP_Mount/AP_Mount.cpp
  2. 2
      libraries/AP_Mount/AP_Mount.h

6
libraries/AP_Mount/AP_Mount.cpp

@ -494,7 +494,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) @@ -494,7 +494,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
/// Return mount status information (depends on the previously set mount configuration)
/// triggered by a MavLink packet.
void AP_Mount::status_msg(mavlink_message_t *msg)
void AP_Mount::status_msg(mavlink_message_t *msg, mavlink_channel_t chan)
{
__mavlink_mount_status_t packet;
mavlink_msg_mount_status_decode(msg, &packet);
@ -524,9 +524,7 @@ void AP_Mount::status_msg(mavlink_message_t *msg) @@ -524,9 +524,7 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
break;
}
// status reply
// TODO: is COMM_3 correct ?
mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component,
mavlink_msg_mount_status_send_buf(msg, chan, packet.target_system, packet.target_component,
packet.pointing_a, packet.pointing_b, packet.pointing_c);
}

2
libraries/AP_Mount/AP_Mount.h

@ -52,7 +52,7 @@ public: @@ -52,7 +52,7 @@ public:
// MAVLink methods
void configure_msg(mavlink_message_t* msg);
void control_msg(mavlink_message_t* msg);
void status_msg(mavlink_message_t* msg);
void status_msg(mavlink_message_t* msg, mavlink_channel_t chan);
void set_roi_cmd(const struct Location *target_loc);
void configure_cmd();
void control_cmd();

Loading…
Cancel
Save