mavlink: receiver handle RC_CHANNELS if from MAV_COMP_ID_SYSTEM_CONTROL

PX4 · Aug 1, 2021 · 6dbfdd3 · 6dbfdd3
1 parent 593883a
commit 6dbfdd3
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 0 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -173,6 +173,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_RC_CHANNELS:
+		handle_message_rc_channels(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
 		handle_message_rc_channels_override(msg);
 		break;
@@ -1859,6 +1863,65 @@ MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_rc_channels(mavlink_message_t *msg)
+{
+	mavlink_rc_channels_t rc_channels;
+	mavlink_msg_rc_channels_decode(msg, &rc_channels);
+
+	if (msg->compid != MAV_COMP_ID_SYSTEM_CONTROL) {
+		PX4_DEBUG("Mavlink receiver only processes RC_CHANNELS from MAV_COMP_ID_SYSTEM_CONTROL");
+		return;
+	}
+
+	input_rc_s rc{};
+
+	rc.timestamp_last_signal = rc.timestamp;
+	rc.rssi = RC_INPUT_RSSI_MAX;
+	rc.rc_total_frame_count = 1;
+	rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
+
+	// channels
+	rc.values[0] = rc_channels.chan1_raw;
+	rc.values[1] = rc_channels.chan2_raw;
+	rc.values[2] = rc_channels.chan3_raw;
+	rc.values[3] = rc_channels.chan4_raw;
+	rc.values[4] = rc_channels.chan5_raw;
+	rc.values[5] = rc_channels.chan6_raw;
+	rc.values[6] = rc_channels.chan7_raw;
+	rc.values[7] = rc_channels.chan8_raw;
+	rc.values[8] = rc_channels.chan9_raw;
+	rc.values[9] = rc_channels.chan10_raw;
+	rc.values[10] = rc_channels.chan11_raw;
+	rc.values[11] = rc_channels.chan12_raw;
+	rc.values[12] = rc_channels.chan13_raw;
+	rc.values[13] = rc_channels.chan14_raw;
+	rc.values[14] = rc_channels.chan15_raw;
+	rc.values[15] = rc_channels.chan16_raw;
+	rc.values[16] = rc_channels.chan17_raw;
+	rc.values[17] = rc_channels.chan18_raw;
+
+	// check how many channels are valid
+	for (int i = 17; i >= 0; i--) {
+		const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX
+		const bool ignore_zero = (i > 7) && (rc.values[i] == 0); // ignore channel 8-18 if value is 0
+
+		if (ignore_max || ignore_zero) {
+			// set all ignored values to zero
+			rc.values[i] = 0;
+
+		} else {
+			// first channel to not ignore -> set count considering zero-based index
+			rc.channel_count = i + 1;
+			break;
+		}
+	}
+
+	// publish uORB message
+	rc.timestamp = hrt_absolute_time();
+	_rc_pub.publish(rc);
+}
+
 void
 MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
 {

diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
@@ -176,6 +176,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_play_tune(mavlink_message_t *msg);
 	void handle_message_play_tune_v2(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
+	void handle_message_rc_channels(mavlink_message_t *msg);
 	void handle_message_rc_channels_override(mavlink_message_t *msg);
 	void handle_message_serial_control(mavlink_message_t *msg);
 	void handle_message_set_actuator_control_target(mavlink_message_t *msg);