drivers/rc_input: cleanup and simplify data processing per type

- don't store data in class

src/drivers/rc_input/RCInput.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,9 +33,9 @@
 
 #include "RCInput.hpp"
 
-#include "crsf_telemetry.h"
 #include <uORB/topics/vehicle_command_ack.h>
 
+#include <poll.h>
 #include <termios.h>
 
 using namespace time_literals;
@@ -44,21 +44,8 @@ constexpr char const *RCInput::RC_SCAN_STRING[];
 
 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
 {
-	// rc input, published to ORB
-	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-
-	// initialize it as RC lost
-	_rc_in.rc_lost = true;
-
-	// initialize raw_rc values and count
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
 	if (device) {
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
@@ -77,6 +64,7 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;
 
 	perf_free(_cycle_perf);
+	perf_free(_cycle_interval_perf);
 	perf_free(_publish_interval_perf);
 }
 
@@ -88,6 +76,11 @@ RCInput::init()
 	RF_RADIO_POWER_CONTROL(true);
 #endif // RF_RADIO_POWER_CONTROL
 
+
+#if defined(RC_SERIAL_PORT)
+	_rc_serial_port_output = (strcmp(_device, RC_SERIAL_PORT) != 0);
+#endif // RC_SERIAL_PORT
+
 	// dsm_init sets some file static variables and returns a file descriptor
 	// it also powers on the radio if needed
 	_rcs_fd = dsm_init(_device);
@@ -164,89 +157,60 @@ RCInput::task_spawn(int argc, char *argv[])
 	_object.store(instance);
 	_task_id = task_id_is_work_queue;
 
-	instance->ScheduleOnInterval(_current_update_interval);
+	instance->ScheduleOnInterval(_backup_update_interval);
 
 	return PX4_OK;
 }
 
-void
-RCInput::fill_rc_in(uint16_t raw_rc_count_local,
-		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		    hrt_abstime now, bool frame_drop, bool failsafe,
-		    unsigned frame_drops, int rssi = -1)
+void RCInput::FillRssi(input_rc_s &input_rc)
 {
-	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
-
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
-	}
-
-	unsigned valid_chans = 0;
-
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
-
-		if (raw_rc_values_local[i] != UINT16_MAX) {
-			valid_chans++;
-		}
-
-		// once filled, reset values back to default
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
-
-	/* fake rssi if no value was provided */
-	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+	if (input_rc.rssi < 0 || input_rc.rssi > input_rc_s::RSSI_MAX) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < input_rc.channel_count)) {
 			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
 			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
 			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
 
 			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+			int rc_rssi = ((input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+
+			input_rc.rssi = math::constrain(rc_rssi, 0, (int)input_rc_s::RSSI_MAX);
+
+#if defined(ADC_RC_RSSI_CHANNEL)
 
 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
 			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
 
-			if (rssi_analog > 100.0f) {
-				rssi_analog = 100.0f;
-			}
-
-			if (rssi_analog < 0.0f) {
-				rssi_analog = 0.0f;
-			}
-
-			_rc_in.rssi = rssi_analog;
-
-		} else {
-			_rc_in.rssi = 255;
+			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
+#endif // ADC_RC_RSSI_CHANNEL
 		}
-
-	} else {
-		_rc_in.rssi = rssi;
 	}
-
-	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
-	}
-
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
 }
 
-void RCInput::set_rc_scan_state(RC_SCAN newState)
+void RCInput::PublishInputRc(input_rc_s &input_rc)
 {
-	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+	FillRssi(input_rc); // requires input_rc.values[]
+
+	input_rc.timestamp = hrt_absolute_time();
+	_input_rc_pub.publish(input_rc);
+
+	perf_count(_publish_interval_perf);
+
+	_last_publish_time = input_rc.timestamp;
+	_rc_scan_locked = true;
+}
+
+void RCInput::set_next_rc_scan_state()
+{
+	int new_state = _rc_scan_state + 1;
+
+	if (new_state >= RC_SCAN::RC_SCAN_MAX) {
+		new_state = 0;
+	}
+
+	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = newState;
+	_rc_scan_state = static_cast<RC_SCAN>(new_state);
 	_rc_scan_locked = false;
 
 	_report_lock = true;
@@ -288,8 +252,6 @@ void RCInput::Run()
 
 	} else {
 
-		perf_begin(_cycle_perf);
-
 		// Check if parameters have changed
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -307,13 +269,10 @@ void RCInput::Run()
 			}
 		}
 
