add SUMD decoder to RCscan

src/drivers/px4fmu/fmu.cpp

@@ -990,7 +990,7 @@ PX4FMU::cycle()
 				for (unsigned i = 0; i < newBytes; i++) {
 					/* set updated flag if one complete packet was parsed */
 					st24_rssi = RC_INPUT_RSSI_MAX;
-					rc_updated |= (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count,
+					rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count,
 									&raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
 				}
 
@@ -1011,6 +1011,42 @@ PX4FMU::cycle()
 		break;
 
 	case RC_SCAN_SUMD:
+		if (_rc_scan_begin == 0) {
+			_rc_scan_begin = now;
+//			// Configure serial port for DSM
+			dsm_config(_rcs_fd);
+			INVERT_RC_INPUT(false);
+
+		} else if (now - _rc_scan_last_lock < rc_scan_max
+			   || now - _rc_scan_begin < rc_scan_max) {
+
+			if (newBytes > 0) {
+				// parse new data
+				uint8_t sumd_rssi, rx_count;
+
+				rc_updated = false;
+
+				for (unsigned i = 0; i < newBytes; i++) {
+					/* set updated flag if one complete packet was parsed */
+					sumd_rssi = RC_INPUT_RSSI_MAX;
+					rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
+									&raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+				}
+
+				if (rc_updated) {
+					// we have a new SUMD frame. Publish it.
+					fill_rc_in(raw_rc_count, raw_rc_values, now,
+						   false, false, frame_drops, sumd_rssi);
+					_rc_scan_last_lock = now;
+					_rc_scan_locked = true;
+				}
+			}
+
+		} else if (!_rc_scan_locked) {
+			// Scan the next protocol
+			set_rc_scan_state(RC_SCAN_SUMD);
+		}
+
 		set_rc_scan_state(RC_SCAN_PPM);
 		break;