From 9a73df1bea90cf3e8d459c5336dd4f1f7f233ca3 Mon Sep 17 00:00:00 2001
From: raspilot <raspilot.io@gmail.com>
Date: Tue, 18 Aug 2015 12:29:11 +1000
Subject: [PATCH] AP_HAL_Linux: added support for raspilot RCInput

---
 libraries/AP_HAL_Linux/RCInput_Raspilot.cpp | 75 +++++++++++++++++++++
 libraries/AP_HAL_Linux/RCInput_Raspilot.h   | 22 ++++++
 2 files changed, 97 insertions(+)
 create mode 100644 libraries/AP_HAL_Linux/RCInput_Raspilot.cpp
 create mode 100644 libraries/AP_HAL_Linux/RCInput_Raspilot.h

diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp
new file mode 100644
index 0000000000..304a109d5a
--- /dev/null
+++ b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp
@@ -0,0 +1,75 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <assert.h>
+
+#include "RCInput_Raspilot.h"
+
+#include "px4io_protocol.h"
+
+static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+using namespace Linux;
+
+void LinuxRCInput_Raspilot::init(void*)
+{
+    _spi = hal.spi->device(AP_HAL::SPIDevice_RASPIO);
+    _spi_sem = _spi->get_semaphore();
+
+    if (_spi_sem == NULL) {
+        hal.scheduler->panic(PSTR("PANIC: RCIutput_Raspilot did not get "
+                                  "valid SPI semaphore!"));
+        return; // never reached
+    }
+
+    // start the timer process to read samples
+    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&LinuxRCInput_Raspilot::_poll_data, void));
+}
+
+void LinuxRCInput_Raspilot::_poll_data(void)
+{
+    // Throttle read rate to 100hz maximum.
+    if (hal.scheduler->micros() - _last_timer < 10000) {
+        return;
+    }
+
+    _last_timer = hal.scheduler->micros();
+
+    if (!_spi_sem->take_nonblocking()) {
+        return;
+    }
+
+    struct IOPacket _dma_packet_tx, _dma_packet_rx;
+    uint16_t count = LINUX_RC_INPUT_NUM_CHANNELS;
+    _dma_packet_tx.count_code = count | PKT_CODE_READ;
+    _dma_packet_tx.page = 4;
+    _dma_packet_tx.offset = 0;
+    _dma_packet_tx.crc = 0;
+    _dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
+    /* set raspilotio to read reg4 */
+    _spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx));
+    /* get reg4 data from raspilotio */
+    _spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx));
+
+    uint16_t num_values = _dma_packet_rx.regs[0];
+    uint16_t rc_ok = _dma_packet_rx.regs[1] & (1 << 4);
+    uint8_t rx_crc = _dma_packet_rx.crc;
+
+    _dma_packet_rx.crc = 0;
+
+    if ( rc_ok && (rx_crc == crc_packet(&_dma_packet_rx)) ) {
+      _update_periods(&_dma_packet_rx.regs[6], (uint8_t)num_values);
+    }
+
+    _spi_sem->give();
+}
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE
diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.h b/libraries/AP_HAL_Linux/RCInput_Raspilot.h
new file mode 100644
index 0000000000..80ebb2b132
--- /dev/null
+++ b/libraries/AP_HAL_Linux/RCInput_Raspilot.h
@@ -0,0 +1,22 @@
+
+#ifndef __AP_HAL_LINUX_RCINPUT_RASPILOT_H__
+#define __AP_HAL_LINUX_RCINPUT_RASPILOT_H__
+
+#include "AP_HAL_Linux.h"
+#include "RCInput.h"
+
+class Linux::LinuxRCInput_Raspilot : public Linux::LinuxRCInput
+{
+public:
+    void init(void*);
+    
+private:
+    uint32_t _last_timer;
+    
+    AP_HAL::SPIDeviceDriver *_spi;
+    AP_HAL::Semaphore *_spi_sem;
+    
+    void _poll_data(void);
+};
+
+#endif // __AP_HAL_LINUX_RCINPUT_RASPILOT_H__