-		const hrt_abstime cycle_timestamp = hrt_absolute_time();
-
-
 		/* vehicle command */
 		vehicle_command_s vcmd;
 
-		if (_vehicle_cmd_sub.update(&vcmd)) {
+		while (_vehicle_cmd_sub.update(&vcmd)) {
 			// Check for a pairing command
 			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
 
@@ -370,9 +329,8 @@ void RCInput::Run()
 			if (_adc_report_sub.copy(&adc)) {
 				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
 					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-						float adc_volt = adc.raw_data[i] *
-								 adc.v_ref /
-								 adc.resolution;
+
+						float adc_volt = adc.raw_data[i] * adc.v_ref / adc.resolution;
 
 						if (_analog_rc_rssi_volt < 0.0f) {
 							_analog_rc_rssi_volt = adc_volt;
@@ -391,18 +349,25 @@ void RCInput::Run()
 
 #endif // ADC_RC_RSSI_CHANNEL
 
-		bool rc_updated = false;
 
 		// This block scans for a supported serial RC input and locks onto the first one found
 		// Scan for 300 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 300_ms;
+		static constexpr hrt_abstime rc_scan_max = 300_ms;
 
-		unsigned frame_drops = 0;
+		// poll with 1 second timeout
+		pollfd fds[1];
+		fds[0].fd = _rcs_fd;
+		fds[0].events = POLLIN;
+		int ret = poll(fds, 1, 1000);
 
-		// TODO: needs work (poll _rcs_fd)
-		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
-		// then update priority to SCHED_PRIORITY_FAST_DRIVER
-		// read all available data from the serial RC input UART
+		perf_begin(_cycle_perf);
+		perf_count(_cycle_interval_perf);
+
+		if (ret < 0) {
+			PX4_DEBUG("poll error %d", ret);
+		}
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -411,7 +376,48 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}
 
+		bool rc_updated = false;
+
 		switch (_rc_scan_state) {
+#if defined(HRT_PPM_CHANNEL)
+
+		case RC_SCAN_PPM:
+			if (_rc_scan_begin == 0) {
+				_rc_scan_begin = cycle_timestamp;
+				// Configure timer input pin for CPPM
+				px4_arch_configgpio(GPIO_PPM_IN);
+
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+				// see if we have new PPM input data
+				if ((ppm_last_valid_decode != _last_publish_time) && (ppm_decoded_channels >= 4)) {
+					// we have a new PPM frame. Publish it.
+					rc_updated = true;
+
+					input_rc_s input_rc{};
+					input_rc.timestamp_last_signal = ppm_last_valid_decode;
+					input_rc.channel_count = math::max(ppm_decoded_channels, (unsigned)input_rc_s::RC_INPUT_MAX_CHANNELS);
+					input_rc.rc_lost = (ppm_decoded_channels == 0);
+					input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+					input_rc.rc_ppm_frame_length = ppm_frame_length;
+
+					for (int i = 0; i < input_rc.channel_count; i++) {
+						input_rc.values[i] = ppm_buffer[i];
+					}
+
+					PublishInputRc(input_rc);
+				}
+
+			} else {
+				// disable CPPM input by mapping it away from the timer capture input
+				px4_arch_unconfiggpio(GPIO_PPM_IN);
+
+				// Scan the next protocol
+				set_next_rc_scan_state();
+			}
+
+			break;
+#endif  // HRT_PPM_CHANNEL
+
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -423,30 +429,40 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					bool sbus_failsafe = false;
 					bool sbus_frame_drop = false;
+					unsigned frame_drops = 0;
 
-					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
+					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe,
 								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   sbus_frame_drop, sbus_failsafe, frame_drops);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_failsafe = sbus_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
 				rc_io_invert(false);
-				set_rc_scan_state(RC_SCAN_DSM);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -461,29 +477,38 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					int8_t dsm_rssi = 0;
-					bool dsm_11_bit = false;
-
 					// parse new data
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+					bool dsm_11_bit = false;
+					unsigned frame_drops = 0;
+					int8_t dsm_rssi = 0;
+
+					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
 							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, false, frame_drops, dsm_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_ST24);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -498,43 +523,43 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t st24_rssi, lost_count;
-
-					rc_updated = false;
+					uint8_t st24_rssi = 0;
+					uint8_t lost_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
 
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// set updated flag if one complete packet was parsed
 						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &raw_rc_count, raw_rc_values,
+										input_rc_s::RC_INPUT_MAX_CHANNELS));
 					}
 
 					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
 					// The only way to detect RC loss is therefore to look at the lost_count.
