Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
frsky_x.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Tom van Dijk <tomvand@users.noreply.github.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
24 
25 #include <string.h>
26 
28 
29 
30 /* Serial device */
31 // Based on usb_ser_hw.c
32 // Generic fifo implementation
33 static void fifo_init(fifo_t *fifo, uint8_t *buf, size_t size);
34 static bool fifo_put(fifo_t *fifo, uint8_t c);
35 static bool fifo_get(fifo_t *fifo, uint8_t *pc);
36 static int fifo_avail(fifo_t *fifo);
37 static int fifo_free(fifo_t *fifo);
38 
39 static void fifo_init(fifo_t *fifo, uint8_t *buf, size_t size) {
40  fifo->head = 0;
41  fifo->tail = 0;
42  fifo->buf = buf;
43  fifo->size = size;
44 }
45 
46 static bool fifo_put(fifo_t *fifo, uint8_t c) {
47  int next;
48 
49  // check if FIFO has room
50  next = (fifo->head + 1) % fifo->size;
51  if (next == fifo->tail) {
52  // full
53  return false;
54  }
55 
56  fifo->buf[fifo->head] = c;
57  fifo->head = next;
58 
59  return true;
60 }
61 
62 static bool fifo_get(fifo_t *fifo, uint8_t *pc) {
63  int next;
64 
65  // check if FIFO has data
66  if (fifo->head == fifo->tail) {
67  return false;
68  }
69 
70  next = (fifo->tail + 1) % fifo->size;
71 
72  *pc = fifo->buf[fifo->tail];
73  fifo->tail = next;
74 
75  return true;
76 }
77 
78 static int fifo_avail(fifo_t *fifo) {
79  return (fifo->size + fifo->head - fifo->tail) % fifo->size;
80 }
81 
82 static int fifo_free(fifo_t *fifo) {
83  return (fifo->size - 1 - fifo_avail(fifo));
84 }
85 
86 
87 // Serial device functions
88 static int frsky_x_serial_check_free_space(void *p, long *fd, uint16_t len);
89 static void frsky_x_serial_put_byte(void *p, long fd, uint8_t c);
90 static void frsky_x_serial_put_buffer(void *p, long fd, const uint8_t *data, uint16_t len);
91 static void frsky_x_serial_send_message(void *p, long fd);
92 static int frsky_x_serial_char_available(void *p);
93 static uint8_t frsky_x_serial_get_byte(void *p);
94 
95 static int frsky_x_serial_check_free_space(void *p, long *fd, uint16_t len) {
96  (void) fd;
97  return (fifo_free(&((struct frsky_x_serial_periph *)p)->downlink_fifo) >= len ? true : false);
98 }
99 
100 static void frsky_x_serial_put_byte(void *p, long fd, uint8_t c) {
101  (void) fd;
102  fifo_put(&((struct frsky_x_serial_periph *)p)->downlink_fifo, c);
103 }
104 
105 static void frsky_x_serial_put_buffer(void *p, long fd, const uint8_t *data, uint16_t len) {
106  for (int i = 0; i < len; ++i) {
107  frsky_x_serial_put_byte(p, fd, data[i]);
108  }
109 }
110 
111 static void frsky_x_serial_send_message(void *p, long fd) {
112  (void) p;
113  (void) fd;
114  /* Do nothing, handled by smartPortDownlink_cb */
115 }
116 
117 static int frsky_x_serial_char_available(void *p) {
118  return fifo_avail(&((struct frsky_x_serial_periph *)p)->uplink_fifo);
119 }
120 
122  uint8_t byte;
123  fifo_get(&((struct frsky_x_serial_periph *)p)->uplink_fifo, &byte);
124  return byte;
125 }
126 
127 
128 /* SmartPort sensor */
129 static bool smartPortDownlink_cb(uint32_t *data) {
131  uint8_t data_u8[4];
132  fifo_get(&frsky_x_serial.downlink_fifo, data_u8 + 0);
133  fifo_get(&frsky_x_serial.downlink_fifo, data_u8 + 1);
134  fifo_get(&frsky_x_serial.downlink_fifo, data_u8 + 2);
135  fifo_get(&frsky_x_serial.downlink_fifo, data_u8 + 3);
136  *data =
137  (data_u8[0] << 24) |
138  (data_u8[1] << 16) |
139  (data_u8[2] << 8) |
140  (data_u8[3]);
141  return true;
142  }
143  return false;
144 }
145 
146 
147 static void smartPortUplink_cb(smartPortPayload_t *payload) {
148  uint8_t shift = 24;
149  for (uint8_t i = 0; i < payload->frameId; ++i) {
150  fifo_put(&frsky_x_serial.uplink_fifo, (uint8_t)((payload->data >> shift) & 0xFF));
151  shift -= 8;
152  }
153 }
154 
155 
157  /* Set up link device */
160  frsky_x_serial.device.periph = (void *)(&frsky_x_serial);
167 
168  /* Attach to SmartPort hooks */
171 }
unsigned short uint16_t
Definition: types.h:16
smartPortDownlinkFn * smartPortDownlink
uint8_t downlink_buf[DOWNLINK_BUFFER_SIZE]
Definition: frsky_x.h:42
static bool fifo_put(fifo_t *fifo, uint8_t c)
Definition: frsky_x.c:46
void datalink_frsky_x_init(void)
Definition: frsky_x.c:156
uint8_t uplink_buf[UPLINK_BUFFER_SIZE]
Definition: frsky_x.h:45
struct link_device device
Generic device interface.
Definition: frsky_x.h:39
static bool fifo_get(fifo_t *fifo, uint8_t *pc)
Definition: frsky_x.c:62
struct frsky_x_serial_periph frsky_x_serial
Definition: frsky_x.c:27
static int frsky_x_serial_check_free_space(void *p, long *fd, uint16_t len)
Definition: frsky_x.c:95
int head
Definition: usb_ser_hw.c:91
#define UPLINK_BUFFER_SIZE
Definition: frsky_x.h:28
unsigned long uint32_t
Definition: types.h:18
uint8_t * buf
Definition: usb_ser_hw.c:93
static uint8_t frsky_x_serial_get_byte(void *p)
Definition: frsky_x.c:121
size_t size
Definition: frsky_x.h:34
static bool smartPortDownlink_cb(uint32_t *data)
Definition: frsky_x.c:129
unsigned char uint8_t
Definition: types.h:14
int tail
Definition: usb_ser_hw.c:92
#define byte
int fd
Definition: serial.c:26
#define DOWNLINK_BUFFER_SIZE
Definition: frsky_x.h:27
static int fifo_free(fifo_t *fifo)
Definition: frsky_x.c:82
static int frsky_x_serial_char_available(void *p)
Definition: frsky_x.c:117
static float p[2][2]
static void frsky_x_serial_put_buffer(void *p, long fd, const uint8_t *data, uint16_t len)
Definition: frsky_x.c:105
static int fifo_avail(fifo_t *fifo)
Definition: frsky_x.c:78
static void frsky_x_serial_send_message(void *p, long fd)
Definition: frsky_x.c:111
fifo_t downlink_fifo
Definition: frsky_x.h:41
static void frsky_x_serial_put_byte(void *p, long fd, uint8_t c)
Definition: frsky_x.c:100
static void fifo_init(fifo_t *fifo, uint8_t *buf, size_t size)
Definition: frsky_x.c:39
smartPortUplinkFn * smartPortUplink
static void smartPortUplink_cb(smartPortPayload_t *payload)
Definition: frsky_x.c:147