reduce scan interval to 100msec

src/drivers/px4fmu/fmu.cpp

@@ -119,14 +119,16 @@ public:
 	int		set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
 
 private:
-	enum RC_SCAN {
+    enum RC_SCAN {
         RC_SCAN_PPM = 0,
 		RC_SCAN_SBUS,
 		RC_SCAN_DSM,
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24
 	};
-    const char* RC_SCAN_STRING[5] = {
+    enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS;
+
+    char const * RC_SCAN_STRING[5] = {
         "PPM",
         "SBUS",
         "DSM",
@@ -134,7 +136,12 @@ private:
         "ST24"
     };
 
-	static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
+    hrt_abstime _rc_scan_last_lock = 0;
+    hrt_abstime _rc_scan_begin = 0;
+    bool _rc_scan_locked = false;
+    bool _report_lock = true;
+
+    static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
 
 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -219,6 +226,7 @@ private:
 			hrt_abstime now, bool frame_drop, bool failsafe,
 			unsigned frame_drops, int rssi);
 	void dsm_bind_ioctl(int dsmMode);
+    void set_rc_scan_state(RC_SCAN _rc_scan_state);
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@@ -663,6 +671,13 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
 	_rc_in.rc_total_frame_count = 0;
 }
 
+void PX4FMU::set_rc_scan_state(RC_SCAN newState)
+{
+    warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]);
+    _rc_scan_begin = 0;
+    _rc_scan_state = newState;
+}
+
 void
 PX4FMU::cycle()
 {
@@ -874,14 +889,8 @@ PX4FMU::cycle()
 
 #ifdef RC_SERIAL_PORT
 	// This block scans for a supported serial RC input and locks onto the first one found
-	static hrt_abstime rc_scan_last_lock = 0;
-	static hrt_abstime rc_scan_begin = 0;
-	static bool rc_scan_locked = false;
-	static bool report_lock = true;
-	static enum RC_SCAN rc_scan_state = RC_SCAN_SBUS;
-
-	// Scan for one second, then switch protocol
-	constexpr hrt_abstime rc_scan_max = 1000 * 1000;
+    // Scan for 100 msec, then switch protocol
+    constexpr hrt_abstime rc_scan_max = 100 * 1000;
 
 	bool sbus_failsafe, sbus_frame_drop;
 	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
@@ -890,25 +899,25 @@ PX4FMU::cycle()
 	bool dsm_11_bit;
 
 
-	if (report_lock && rc_scan_locked) {
-		report_lock = false;
-        warnx("fmu: %s RC input locked", RC_SCAN_STRING[rc_scan_state]);
+    if (_report_lock && _rc_scan_locked) {
+        _report_lock = false;
+        warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 	}
 
 	// read all available data from the serial RC input UART
 	hrt_abstime now = hrt_absolute_time();
 	int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE);
 
-	switch (rc_scan_state) {
+    switch (_rc_scan_state) {
 	case RC_SCAN_SBUS:
-		if (rc_scan_begin == 0) {
-			rc_scan_begin = now;
+        if (_rc_scan_begin == 0) {
+            _rc_scan_begin = now;
 			// Configure serial port for SBUS
 			sbus_config(_rcs_fd, false);
 			INVERT_RC_INPUT(true);
 
-		} else if (now - rc_scan_last_lock < rc_scan_max
-			   || now - rc_scan_begin < rc_scan_max) {
+        } else if (now - _rc_scan_last_lock < rc_scan_max
+               || now - _rc_scan_begin < rc_scan_max) {
 
 			// parse new data
 			if (newBytes > 0) {
@@ -919,29 +928,27 @@ PX4FMU::cycle()
 					// we have a new SBUS frame. Publish it.
 					fill_rc_in(raw_rc_count, raw_rc_values, now,
 						   sbus_frame_drop, sbus_failsafe, frame_drops);
-					rc_scan_last_lock = now;
-					rc_scan_locked = true;
+                    _rc_scan_last_lock = now;
+                    _rc_scan_locked = true;
 				}
 			}
 
-		} else if (!rc_scan_locked) {
-			// This triggers the port re-configuration
-			rc_scan_begin = 0;
+        } else if (!_rc_scan_locked) {
 			// Scan the next protocol
-			rc_scan_state = RC_SCAN_DSM;
+            set_rc_scan_state(RC_SCAN_DSM);
 		}
 
 		break;
 
 	case RC_SCAN_DSM:
-		if (rc_scan_begin == 0) {
-			rc_scan_begin = now;
+        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) {
+        } else if (now - _rc_scan_last_lock < rc_scan_max
+               || now - _rc_scan_begin < rc_scan_max) {
 
 			if (newBytes > 0) {
 				// parse new data
@@ -952,29 +959,27 @@ PX4FMU::cycle()
 					// we have a new DSM frame. Publish it.
 					fill_rc_in(raw_rc_count, raw_rc_values, now,
 						   false, false, frame_drops);
-					rc_scan_last_lock = now;
-					rc_scan_locked = true;
+                    _rc_scan_last_lock = now;
+                    _rc_scan_locked = true;
 				}
 			}
 
-		} else if (!rc_scan_locked) {
-			// This triggers the port re-configuration
-			rc_scan_begin = 0;
+        } else if (!_rc_scan_locked) {
 			// Scan the next protocol
-			rc_scan_state = RC_SCAN_ST24;
+            set_rc_scan_state(RC_SCAN_ST24);
 		}
 
 		break;
 
 	case RC_SCAN_ST24:
-		if (rc_scan_begin == 0) {
-			rc_scan_begin = now;
+        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) {
+        } else if (now - _rc_scan_last_lock < rc_scan_max
+               || now - _rc_scan_begin < rc_scan_max) {
 
 			if (newBytes > 0) {
 				// parse new data
@@ -993,34 +998,32 @@ PX4FMU::cycle()
 					// we have a new ST24 frame. Publish it.
 					fill_rc_in(raw_rc_count, raw_rc_values, now,
 						   false, false, frame_drops, st24_rssi);
-					rc_scan_last_lock = now;
-					rc_scan_locked = true;
+                    _rc_scan_last_lock = now;
+                    _rc_scan_locked = true;
 				}
 			}
 
-		} else if (!rc_scan_locked) {
-			// This triggers the port re-configuration
-			rc_scan_begin = 0;
+        } else if (!_rc_scan_locked) {
 			// Scan the next protocol
-			rc_scan_state = RC_SCAN_SUMD;
+            set_rc_scan_state(RC_SCAN_SUMD);
 		}
 
 		break;
 
 	case RC_SCAN_SUMD:
-		rc_scan_state = RC_SCAN_PPM;
-		break;
+        set_rc_scan_state(RC_SCAN_PPM);
+        break;
 
 	case RC_SCAN_PPM:
 		// skip PPM if it's not supported
 #ifdef HRT_PPM_CHANNEL
-		if (rc_scan_begin == 0) {
-			rc_scan_begin = now;
+        if (_rc_scan_begin == 0) {
+            _rc_scan_begin = now;
 			// Configure timer input pin for CPPM
 			stm32_configgpio(GPIO_PPM_IN);
 
-		} else if (now - rc_scan_last_lock < rc_scan_max
-			   || now - rc_scan_begin < rc_scan_max) {
+        } else if (now - _rc_scan_last_lock < rc_scan_max
+               || now - _rc_scan_begin < rc_scan_max) {
 
 			// see if we have new PPM input data
 			if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal)
@@ -1029,21 +1032,19 @@ PX4FMU::cycle()
 				rc_updated = true;
 				fill_rc_in(ppm_decoded_channels, ppm_buffer, now,
 					   false, false, 0);
-				rc_scan_last_lock = now;
-				rc_scan_locked = true;
+                _rc_scan_last_lock = now;
+                _rc_scan_locked = true;
 			}
 
-		} else if (!rc_scan_locked) {
-			// This triggers the port re-configuration
-			rc_scan_begin = 0;
+        } else if (!_rc_scan_locked) {
 			// disable CPPM input by mapping it away from the timer capture input
 			stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK));
 			// Scan the next protocol
-			rc_scan_state = RC_SCAN_SBUS;
+            set_rc_scan_state(RC_SCAN_SBUS);
 		}
 
 #else   // skip PPM if it's not supported
-		rc_scan_state = RC_SCAN_SBUS;
+        set_rc_scan_state(RC_SCAN_SBUS);
 
 #endif  // HRT_PPM_CHANNEL