-
 					if (rc_updated) {
-						if (lost_count == 0) {
-							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
-							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-								   false, false, frame_drops, st24_rssi);
-							_rc_scan_locked = true;
+						// we have a new ST24 frame. Publish it.
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = st24_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0) || (lost_count > 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
 
-						} else {
-							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SUMD);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -549,74 +574,47 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t sumd_rssi, rx_count;
-					bool sumd_failsafe;
-
-					rc_updated = false;
+					uint8_t sumd_rssi = 0;
+					uint8_t rx_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					bool sumd_failsafe = false;
 
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// set updated flag if one complete packet was parsed
 						sumd_rssi = input_rc_s::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, &sumd_failsafe));
+						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, &sumd_failsafe));
 					}
 
 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, sumd_failsafe, frame_drops, sumd_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = sumd_rssi;
+						input_rc.rc_failsafe = sumd_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_PPM);
+				set_next_rc_scan_state();
 			}
 
 			break;
 
-		case RC_SCAN_PPM:
-			// skip PPM if it's not supported
-#ifdef HRT_PPM_CHANNEL
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
-					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_CRSF);
-			}
-
-#else   // skip PPM if it's not supported
-			set_rc_scan_state(RC_SCAN_CRSF);
-
-#endif  // HRT_PPM_CHANNEL
-
-			break;
-
 		case RC_SCAN_CRSF:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -627,40 +625,44 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+
+					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
 								input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
 
-						// on Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_crsf_telemetry) {
-							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
 
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+						PublishInputRc(input_rc);
 
-						_rc_scan_locked = true;
+						if (_rc_serial_port_output) {
+							if (!_rc_scan_locked && !_crsf_telemetry) {
+								_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+							}
 
-						if (_crsf_telemetry) {
-							_crsf_telemetry->update(cycle_timestamp);
+							if (_crsf_telemetry) {
+								_crsf_telemetry->update(cycle_timestamp);
+							}
 						}
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -675,55 +677,55 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				// parse new data
 				if (newBytes > 0) {
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+								input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = ghst_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
 
-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
 
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+						PublishInputRc(input_rc);
 
-						_rc_scan_locked = true;
+						if (_rc_serial_port_output) {
+							if (!_rc_scan_locked && !_ghst_telemetry) {
+								_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+							}
 
-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
+							if (_ghst_telemetry) {
+								_ghst_telemetry->update(cycle_timestamp);
+							}
 						}
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SBUS);
+				set_next_rc_scan_state();
 			}
 
 			break;
+
+		default:
+			// Scan the next protocol
+			set_next_rc_scan_state();
 		}
 
-		perf_end(_cycle_perf);
-
-		if (rc_updated) {
-			perf_count(_publish_interval_perf);
-
-			_to_input_rc.publish(_rc_in);
-
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -731,6 +733,16 @@ void RCInput::Run()
 			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 		}
+
+		// reschedule immediately if RC is locked
+		if (_rc_scan_locked) {
+			ScheduleNow();
+
+		} else {
+			ScheduleDelayed(_backup_update_interval);
+		}
+
+		perf_end(_cycle_perf);
 	}
 }
 
