diff --git a/libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eb3938c5791a70ea3d5e1e37b4f45f0b424ef9b1 --- /dev/null +++ b/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 diff --git a/libraries/AP_HAL_Linux/RCInput_ZYNQ.h b/libraries/AP_HAL_Linux/RCInput_ZYNQ.h new file mode 100644 index 0000000000000000000000000000000000000000..198dc0b3aca11d89839761b7c095ba2b8c57cffb --- /dev/null +++ b/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__ diff --git a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b66e452658f16ffe493f8c2ef0222c318668ae7c --- /dev/null +++ b/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 diff --git a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.h b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.h new file mode 100644 index 0000000000000000000000000000000000000000..530f2934b35ef51b10b68feef451f9b598ca0cc7 --- /dev/null +++ b/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__