diff options
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 14 |
1 files changed, 10 insertions, 4 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9e3842614..64ba93f9c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,11 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && - mavlink_system.compid == local_ned_position_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (local_ned_position_setpoint_external.coordinate_frame) { @@ -482,8 +485,11 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == attitude_setpoint_external.target_system && - mavlink_system.compid == attitude_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == attitude_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } |