mavlink_receiver: generate rc channel count

PX4 · Jan 24, 2019 · fcf25c9 · fcf25c9
1 parent 2ffb49b
commit fcf25c9
Showing 1 changed file with 11 additions and 1 deletion.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1834,7 +1834,6 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
 	// metadata
 	rc.timestamp = hrt_absolute_time();
 	rc.timestamp_last_signal = rc.timestamp;
-	rc.channel_count = 18;
 	rc.rssi = RC_INPUT_RSSI_MAX;
 	rc.rc_failsafe = false;
 	rc.rc_lost = false;
@@ -1862,6 +1861,17 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
 	rc.values[16] = man.chan17_raw;
 	rc.values[17] = man.chan18_raw;
 
+	// check how many channels are valid
+	for (int i = 17; i >= 0; i--) {
+		const bool ignore_field = rc.values[i] == UINT16_MAX ||
+					  (rc.values[i] == 0 && (i > 7));
+
+		if (!ignore_field) {
+			rc.channel_count = i + 1;
+			break;
+		}
+	}
+
 	// publish uORB message
 	int instance; // provides the instance ID or the publication
 	ORB_PRIO priority = ORB_PRIO_HIGH; // since it is an override, set priority high