HAL_Linux: RCInput support for Zynq

libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp

@@ -0,0 +1,54 @@
+#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 "GPIO.h"
+#include "RCInput.h"
+
+extern const AP_HAL::HAL& hal;
+
+using namespace Linux;
+
+void LinuxRCInput_ZYNQ::init(void*)
+{
+    int mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
+    if (mem_fd == -1) {
+        hal.scheduler->panic("Unable to open /dev/mem");
+    }
+    pulse_input = (volatile uint32_t*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, 
+                                                      MAP_SHARED, mem_fd, RCIN_ZYNQ_PULSE_INPUT_BASE);
+    close(mem_fd);
+
+    _s0_time = 0;
+}
+
+/*
+  called at 1kHz to check for new pulse capture data from the PL pulse timer
+ */
+void LinuxRCInput_ZYNQ::_timer_tick()
+{
+    uint32_t v;
+
+    // all F's means no samples available
+    while((v = *pulse_input) != 0xffffffff) {
+        // Hi bit indicates pin state, low bits denote pulse length
+        if(!(v & 0x80000000))
+            _s0_time = (v & 0x7fffffff)/TICK_PER_US;
+        else
+            _process_rc_pulse(_s0_time, (v & 0x7fffffff)/TICK_PER_US);
+    }
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_Linux/RCInput_ZYNQ.h

@@ -0,0 +1,32 @@
+
+#ifndef __AP_HAL_LINUX_RCINPUT_ZYNQ_H__
+#define __AP_HAL_LINUX_RCINPUT_ZYNQ_H__
+
+/*
+  This class implements RCInput on the ZYNQ / ZyboPilot platform with custom
+  logic doing the edge detection of the PPM sum input
+ */
+
+#include <AP_HAL_Linux.h>
+
+// FIXME A puppie dies when you hard code an address
+#define RCIN_ZYNQ_PULSE_INPUT_BASE  0x43c10000
+
+class Linux::LinuxRCInput_ZYNQ : public Linux::LinuxRCInput 
+{
+public:
+    void init(void*);
+    void _timer_tick(void);
+
+ private:
+    static const int TICK_PER_US=100;
+    static const int TICK_PER_S=100000000;
+
+    // Memory mapped keyhole register to pulse input FIFO
+    volatile uint32_t *pulse_input;
+
+    // time spent in the low state
+    uint32_t _s0_time;
+};
+
+#endif // __AP_HAL_LINUX_RCINPUT_ZYNQ_H__

libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp

@@ -0,0 +1,101 @@
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include "RCOutput_ZYNQ.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <sys/ioctl.h>
+#include <linux/spi/spidev.h>
+#include <sys/mman.h>
+#include <signal.h>
+using namespace Linux;
+
+#define PWM_CHAN_COUNT 8	// FIXME
+
+static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+static void catch_sigbus(int sig)
+{
+    hal.scheduler->panic("RCOutput.cpp:SIGBUS error gernerated\n");
+}
+void LinuxRCOutput_ZYNQ::init(void* machtnicht)
+{
+    uint32_t mem_fd;
+    signal(SIGBUS,catch_sigbus);
+    mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
+    sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ|PROT_WRITE, 
+                                            MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE);
+    close(mem_fd);
+
+    // all outputs default to 50Hz, the top level vehicle code
+    // overrides this when necessary
+    set_freq(0xFFFFFFFF, 50);
+}
+
+void LinuxRCOutput_ZYNQ::set_freq(uint32_t chmask, uint16_t freq_hz)            //LSB corresponds to CHAN_1
+{
+    uint8_t i;
+    unsigned long tick=TICK_PER_S/(unsigned long)freq_hz;
+
+    for (i=0;i<PWM_CHAN_COUNT;i++) {
+        if (chmask & (1U<<i)) {
+            sharedMem_cmd->periodhi[i].period=tick;
+        }
+    }
+}
+
+uint16_t LinuxRCOutput_ZYNQ::get_freq(uint8_t ch)
+{
+    return TICK_PER_S/sharedMem_cmd->periodhi[ch].period;;
+}
+
+void LinuxRCOutput_ZYNQ::enable_ch(uint8_t ch)
+{
+    // sharedMem_cmd->enmask |= 1U<<chan_pru_map[ch];
+}
+
+void LinuxRCOutput_ZYNQ::disable_ch(uint8_t ch)
+{
+    // sharedMem_cmd->enmask &= !(1U<<chan_pru_map[ch]);
+}
+
+void LinuxRCOutput_ZYNQ::write(uint8_t ch, uint16_t period_us)
+{
+    sharedMem_cmd->periodhi[ch].hi = TICK_PER_US*period_us;
+}
+
+void LinuxRCOutput_ZYNQ::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{
+    uint8_t i;
+    if(len>PWM_CHAN_COUNT){
+        len = PWM_CHAN_COUNT;
+    }
+    for(i=0;i<len;i++){
+        write(ch+i,period_us[i]);
+    }
+}
+
+uint16_t LinuxRCOutput_ZYNQ::read(uint8_t ch)
+{
+    return (sharedMem_cmd->periodhi[ch].hi/TICK_PER_US);
+}
+
+void LinuxRCOutput_ZYNQ::read(uint16_t* period_us, uint8_t len)
+{
+    uint8_t i;
+    if(len>PWM_CHAN_COUNT){
+        len = PWM_CHAN_COUNT;
+    }
+    for(i=0;i<len;i++){
+        period_us[i] = sharedMem_cmd->periodhi[i].hi/TICK_PER_US;
+    }
+}
+
+#endif

libraries/AP_HAL_Linux/RCOutput_ZYNQ.h

@@ -0,0 +1,43 @@
+
+#ifndef __AP_HAL_LINUX_RCOUTPUT_ZYNQ_H__
+#define __AP_HAL_LINUX_RCOUTPUT_ZYNQ_H__
+
+#include <AP_HAL_Linux.h>
+#define RCOUT_ZYNQ_PWM_BASE	 0x43c00000	//FIXME hardcoding is the devil's work
+#define MAX_PWMS                 8	//FIXME
+#define PWM_CMD_CONFIG	         0	/* full configuration in one go */
+#define PWM_CMD_ENABLE	         1	/* enable a pwm */
+#define PWM_CMD_DISABLE	         2	/* disable a pwm */
+#define PWM_CMD_MODIFY	         3	/* modify a pwm */
+#define PWM_CMD_SET	         4	/* set a pwm output explicitly */
+#define PWM_CMD_CLR	         5	/* clr a pwm output explicitly */
+#define PWM_CMD_TEST	         6	/* various crap */
+
+
+class Linux::LinuxRCOutput_ZYNQ : public AP_HAL::RCOutput {
+    void     init(void* machtnichts);
+    void     set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void     enable_ch(uint8_t ch);
+    void     disable_ch(uint8_t ch);
+    void     write(uint8_t ch, uint16_t period_us);
+    void     write(uint8_t ch, uint16_t* period_us, uint8_t len);
+    uint16_t read(uint8_t ch);
+    void     read(uint16_t* period_us, uint8_t len);
+
+private:
+    static const int TICK_PER_US=100;
+    static const int TICK_PER_S=100000000;
+
+    // Period|Hi 32 bits each
+    struct s_period_hi {
+        uint32_t period;
+        uint32_t hi;
+    };
+    struct pwm_cmd {
+        struct s_period_hi periodhi[MAX_PWMS];
+    };
+    volatile struct pwm_cmd *sharedMem_cmd;
+};
+
+#endif // __AP_HAL_LINUX_RCOUTPUT_ZYNQ_H__