Cleaned up target system handling for some commands

src/modules/mavlink/mavlink_receiver.cpp

@@ -279,29 +279,37 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	_mavlink->set_has_received_messages(true);
 }
 
-void
-MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
+bool
+MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component)
 {
-	/* command */
-	mavlink_command_long_t cmd_mavlink;
-	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
-
 	/* evaluate if this system should accept this command */
-	bool target_ok;
-	switch (cmd_mavlink.command) {
+	bool target_ok = false;
+	switch (command) {
 
 		case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
-			/* broadcast */
-			target_ok = (cmd_mavlink.target_system == 0);
+			/* broadcast and ignore component */
+			target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
 			break;
 
 		default:
-			target_ok = (cmd_mavlink.target_system == mavlink_system.sysid);
+			target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
+			|| (target_component == MAV_COMP_ID_ALL));
 			break;
 	}
 
-	if (target_ok && ((cmd_mavlink.target_component == mavlink_system.compid)
-			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+	return target_ok;
+}
+
+void
+MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
+{
+	/* command */
+	mavlink_command_long_t cmd_mavlink;
+	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+
+	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
+
+	if (target_ok) {
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
@@ -362,8 +370,9 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 	mavlink_command_int_t cmd_mavlink;
 	mavlink_msg_command_int_decode(msg, &cmd_mavlink);
 
-	if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
+
+	if (target_ok) {
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */

src/modules/mavlink/mavlink_receiver.h

@@ -161,6 +161,8 @@ private:
 	 */
 	int decode_switch_pos_n(uint16_t buttons, unsigned sw);
 
+	bool	evaluate_target_ok(int command, int target_system, int target_component);
+
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
 	struct vehicle_land_detected_s hil_land_detector;