APM:Libraries
RCOutput_ZYNQ.cpp
Go to the documentation of this file.
1 
2 #include <AP_HAL/AP_HAL.h>
3 
4 #include "RCOutput_ZYNQ.h"
5 
6 #include <dirent.h>
7 #include <fcntl.h>
8 #include <linux/spi/spidev.h>
9 #include <signal.h>
10 #include <stdint.h>
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <sys/ioctl.h>
14 #include <sys/mman.h>
15 #include <sys/stat.h>
16 #include <sys/types.h>
17 #include <unistd.h>
18 
19 using namespace Linux;
20 
21 #define PWM_CHAN_COUNT 8
22 #define RCOUT_ZYNQ_PWM_BASE 0x43c00000
23 #define PWM_CMD_CONFIG 0 /* full configuration in one go */
24 #define PWM_CMD_ENABLE 1 /* enable a pwm */
25 #define PWM_CMD_DISABLE 2 /* disable a pwm */
26 #define PWM_CMD_MODIFY 3 /* modify a pwm */
27 #define PWM_CMD_SET 4 /* set a pwm output explicitly */
28 #define PWM_CMD_CLR 5 /* clr a pwm output explicitly */
29 #define PWM_CMD_TEST 6 /* various crap */
30 
31 
32 static void catch_sigbus(int sig)
33 {
34  AP_HAL::panic("RCOutput.cpp:SIGBUS error gernerated\n");
35 }
37 {
38  uint32_t mem_fd;
39  signal(SIGBUS,catch_sigbus);
40  mem_fd = open("/dev/mem", O_RDWR|O_SYNC|O_CLOEXEC);
41  sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ|PROT_WRITE,
42  MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE);
43  close(mem_fd);
44 
45  // all outputs default to 50Hz, the top level vehicle code
46  // overrides this when necessary
47  set_freq(0xFFFFFFFF, 50);
48 }
49 
50 void RCOutput_ZYNQ::set_freq(uint32_t chmask, uint16_t freq_hz) //LSB corresponds to CHAN_1
51 {
52  uint8_t i;
53  unsigned long tick=TICK_PER_S/(unsigned long)freq_hz;
54 
55  for (i=0;i<PWM_CHAN_COUNT;i++) {
56  if (chmask & (1U<<i)) {
57  sharedMem_cmd->periodhi[i].period=tick;
58  }
59  }
60 }
61 
62 uint16_t RCOutput_ZYNQ::get_freq(uint8_t ch)
63 {
64  if (ch >= PWM_CHAN_COUNT) {
65  return 0;
66  }
67 
69 }
70 
71 void RCOutput_ZYNQ::enable_ch(uint8_t ch)
72 {
73  // sharedMem_cmd->enmask |= 1U<<chan_pru_map[ch];
74 }
75 
76 void RCOutput_ZYNQ::disable_ch(uint8_t ch)
77 {
78  // sharedMem_cmd->enmask &= !(1U<<chan_pru_map[ch]);
79 }
80 
81 void RCOutput_ZYNQ::write(uint8_t ch, uint16_t period_us)
82 {
83  if (ch >= PWM_CHAN_COUNT) {
84  return;
85  }
86 
87  if (corked) {
88  pending[ch] = period_us;
89  pending_mask |= (1U << ch);
90  } else {
91  sharedMem_cmd->periodhi[ch].hi = TICK_PER_US*period_us;
92  }
93 }
94 
95 uint16_t RCOutput_ZYNQ::read(uint8_t ch)
96 {
97  if (ch >= PWM_CHAN_COUNT) {
98  return 0;
99  }
100 
101  return sharedMem_cmd->periodhi[ch].hi/TICK_PER_US;
102 }
103 
104 void RCOutput_ZYNQ::read(uint16_t* period_us, uint8_t len)
105 {
106  uint8_t i;
107  if(len>PWM_CHAN_COUNT){
108  len = PWM_CHAN_COUNT;
109  }
110  for(i=0;i<len;i++){
111  period_us[i] = sharedMem_cmd->periodhi[i].hi/TICK_PER_US;
112  }
113 }
114 
116 {
117  corked = true;
118 }
119 
121 {
122  if (!corked) {
123  return;
124  }
125  corked = false;
126  for (uint8_t i=0; i<MAX_ZYNQ_PWMS; i++) {
127  if (pending_mask & (1U << i)) {
128  write(i, pending[i]);
129  }
130  }
131  pending_mask = 0;
132 }
#define RCOUT_ZYNQ_PWM_BASE
#define PWM_CHAN_COUNT
void set_freq(uint32_t chmask, uint16_t freq_hz)
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
Definition: posix.c:885
void cork(void) override
void enable_ch(uint8_t ch)
void write(uint8_t ch, uint16_t period_us)
static const int TICK_PER_S
Definition: RCOutput_ZYNQ.h:25
int close(int fileno)
POSIX Close a file with fileno handel.
Definition: posix.c:675
uint16_t get_freq(uint8_t ch)
static const int TICK_PER_US
Definition: RCOutput_ZYNQ.h:24
struct s_period_hi periodhi[MAX_ZYNQ_PWMS]
Definition: RCOutput_ZYNQ.h:37
#define MAX_ZYNQ_PWMS
Definition: RCOutput_ZYNQ.h:7
void disable_ch(uint8_t ch)
static void catch_sigbus(int sig)
volatile struct pwm_cmd * sharedMem_cmd
Definition: RCOutput_ZYNQ.h:39
uint16_t read(uint8_t ch)
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
uint16_t pending[MAX_ZYNQ_PWMS]
Definition: RCOutput_ZYNQ.h:41
void push(void) override