Paparazzi UAS  v5.15_devel-164-g81d4ceb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
syslink_dl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
29 #include "subsystems/electrical.h"
30 #include "mcu_periph/uart.h"
31 #include <string.h>
32 #include "led.h"
33 
35 
37 #include "pprz_mutex.h"
38 PPRZ_MUTEX(syslink_tx_mtx);
39 
43 {
45  uint8_t buf[sizeof(syslink_message_t)];
46  buf[0] = syslink_stx[0];
47  buf[1] = syslink_stx[1];
48  buf[2] = msg->type;
49  buf[3] = msg->length;
50  for (int i = 0; i < msg->length; i++) {
51  buf[4+i] = msg->data[i];
52  }
53  buf[msg->length+4] = msg->cksum[0];
54  buf[msg->length+5] = msg->cksum[1];
55  uart_put_buffer(&(SYSLINK_DEV), 0, buf, msg->length+6);
56  //uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)syslink_stx, 2);
57  //uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->type), sizeof(msg->type));
58  //uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->length), sizeof(msg->length));
59  //uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->data), msg->length);
60  //uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->cksum), sizeof(msg->cksum));
61 }
62 
67 {
68  if (msg->length != 9) {
69  return;
70  }
71 
72  // check flag
73  uint8_t flags = msg->data[0];
74  syslink.charging = (bool) (flags & 1);
75  syslink.powered = (bool) (flags & 2);
76 
77  // update voltage
78  float vbat;
79  memcpy(&vbat, &msg->data[1], sizeof(float));
80  if (syslink.powered && vbat > 3.f) {
81  electrical.vsupply = vbat; // remove 0 reading when powered on USB ?
82  } else if (!syslink.powered) {
83  electrical.vsupply = vbat; // running on battery
84  }
85 }
86 
91 {
92  // This function doesn't actually do anything
93  // It is just here to return null responses to most standard messages
94  // Setup order from the cflib-python is
95  // - platform link source (name, protocol version)
96  // - log TOC
97  // - mem update
98  // - param TOC
99  // -> call connected callback and start getting data
100  // - update param if needed
101 
102  crtp_message_t *c = (crtp_message_t *) &msg->length;
103 
104  if (c->port == CRTP_PORT_LOG) {
105  if (c->channel == 0) { // Table of Contents Access
106  uint8_t cmd = c->data[0];
107  if (cmd == 0) { // GET_ITEM
108  //int id = c->data[1];
109  memset(&c->data[2], 0, 3);
110  c->data[2] = 1; // type
111  c->size = 1 + 5;
112  send_message(msg);
113  } else if (cmd == 1) { // GET_INFO
114  memset(&c->data[1], 0, 7);
115  c->size = 1 + 8;
116  send_message(msg);
117  }
118  }
119  else if (c->channel == 1) { // Log control
120  c->data[2] = 0; // Success
121  c->size = 3 + 1;
122  // resend message
123  send_message(msg);
124  }
125  else if (c->channel == 2) { // Log data
126  // nothing
127  }
128  }
129  else if (c->port == CRTP_PORT_MEM) {
130  if (c->channel == 0) { // Info
131  int cmd = c->data[0];
132  if (cmd == 1) { // GET_NBR_OF_MEMS
133  c->data[1] = 0;
134  c->size = 2 + 1;
135  // resend message
136  send_message(msg);
137  }
138  }
139  }
140  else if (c->port == CRTP_PORT_PARAM) {
141  if (c->channel == 0) { // TOC Access
142  c->data[1] = 0; // Last parameter (id = 0)
143  memset(&c->data[2], 0, 10);
144  c->size = 1 + 8;
145  send_message(msg);
146  }
147  else if (c->channel == 1) { // Param read
148  // 0 is ok
149  c->data[1] = 0; // value
150  c->size = 1 + 2;
151  send_message(msg);
152  }
153  }
154  else if (c->port == CRTP_PORT_LINK) {
155  if (c->channel == 0) { // Echo
156  send_message(msg);
157  }
158  else if (c->channel == 1) { // Reply platform name
160  bzero(c->data, CRTP_MAX_DATA_SIZE);
161  strcpy((char *)c->data, "Bitcraze PPRZ");
162  send_message(msg);
163  }
164  }
165  else {
166  // TODO handle error ?
167  }
168 }
169 
187 {
188  crtp_message_t *c = (crtp_message_t *) &msg->length;
189 
190  if (CRTP_NULL(*c)) {
191  if (c->size >= 3) {
192  //handle_bootloader(sys);
193  }
194  }
195  else if (c->port == CRTP_PORT_COMMANDER) {
196  //crtp_commander *cmd = (crtp_commander *) &c->data[0];
197  // TODO set RC from cmd message
198  }
199  else if (c->port == CRTP_PORT_PPRZLINK) {
200  // fill rx buffer with CRTP data
201  uint8_t data_size = c->size;
202  uint16_t available = SYSLINK_RX_BUF_LEN - syslink.device.char_available(&syslink);
203  if (available > data_size) {
204  // enough room to add new bytes
206  // copy in one block
207  memcpy(&syslink.rx_buf[syslink.rx_insert_idx], c->data, data_size);
208  } else {
209  // copy in two parts
211  memcpy(&syslink.rx_buf[syslink.rx_insert_idx], c->data, split);
212  memcpy(&syslink.rx_buf[0], &c->data[split], data_size - split);
213  }
215  }
216  }
217  else {
218  handle_raw_other(msg);
219  }
220 
221  // send next raw message if fifo is not empty
223  PPRZ_MUTEX_LOCK(syslink_tx_mtx);
224  syslink_message_t msg_raw;
225  msg_raw.type = SYSLINK_RADIO_RAW;
226  memcpy(&msg_raw.length, &syslink.msg_tx[syslink.tx_extract_idx], sizeof(crtp_message_t));
227  send_message(&msg_raw);
228  // move fifo indexes
232  }
233  PPRZ_MUTEX_UNLOCK(syslink_tx_mtx);
234  }
235 
236 }
237 
239 {
240  if (msg->type == SYSLINK_RADIO_RSSI) {
241  uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
242  syslink.rssi = 140 - rssi * 100 / (100 - 40);
243  }
244  else if (msg->type == SYSLINK_RADIO_CHANNEL) {
245  // ack radio channel
246  }
247  else if (msg->type == SYSLINK_RADIO_DATARATE) {
248  // ack radio datarate
249  }
250  else if (msg->type == SYSLINK_RADIO_ADDRESS) {
251  // ack radio address
252  }
253 }
254 
255 
259 {
260  if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
261  // power button is hit
262  // don't do anything for now ?
263  }
264  else if (msg->type == SYSLINK_PM_BATTERY_STATE) {
265  handle_battery(msg);
266  }
267  else if (msg->type == SYSLINK_RADIO_RAW) {
268  handle_raw(msg);
269  }
270  else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
271  handle_radio(msg);
272  }
273  else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
274  // one-wire memory access
275  // don't do anything for now
276  }
277  else {
278  // handle errors ?
279  }
280 }
281 
282 
287 // check free space: nb of CRTP slots x space in a slot
288 static bool syslink_check_free_space(struct syslink_dl *s, long *fd UNUSED, uint16_t len)
289 {
290  int16_t slots = s->tx_extract_idx - s->tx_insert_idx;
291  if (slots <= 0) {
292  slots += CRTP_BUF_LEN;
293  }
294  uint16_t space = (uint16_t)(CRTP_MAX_DATA_SIZE * (slots - 1)) >= len;
295  if (space > 0) {
296  PPRZ_MUTEX_LOCK(syslink_tx_mtx);
297  }
298  return space;
299 }
300 
301 // implementation of put_buffer, fill CRTP slots
302 static void syslink_put_buffer(struct syslink_dl *s, long fd UNUSED, const uint8_t *data, uint16_t len)
303 {
304  uint16_t buf_rem = len;
305  // fill slots until they are full
306  // we assume that the available space have been check before
307  while (buf_rem > 0) {
308  // get current slot
309  crtp_message_t *c = &s->msg_tx[s->tx_insert_idx];
310  if (c->size < CRTP_MAX_DATA_SIZE) {
311  // fill current buffer
312  uint16_t data_size = Min(buf_rem, CRTP_MAX_DATA_SIZE - c->size);
313  memcpy(&c->data[c->size - sizeof(c->header)], &data[len - buf_rem], data_size);
314  c->size += data_size;
315  buf_rem -= data_size;
316  }
317  else {
318  // start a new slot
319  uint8_t tmp = (s->tx_insert_idx + 1) % CRTP_BUF_LEN;
320  if (tmp == s->tx_extract_idx) {
321  // no more slots
322  // this should be be possible when check_free_space is called before
323  return;
324  }
325  crtp_message_t *c = &s->msg_tx[tmp];
326  uint16_t data_size = Min(buf_rem, CRTP_MAX_DATA_SIZE);
327  memcpy(c->data, &data[len - buf_rem], data_size);
328  c->size = sizeof(c->header) + data_size;
329  buf_rem -= data_size;
330  s->tx_insert_idx = tmp;
331  }
332  }
333 }
334 
335 // implementation of put_byte using put_buffer
336 static void syslink_put_byte(struct syslink_dl *s, long fd, const uint8_t b)
337 {
338  syslink_put_buffer(s, fd, &b, 1);
339 }
340 
341 // send_message is not needed as messages are stored in a fifo
342 static void syslink_send_message(struct syslink_dl *s UNUSED, long fd UNUSED)
343 {
344  PPRZ_MUTEX_UNLOCK(syslink_tx_mtx); // release mutex
345 }
346 
348 {
349  uint8_t ret = s->rx_buf[s->rx_extract_idx];
351  return ret;
352 }
353 
355 {
356  int16_t available = s->rx_insert_idx - s->rx_extract_idx;
357  if (available < 0) {
358  available += SYSLINK_RX_BUF_LEN;
359  }
360  return (uint16_t)available;
361 }
362 
363 
365 void syslink_dl_init(void)
366 {
370  syslink.rssi = 0;
371  syslink.charging = false;
372  syslink.powered = false;
373 
374  for (int i = 0; i < CRTP_BUF_LEN; i++) {
375  // prepare raw pprzlink datalink headers
376  syslink.msg_tx[i].header = 0;
378  syslink.msg_tx[i].channel = i % 4;
379  syslink.msg_tx[i].size = sizeof(syslink.msg_tx[i].header);
380  }
381 
382  // generic device
383  syslink.device.periph = (void *)(&syslink);
384  syslink.device.check_free_space = (check_free_space_t) syslink_check_free_space;
385  syslink.device.put_byte = (put_byte_t) syslink_put_byte;
386  syslink.device.put_buffer = (put_buffer_t) syslink_put_buffer;
387  syslink.device.send_message = (send_message_t) syslink_send_message;
388  syslink.device.char_available = (char_available_t) syslink_char_available;
389  syslink.device.get_byte = (get_byte_t) syslink_getch;
390 
391  // init mutex if needed
392  PPRZ_MUTEX_INIT(syslink_tx_mtx);
393 }
394 
397 {
398 #ifdef CHARGING_LED
399  if (syslink.charging) {
400  LED_TOGGLE(CHARGING_LED);
401  }
402  else if (syslink.powered) {
403  LED_ON(CHARGING_LED);
404  }
405  else {
406  LED_OFF(CHARGING_LED);
407  }
408 #endif
409 }
410 
413 {
414  while (uart_char_available(&(SYSLINK_DEV))) {
415  uint8_t ch = uart_getch(&(SYSLINK_DEV));
418  }
419  }
420 }
421 
422 
#define CRTP_PORT_PPRZLINK
Definition: crtp.h:40
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define CRTP_PORT_PARAM
Definition: crtp.h:35
static uint32_t s
#define Min(x, y)
Definition: esc_dshot.c:85
#define CRTP_MAX_DATA_SIZE
Definition: crtp.h:50
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:867
#define CRTP_PORT_MEM
Definition: crtp.h:37
#define LED_OFF(i)
Definition: led_hw.h:52
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
uint8_t last_wp UNUSED
Definition: navigation.c:92
void uart_put_buffer(struct uart_periph *p, long fd, const uint8_t *data, uint16_t len)
Uart transmit buffer implementation.
Definition: uart_arch.c:1021
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
float vsupply
supply voltage in V
Definition: electrical.h:45
uint8_t data[CRTP_MAX_DATA_SIZE]
Definition: crtp.h:63
Interface for electrical status: supply voltage, current, battery status, etc.
uint8_t size
Definition: crtp.h:53
signed short int16_t
Definition: types.h:17
#define CRTP_PORT_COMMANDER
Definition: crtp.h:36
#define CRTP_PORT_LINK
Definition: crtp.h:44
#define PPRZ_MUTEX_INIT(_mtx)
Definition: pprz_mutex.h:45
#define LED_TOGGLE(i)
Definition: led_hw.h:53
Utility functions and macros to abstract some RTOS functionalities such as mutexes.
unsigned char uint8_t
Definition: types.h:14
int fd
Definition: serial.c:26
arch independent LED (Light Emitting Diodes) API
#define LED_ON(i)
Definition: led_hw.h:51
struct Electrical electrical
Definition: electrical.c:66
#define CRTP_NULL(x)
Definition: crtp.h:46
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
#define CRTP_PORT_LOG
Definition: crtp.h:38