From cebfef3ead15c13ae2463adf45b8fc384955c5a2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 9 Oct 2014 09:19:35 +1100
Subject: [PATCH] HAL_Linux: don't accept less than 5 input channels

---
 libraries/AP_HAL_Linux/RCInput.cpp | 50 +++++++++++++++++++-----------
 libraries/AP_HAL_Linux/RCInput.h   | 13 +++++---
 2 files changed, 40 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp
index 528630c771..2946785090 100644
--- a/libraries/AP_HAL_Linux/RCInput.cpp
+++ b/libraries/AP_HAL_Linux/RCInput.cpp
@@ -23,9 +23,10 @@ extern const AP_HAL::HAL& hal;
 using namespace Linux;
 
 LinuxRCInput::LinuxRCInput() :
-    new_rc_input(false),
-    _channel_counter(-1)
-{}
+    new_rc_input(false)
+{
+    ppm_state._channel_counter = -1;
+}
 
 void LinuxRCInput::init(void* machtnichts)
 {
@@ -47,8 +48,10 @@ uint16_t LinuxRCInput::read(uint8_t ch)
     if (_override[ch]) {
         return _override[ch];
     }
-    
-    return _pulse_capt[ch];
+    if (ch >= _num_channels) {
+        return 0;
+    }
+    return _pwm_values[ch];
 }
 
 uint8_t LinuxRCInput::read(uint16_t* periods, uint8_t len) 
@@ -106,14 +109,17 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
     if (width_usec >= 4000) {
         // a long pulse indicates the end of a frame. Reset the
         // channel counter so next pulse is channel 0
-        if (_channel_counter != -1) {
+        if (ppm_state._channel_counter >= 5) {
+            for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
+                _pwm_values[i] = ppm_state._pulse_capt[i];
+            }
+            _num_channels = ppm_state._channel_counter;
             new_rc_input = true;
-            _num_channels = _channel_counter;
         }
-        _channel_counter = 0;
+        ppm_state._channel_counter = 0;
         return;
     }
-    if (_channel_counter == -1) {
+    if (ppm_state._channel_counter == -1) {
         // we are not synchronised
         return;
     }
@@ -125,18 +131,22 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
      */
     if (width_usec > 700 && width_usec < 2300) {
         // take a reading for the current channel
-        _pulse_capt[_channel_counter] = width_usec;
+        // buffer these
+        ppm_state._pulse_capt[ppm_state._channel_counter] = width_usec;
 
         // move to next channel
-        _channel_counter++;
+        ppm_state._channel_counter++;
     }
 
     // if we have reached the maximum supported channels then
     // mark as unsynchronised, so we wait for a wide pulse
-    if (_channel_counter >= LINUX_RC_INPUT_NUM_CHANNELS) {
+    if (ppm_state._channel_counter >= LINUX_RC_INPUT_NUM_CHANNELS) {
+        for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
+            _pwm_values[i] = ppm_state._pulse_capt[i];
+        }
+        _num_channels = ppm_state._channel_counter;
         new_rc_input = true;
-        _channel_counter = -1;
-        _num_channels = _channel_counter;
+        ppm_state._channel_counter = -1;
     }
 }
 
@@ -206,11 +216,13 @@ void LinuxRCInput::_process_sbus_pulse(uint16_t width_s0, uint16_t width_s1)
         bool sbus_failsafe=false, sbus_frame_drop=false;
         if (sbus_decode(bytes, values, &num_values, 
                         &sbus_failsafe, &sbus_frame_drop, 
-                        LINUX_RC_INPUT_NUM_CHANNELS)) {
+                        LINUX_RC_INPUT_NUM_CHANNELS) && 
+            num_values >= 5) {
             for (i=0; i<num_values; i++) {
-                _pulse_capt[i] = values[i];
+                _pwm_values[i] = values[i];
             }
             _num_channels = num_values;
+            new_rc_input = true;
         }
         goto reset;
     } else if (bits_s1 > 12) {
@@ -268,11 +280,13 @@ void LinuxRCInput::_process_dsm_pulse(uint16_t width_s0, uint16_t width_s1)
             }
             uint16_t values[8];
             uint16_t num_values=0;
-            if (dsm_decode(hal.scheduler->micros64(), bytes, values, &num_values, 8)) {
+            if (dsm_decode(hal.scheduler->micros64(), bytes, values, &num_values, 8) && 
+                num_values >= 5) {
                 for (i=0; i<num_values; i++) {
-                    _pulse_capt[i] = values[i];
+                    _pwm_values[i] = values[i];
                 }
                 _num_channels = num_values;                
+                new_rc_input = true;
             }
         }
         memset(&dsm_state, 0, sizeof(dsm_state));
diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h
index 1489d84c9f..ac963d5281 100644
--- a/libraries/AP_HAL_Linux/RCInput.h
+++ b/libraries/AP_HAL_Linux/RCInput.h
@@ -28,12 +28,9 @@ public:
 
  private:
     volatile bool new_rc_input;
-    
-    uint16_t _pulse_capt[LINUX_RC_INPUT_NUM_CHANNELS];
-    uint8_t  _num_channels;
 
-    // the channel we will receive input from next, or -1 when not synchronised
-    int8_t _channel_counter;
+    uint16_t _pwm_values[LINUX_RC_INPUT_NUM_CHANNELS];    
+    uint8_t  _num_channels;
 
     void _process_ppmsum_pulse(uint16_t width);
     void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1);
@@ -42,6 +39,12 @@ public:
     /* override state */
     uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];
 
+    // state of ppm decoder
+    struct {
+        int8_t _channel_counter;
+        uint16_t _pulse_capt[LINUX_RC_INPUT_NUM_CHANNELS];
+    } ppm_state;
+
     // state of SBUS bit decoder
     struct {
 	uint16_t bytes[25]; // including start bit, parity and stop bits