HAL_Linux: Add support for PPM input to HAL_Linux

rcin thread continuously scans for values inside ring buffer maintained by pru with rcinpru0 firmware.

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -85,6 +85,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const
     scheduler->init(NULL);
     gpio->init();
     rcout->init(NULL);
+    rcin->init(NULL);
     uartA->begin(115200);
     i2c->begin();
     spi->init(NULL);

libraries/AP_HAL_Linux/RCInput.cpp

@@ -1,6 +1,18 @@
 #include <AP_HAL.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include <stdio.h>
+#include <sys/time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <stdint.h>
 
 #include "RCInput.h"
 
@@ -10,7 +22,12 @@ new_rc_input(false)
 {}
 
 void LinuxRCInput::init(void* machtnichts)
-{}
+{
+    int mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
+    ring_buffer = (volatile struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, PRUSS_SHAREDRAM_BASE);
+    close(mem_fd);
+    ring_buffer->ring_head = 0;
+}
 
 bool LinuxRCInput::new_input() 
 {
@@ -19,7 +36,7 @@ bool LinuxRCInput::new_input()
 
 uint8_t LinuxRCInput::num_channels() 
 {
-    return 8;
+    return _num_channels;
 }
 
 uint16_t LinuxRCInput::read(uint8_t ch) 
@@ -28,24 +45,30 @@ uint16_t LinuxRCInput::read(uint8_t ch)
     if (_override[ch]) {
         return _override[ch];
     }
-    if (ch == 2) {
-        // force low throttle for now
-        return 900;
-    }
-    return 1500;
+    
+    return _pulse_capt[ch];
 }
 
 uint8_t LinuxRCInput::read(uint16_t* periods, uint8_t len) 
 {
-    for (uint8_t i=0; i<len; i++) {
-	periods[i] = read(i);
+    uint8_t i;
+    for (i=0; i<len; i++) {
+        if((periods[i] = read(i))){
+            continue;
+        }
+        else{
+            break;
+        }
     }
-    return 8;
+    return (i+1);
 }
 
 bool LinuxRCInput::set_overrides(int16_t *overrides, uint8_t len) 
 {
     bool res = false;
+    if(len > LINUX_RC_INPUT_NUM_CHANNELS){
+        len = LINUX_RC_INPUT_NUM_CHANNELS;
+    }
     for (uint8_t i = 0; i < len; i++) {
         res |= set_override(i, overrides[i]);
     }
@@ -55,7 +78,7 @@ bool LinuxRCInput::set_overrides(int16_t *overrides, uint8_t len)
 bool LinuxRCInput::set_override(uint8_t channel, int16_t override) 
 {
     if (override < 0) return false; /* -1: no change. */
-    if (channel < 8) {
+    if (channel < LINUX_RC_INPUT_NUM_CHANNELS) {
         _override[channel] = override;
         if (override != 0) {
             new_rc_input = true;
@@ -67,9 +90,33 @@ bool LinuxRCInput::set_override(uint8_t channel, int16_t override)
 
 void LinuxRCInput::clear_overrides()
 {
-    for (uint8_t i = 0; i < 8; i++) {
-	_override[i] = 0;
+    for (uint8_t i = 0; i < LINUX_RC_INPUT_NUM_CHANNELS; i++) {
+       _override[i] = 0;
     }
 }
 
+void LinuxRCInput::_timer_tick()
+{
+    uint8_t channel_ctr;
+    uint16_t s1_time, s2_time;
+    //scan for the latest start pulse
+    while(ring_buffer->buffer[ring_buffer->ring_head].delta_t < 8000){
+        ring_buffer->ring_head = (ring_buffer->ring_head + 1) % NUM_RING_ENTRIES;
+    }
+    for(channel_ctr = 0; channel_ctr < LINUX_RC_INPUT_NUM_CHANNELS; channel_ctr++){
+        ring_buffer->ring_head = (ring_buffer->ring_head + 2) % NUM_RING_ENTRIES;
+        //wait until we receive two pulse_width(State1 & State2) values inside buffer
+        while(ring_buffer->ring_head <= (ring_buffer->ring_tail));
+        s1_time = (uint32_t)ring_buffer->buffer[ring_buffer->ring_head - 1].delta_t;
+        s2_time = (uint32_t)ring_buffer->buffer[ring_buffer->ring_head].delta_t;
+        _pulse_capt[channel_ctr] = s1_time + s2_time;
+        if(_pulse_capt[channel_ctr] > RC_INPUT_MAX_PULSEWIDTH){
+            _pulse_capt[channel_ctr] = RC_INPUT_MAX_PULSEWIDTH;
+        }
+        else if(_pulse_capt[channel_ctr] < RC_INPUT_MIN_PULSEWIDTH){
+            _pulse_capt[channel_ctr] = RC_INPUT_MIN_PULSEWIDTH;
+        }
+    }    
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_Linux/RCInput.h

@@ -4,6 +4,11 @@
 
 #include <AP_HAL_Linux.h>
 
+#define LINUX_RC_INPUT_NUM_CHANNELS 8
+#define PRUSS_SHAREDRAM_BASE        0x4a312000
+#define NUM_RING_ENTRIES            200
+
+
 class Linux::LinuxRCInput : public AP_HAL::RCInput {
 public:
     LinuxRCInput();
@@ -16,12 +21,25 @@ public:
     bool set_overrides(int16_t *overrides, uint8_t len);
     bool set_override(uint8_t channel, int16_t override);
     void clear_overrides();
-
+    
+    void _timer_tick(void);
+    
  private:
     bool new_rc_input;
-
+    
+    uint16_t _pulse_capt[LINUX_RC_INPUT_NUM_CHANNELS];
+    uint8_t  _num_channels;
     /* override state */
-    uint16_t _override[8];
+    uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];
+    struct ring_buffer {
+        volatile uint16_t ring_head;
+        volatile uint16_t ring_tail;
+        struct __attribute__((__packed__)) {
+               uint16_t pin_value;
+               uint16_t delta_t;
+        } buffer[NUM_RING_ENTRIES];
+    };
+    volatile struct ring_buffer *ring_buffer;
 };
 
 #endif // __AP_HAL_LINUX_RCINPUT_H__

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -4,6 +4,7 @@
 
 #include "Scheduler.h"
 #include "Storage.h"
+#include "RCInput.h"
 #include "UARTDriver.h"
 #include <sys/time.h>
 #include <poll.h>
@@ -17,8 +18,9 @@ using namespace Linux;
 
 extern const AP_HAL::HAL& hal;
 
-#define APM_LINUX_TIMER_PRIORITY    13
-#define APM_LINUX_UART_PRIORITY     12
+#define APM_LINUX_TIMER_PRIORITY    14
+#define APM_LINUX_UART_PRIORITY     13
+#define APM_LINUX_RCIN_PRIORITY     12
 #define APM_LINUX_MAIN_PRIORITY     11
 #define APM_LINUX_IO_PRIORITY       10
 
@@ -66,7 +68,15 @@ void LinuxScheduler::init(void* machtnichts)
     pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
 
     pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread, this);
-    
+
+    // the RCIN thread runs at a lower medium priority    
+    pthread_attr_init(&thread_attr);
+    param.sched_priority = APM_LINUX_RCIN_PRIORITY;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+    pthread_create(&_rcin_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_rcin_thread, this);
+  
     // the IO thread runs at lower priority
     pthread_attr_init(&thread_attr);
     param.sched_priority = APM_LINUX_IO_PRIORITY;
@@ -254,6 +264,20 @@ void LinuxScheduler::_run_io(void)
     _in_io_proc = false;
 }
 
+void *LinuxScheduler::_rcin_thread(void)
+{
+    _setup_realtime(32768);
+    while (system_initializing()) {
+        poll(NULL, 0, 1);        
+    }
+    while (true) {
+        _microsleep(10000);
+
+        ((LinuxRCInput *)hal.rcin)->_timer_tick();
+    }
+    return NULL;
+}
+
 void *LinuxScheduler::_uart_thread(void)
 {
     _setup_realtime(32768);

libraries/AP_HAL_Linux/Scheduler.h

@@ -68,10 +68,12 @@ private:
 
     pthread_t _timer_thread_ctx;
     pthread_t _io_thread_ctx;
+    pthread_t _rcin_thread_ctx;
     pthread_t _uart_thread_ctx;
 
     void *_timer_thread(void);
     void *_io_thread(void);
+    void *_rcin_thread(void);
     void *_uart_thread(void);
 
     void _run_timers(bool called_from_timer_thread);