@@ -805,8 +817,6 @@ int RCInput::custom_command(int argc, char *argv[])
 
 int RCInput::print_status()
 {
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
-
 	if (_device[0] != '\0') {
 		PX4_INFO("UART device: %s", _device);
 		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
@@ -828,42 +838,23 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;
 
-
-		case RC_SCAN_DSM:
-			// DSM status output
-#if defined(SPEKTRUM_POWER)
-#endif
-			break;
-
-		case RC_SCAN_PPM:
-			// PPM status output
-			break;
-
-		case RC_SCAN_SUMD:
-			// SUMD status output
-			break;
-
-		case RC_SCAN_ST24:
-			// SUMD status output
+		default:
 			break;
 		}
 	}
 
-#if ADC_RC_RSSI_CHANNEL
+#if defined(ADC_RC_RSSI_CHANNEL)
 
 	if (_analog_rc_rssi_stable) {
 		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
 	}
 
-#endif
+#endif // ADC_RC_RSSI_CHANNEL
 
 	perf_print_counter(_cycle_perf);
+	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);
 
-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
-	}
-
 	return 0;
 }
 

src/drivers/rc_input/RCInput.hpp

@@ -91,23 +91,31 @@ public:
 private:
 
 	enum RC_SCAN {
-		RC_SCAN_PPM = 0,
+#if defined(HRT_PPM_CHANNEL)
+		RC_SCAN_PPM,
+#endif // HRT_PPM_CHANNEL
 		RC_SCAN_SBUS,
 		RC_SCAN_DSM,
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST
+		RC_SCAN_GHST,
+
+		RC_SCAN_MAX
 	} _rc_scan_state{RC_SCAN_SBUS};
 
-	static constexpr char const *RC_SCAN_STRING[7] {
+	static constexpr char const *RC_SCAN_STRING[] {
+#if defined(HRT_PPM_CHANNEL)
 		"PPM",
+#endif // HRT_PPM_CHANNEL
 		"SBUS",
 		"DSM",
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST"
+		"GHST",
+
+		"NONE"
 	};
 
 	void Run() override;
@@ -116,38 +124,36 @@ private:
 	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
 #endif // SPEKTRUM_POWER
 
-	void fill_rc_in(uint16_t raw_rc_count_local,
-			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			hrt_abstime now, bool frame_drop, bool failsafe,
-			unsigned frame_drops, int rssi);
-
-	void set_rc_scan_state(RC_SCAN _rc_scan_state);
+	void FillRssi(input_rc_s &input_rc);
+	void PublishInputRc(input_rc_s &input_rc);
 
 	void rc_io_invert(bool invert);
+	void set_next_rc_scan_state();
 
 	hrt_abstime _rc_scan_begin{0};
+	hrt_abstime _last_publish_time{0};
 
 	bool _initialized{false};
 	bool _rc_scan_locked{false};
 	bool _report_lock{true};
 
-	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+	static constexpr unsigned	_backup_update_interval{100000}; // 10 Hz (backup schedule)
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
+#if defined(ADC_RC_RSSI_CHANNEL)
+	uORB::Subscription	_adc_report_sub {ORB_ID(adc_report)};
+	float _analog_rc_rssi_volt{-1.0f};
+	bool _analog_rc_rssi_stable{false};
+#endif // ADC_RC_RSSI_CHANNEL
+
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	input_rc_s	_rc_in{};
-
-	float		_analog_rc_rssi_volt{-1.0f};
-	bool		_analog_rc_rssi_stable{false};
-
+	bool _rc_serial_port_output{true};
 	bool _armed{false};
 
-
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -155,14 +161,13 @@ private:
 	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
 	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
 
-	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
-	uint16_t _raw_rc_count{};
-
 	CRSFTelemetry *_crsf_telemetry{nullptr};
 	GHSTTelemetry *_ghst_telemetry{nullptr};
 
-	perf_counter_t	_cycle_perf;
-	perf_counter_t	_publish_interval_perf;
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+
 	uint32_t	_bytes_rx{0};
 
 	DEFINE_PARAMETERS(