From 6f993fe64a4864fcc35bacd31718b51b8dd99563 Mon Sep 17 00:00:00 2001
From: Mikhail Avkhimenia <mikhail.avkhimenia@emlid.com>
Date: Tue, 28 Oct 2014 17:37:25 +0300
Subject: [PATCH] HAL_Linux: add prototype RCInput code for Navio

---
 libraries/AP_HAL_Linux/RCInput_Navio.cpp | 52 ++++++++++++++++++++++--
 libraries/AP_HAL_Linux/RCInput_Navio.h   | 13 ++++++
 2 files changed, 62 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_HAL_Linux/RCInput_Navio.cpp b/libraries/AP_HAL_Linux/RCInput_Navio.cpp
index ff99f86b9e..b2ac08eb94 100644
--- a/libraries/AP_HAL_Linux/RCInput_Navio.cpp
+++ b/libraries/AP_HAL_Linux/RCInput_Navio.cpp
@@ -15,16 +15,62 @@ extern const AP_HAL::HAL& hal;
 
 using namespace Linux;
 
-
+// This is a prototype code for RC input decoding on Raspberry Pi.
+// It uses pigpio daemon to sample GPIOs over DMA with 1 microsecond resolution.
+// This code should be rewritten to configure DMA GPIO sampling without pigpio.
 
 void LinuxRCInput_Navio::init(void*)
 {
-    // To be added
+    curtick = 0;
+    prevtick = 0;
+    width_s0 = 0;
+    width_s1 = 0;
+    
+    // Kills pigpio daemon in case it was run with wrong parameters   
+    
+    system("killall pigpiod -q");
+    hal.scheduler->delay(1000);
+    
+    // Starts pigpio daemon with 1 microsecond sampling resolution
+    
+    system("pigpiod -s 1");
+    hal.scheduler->delay(1000);
+    
+    // Configures pigpiod to send GPIO change notifications to /dev/pigpio0
+    
+    system("pigs NO NB 0 0x10");
+    hal.scheduler->delay(1000);
+    
+    // Configures /dev/pigpio0 for non-blocking read
+    
+    pigpio = open("/dev/pigpio0", O_RDONLY);
+    
+    if (pigpio == -1)
+        hal.scheduler->panic("No pigpio interface for RCInput");
 }
 
 void LinuxRCInput_Navio::_timer_tick()
 {
-    // To be added
+    while (true) {
+        int bytesAvailable;
+        ioctl(pigpio, FIONREAD, &bytesAvailable);
+        
+        if (bytesAvailable >= 12)
+            ::read(pigpio, reinterpret_cast<uint8_t*>(&gpioReport), 12);
+        else
+            break;
+            
+        prevtick = curtick;
+        curtick = gpioReport.tick;
+        
+        if (((gpioReport.level >> 4) & 0x01) == 0) {
+            width_s0 = curtick - prevtick;  
+        }          
+        else {
+            width_s1 = curtick - prevtick;
+            _process_rc_pulse(width_s0, width_s1);
+        }
+    } 
 }
 
 #endif // CONFIG_HAL_BOARD_SUBTYPE
diff --git a/libraries/AP_HAL_Linux/RCInput_Navio.h b/libraries/AP_HAL_Linux/RCInput_Navio.h
index 64093a092d..0cb3d9f60f 100644
--- a/libraries/AP_HAL_Linux/RCInput_Navio.h
+++ b/libraries/AP_HAL_Linux/RCInput_Navio.h
@@ -12,7 +12,20 @@ public:
     void _timer_tick(void);
     
 private:
+    int pigpio;
     
+    struct gpioReport_t
+    {
+        uint16_t seqno;
+        uint16_t flags;
+        uint32_t tick;
+        uint32_t level;
+    } gpioReport;
+    
+    int curtick;
+    int prevtick;
+    uint16_t width_s0;
+    uint16_t width_s1;
 };
 
 #endif // __AP_HAL_LINUX_RCINPUT_NAVIO_H__