Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
jevois.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 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  */
27 
28 #include "std.h"
29 #include "mcu_periph/uart.h"
30 #include "subsystems/abi.h"
33 #include <string.h>
34 #include <stdio.h>
35 
38 
39 // max string length
40 #define JEVOIS_MAX_LEN 32
41 // max number of coordinates
42 #define JEVOIS_MAX_COORD 18
43 // check delimiter
44 #define JEVOIS_CHECK_DELIM(_c) (_c == ' ' || _c == '\n' || _c == '\r' || _c == '\0')
45 
46 // generic JEVOIS message structure
47 struct jevois_msg_t {
49  char id[JEVOIS_MAX_LEN];
53  struct FloatQuat quat;
55 };
56 
57 // decoder state
59  JV_SYNC = 0,
68 };
69 
70 // jevois struct
71 struct jevois_t {
72  enum jevois_state state; // decoder state
73  char buf[JEVOIS_MAX_LEN]; // temp buffer
74  uint8_t idx; // temp buffer index
75  uint8_t n; // temp coordinates/dimension index
76  struct jevois_msg_t msg; // last decoded message
77  bool data_available; // new data to report
78 };
79 
81 
82 // reporting function, send telemetry message
83 void jevois_report(void)
84 {
85  if (jevois.data_available == false) {
86  // no new data, return
87  return;
88  }
89 
90  float quat[4] = {
91  jevois.msg.quat.qi,
92  jevois.msg.quat.qx,
93  jevois.msg.quat.qy,
95  };
96  uint8_t len = strlen(jevois.msg.id);
97  char none[] = "None";
98  char *id = jevois.msg.id;
99  if (len == 0) {
100  id = none;
101  len = 4;
102  }
103  DOWNLINK_SEND_JEVOIS(DefaultChannel, DefaultDevice,
104  &jevois.msg.type,
105  len, id,
106  &jevois.msg.nb,
108  jevois.msg.dim,
109  quat);
110  jevois.data_available = false;
111 }
112 
113 // initialization
114 void jevois_init(void)
115 {
116  // dummy settings
118  jevois_stream_setting = false;
119 
120  jevois.state = JV_SYNC;
121  jevois.idx = 0;
122  jevois.n = 0;
123  jevois.data_available = false;
124  memset(jevois.buf, 0, JEVOIS_MAX_LEN);
125 }
126 
127 // send specific message if requested
128 static void jevois_send_message(void)
129 {
130 #if JEVOIS_SEND_MSG
131  // send pprzlink JEVOIS message
132  jevois_report();
133 #endif
134 #if JEVOIS_SEND_FOLLOW_TARGET
135  float cam_heading = (JEVOIS_HFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[0]);
136  float cam_height = (JEVOIS_VFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[1]);
137  // send a FOLLOW_TARGET message in camera frame
138  AbiSendMsgFOLLOW_TARGET(CAM_JEVOIS_ID, 0, 0, cam_heading, cam_height, 0.f);
139 #endif
140 }
141 
142 // raw message parsing function
143 static void jevois_parse(struct jevois_t *jv, char c)
144 {
145  switch (jv->state) {
146  case JV_SYNC:
147  // wait for sync (newline character)
148  if (c == '\n') {
149  jv->state = JV_TYPE;
150  jv->idx = 0;
151  jv->n = 0;
152  }
153  break;
154  case JV_TYPE:
155  jv->buf[jv->idx++] = c; // fill buffer
156  // parse type
157  if (jv->idx > 2) { // msg type + white space
158  if (jv->buf[0] == 'T' && jv->buf[1] == '1') {
159  jv->state = JV_COORD;
160  jv->msg.type = JEVOIS_MSG_T1;
161  jv->msg.nb = 1;
162  } else if (jv->buf[0] == 'N' && jv->buf[1] == '1') {
163  jv->state = JV_ID;
164  jv->msg.type = JEVOIS_MSG_N1;
165  jv->msg.nb = 1;
166  } else if (jv->buf[0] == 'D' && jv->buf[1] == '1') {
167  jv->state = JV_ID;
168  jv->msg.type = JEVOIS_MSG_D1;
169  jv->msg.nb = 2;
170  } else if (jv->buf[0] == 'T' && jv->buf[1] == '2') {
171  jv->state = JV_COORD;
172  jv->msg.type = JEVOIS_MSG_T2;
173  jv->msg.nb = 2;
174  } else if (jv->buf[0] == 'N' && jv->buf[1] == '2') {
175  jv->state = JV_ID;
176  jv->msg.type = JEVOIS_MSG_N2;
177  jv->msg.nb = 2;
178  } else if (jv->buf[0] == 'D' && jv->buf[1] == '2') {
179  jv->state = JV_ID;
180  jv->msg.type = JEVOIS_MSG_D2;
181  jv->msg.nb = 8;
182  } else if (jv->buf[0] == 'F' && jv->buf[1] == '2') {
183  jv->state = JV_ID;
184  jv->msg.type = JEVOIS_MSG_F2;
185  jv->msg.nb = 0;
186  } else if (jv->buf[0] == 'T' && jv->buf[1] == '3') {
187  jv->state = JV_COORD;
188  jv->msg.type = JEVOIS_MSG_T3;
189  jv->msg.nb = 3;
190  } else if (jv->buf[0] == 'N' && jv->buf[1] == '3') {
191  jv->state = JV_ID;
192  jv->msg.type = JEVOIS_MSG_N3;
193  jv->msg.nb = 3;
194  } else if (jv->buf[0] == 'D' && jv->buf[1] == '3') {
195  jv->state = JV_ID;
196  jv->msg.type = JEVOIS_MSG_D3;
197  jv->msg.nb = 3;
198  } else if (jv->buf[0] == 'F' && jv->buf[1] == '3') {
199  jv->state = JV_ID;
200  jv->msg.type = JEVOIS_MSG_F3;
201  jv->msg.nb = 0;
202  } else {
203  jv->state = JV_SYNC; // error
204  }
205  jv->idx = 0;
206  }
207  break;
208  case JV_ID:
209  if (JEVOIS_CHECK_DELIM(c)) {
210  jv->msg.id[jv->idx] = '\0'; // end string
211  if (jv->msg.type == JEVOIS_MSG_F2 ||
212  jv->msg.type == JEVOIS_MSG_F3) {
213  jv->state = JV_SIZE; // parse n before coordinates
214  } else {
215  jv->state = JV_COORD; // parse directly coordinates
216  }
217  jv->idx = 0;
218  break;
219  }
220  else {
221  jv->msg.id[jv->idx++] = c;
222  if (jv->idx > JEVOIS_MAX_LEN - 1) {
223  jv->state = JV_SYNC; // too long, return to sync
224  }
225  }
226  break;
227  case JV_SIZE:
228  if (JEVOIS_CHECK_DELIM(c)) {
229  jv->buf[jv->idx] = '\0'; // end string
230  jv->msg.nb = (uint8_t)atoi(jv->buf); // store size
231  jv->state = JV_COORD;
232  jv->idx = 0;
233  }
234  else {
235  jv->buf[jv->idx++] = c; // fill buffer
236  }
237  break;
238  case JV_COORD:
239  if (JEVOIS_CHECK_DELIM(c)) {
240  jv->buf[jv->idx] = '\0'; // end string
241  jv->msg.coord[jv->n++] = (int16_t)atoi(jv->buf); // store value
242  if (jv->n == jv->msg.nb) {
243  // got all coordinates, go to next state
244  jv->n = 0; // reset number of received elements
245  jv->idx = 0; // reset index
246  switch (jv->msg.type) {
247  case JEVOIS_MSG_T1:
248  case JEVOIS_MSG_T2:
249  case JEVOIS_MSG_T3:
250  jv->state = JV_SEND_MSG;
251  break;
252  case JEVOIS_MSG_N1:
253  case JEVOIS_MSG_N2:
254  case JEVOIS_MSG_N3:
255  case JEVOIS_MSG_D3:
256  jv->state = JV_DIM;
257  break;
258  case JEVOIS_MSG_D1:
259  case JEVOIS_MSG_D2:
260  case JEVOIS_MSG_F2:
261  case JEVOIS_MSG_F3:
262  jv->state = JV_EXTRA;
263  break;
264  default:
265  jv->state = JV_SYNC; // error
266  break;
267  }
268  }
269  jv->idx = 0; // reset index
270  }
271  else {
272  jv->buf[jv->idx++] = c; // fill buffer
273  }
274  break;
275  case JV_DIM:
276  if (JEVOIS_CHECK_DELIM(c)) {
277  jv->buf[jv->idx] = '\0'; // end string
278  jv->msg.dim[jv->n++] = (uint16_t)atoi(jv->buf); // store dimension
279  if (jv->n == jv->msg.nb) {
280  // got all dimensions, go to next state
281  jv->n = 0; // reset number of received elements
282  jv->idx = 0; // reset index
283  if (jv->msg.type == JEVOIS_MSG_D3) {
284  jv->state = JV_QUAT;
285  } else {
286  jv->state = JV_SEND_MSG;
287  }
288  break;
289  }
290  jv->idx = 0; // reset index
291  }
292  else {
293  jv->buf[jv->idx++] = c; // fill buffer
294  }
295  break;
296  case JV_QUAT:
297  if (JEVOIS_CHECK_DELIM(c)) {
298  jv->buf[jv->idx] = '\0';
299  float q = (float)atof(jv->buf);
300  switch (jv->n) {
301  case 0:
302  jv->msg.quat.qi = q;
303  break;
304  case 1:
305  jv->msg.quat.qx = q;
306  break;
307  case 2:
308  jv->msg.quat.qy = q;
309  break;
310  case 3:
311  jv->msg.quat.qz = q;
312  break;
313  case 4:
314  jv->state = JV_EXTRA;
315  break;
316  default:
317  jv->state = JV_SYNC; // error
318  break;
319  }
320  jv->n++;
321  jv->idx = 0; // reset index
322  }
323  else {
324  jv->buf[jv->idx++] = c; // fill buffer
325  }
326  break;
327  case JV_EXTRA:
328  if (JEVOIS_CHECK_DELIM(c)) {
329  jv->msg.extra[jv->idx] = '\0'; // end string
330  jv->state = JV_SEND_MSG;
331  jv->idx = 0; // reset index
332  }
333  else {
334  jv->msg.extra[jv->idx++] = c; // store extra string
335  if (jv->idx > JEVOIS_MAX_LEN - 1) {
336  jv->state = JV_SYNC; // too long, return to sync
337  }
338  }
339  break;
340  case JV_SEND_MSG:
341  // send ABI message
342  AbiSendMsgJEVOIS_MSG(CAM_JEVOIS_ID,
343  jv->msg.type,
344  jv->msg.id,
345  jv->msg.nb,
346  jv->msg.coord,
347  jv->msg.dim,
348  jv->msg.quat,
349  jv->msg.extra);
350  // also send specific messages if needed
352  jv->data_available = true;
353  jv->state = JV_SYNC;
354  break;
355  default:
356  // error, back to SYNC
357  jv->state = JV_SYNC;
358  break;
359  }
360 }
361 
362 
363 // UART polling function
364 void jevois_event(void)
365 {
366  // Look for data on serial link and send to parser
367  while (uart_char_available(&(JEVOIS_DEV))) {
368  uint8_t ch = uart_getch(&(JEVOIS_DEV));
369  jevois_parse(&jevois, ch);
370  }
371 }
372 
373 // utility function to send a string
375 {
376  uint8_t i = 0;
377  while (s[i]) {
378  uart_put_byte(&(JEVOIS_DEV), 0, (uint8_t)(s[i]));
379  i++;
380  }
381 }
382 
383 void jevois_stream(bool activate)
384 {
385  jevois_stream_setting = activate;
386  if (activate) {
387  jevois_send_string("streamon\r\n");
388  } else {
389  jevois_send_string("streamoff\r\n");
390  }
391 }
392 
393 void jevois_setmapping(int number)
394 {
395  jevois_mapping_setting = number;
396  jevois_stream(false);
397  jevois_send_string("setmapping ");
398  char s[4];
399 #ifndef SITL
400  itoa(number, s, 10);
401 #endif
403  jevois_send_string("\r\n");
404  jevois_stream(true);
405 }
406 
int jevois_mapping_setting
Definition: jevois.c:36
unsigned short uint16_t
Definition: types.h:16
Definition: jevois.c:61
#define CAM_JEVOIS_ID
Definition: jevois.c:60
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
char buf[JEVOIS_MAX_LEN]
Definition: jevois.c:73
static uint32_t s
void jevois_setmapping(int number)
Set video mapping.
Definition: jevois.c:393
Definition: jevois.c:64
#define JEVOIS_MSG_D3
Definition: jevois.h:41
void jevois_event(void)
Definition: jevois.c:364
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:845
bool jevois_stream_setting
Definition: jevois.c:37
uint8_t type
Definition: jevois.c:48
uint16_t dim[3]
Definition: jevois.c:52
#define JEVOIS_MSG_T2
Definition: jevois.h:35
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
void jevois_send_string(char *s)
Generic function to send a string command to Jevois.
Definition: jevois.c:374
Main include for ABI (AirBorneInterface).
struct FloatQuat quat
Definition: jevois.c:53
#define JEVOIS_MAX_COORD
Definition: jevois.c:42
void jevois_init(void)
Definition: jevois.c:114
uint8_t idx
Definition: jevois.c:74
static void jevois_send_message(void)
Definition: jevois.c:128
Roation quaternion.
#define JEVOIS_MSG_D1
Definition: jevois.h:34
#define JEVOIS_MSG_F2
Definition: jevois.h:38
int16_t coord[JEVOIS_MAX_COORD]
Definition: jevois.c:51
Paparazzi floating point algebra.
Definition: jevois.c:65
static void jevois_parse(struct jevois_t *jv, char c)
Definition: jevois.c:143
signed short int16_t
Definition: types.h:17
#define JEVOIS_MSG_T1
JEVOIS messages types.
Definition: jevois.h:32
#define JEVOIS_MSG_N1
Definition: jevois.h:33
void jevois_stream(bool activate)
Start and stop streaming.
Definition: jevois.c:383
#define JEVOIS_MSG_N2
Definition: jevois.h:36
#define JEVOIS_CHECK_DELIM(_c)
Definition: jevois.c:44
char id[JEVOIS_MAX_LEN]
Definition: jevois.c:49
#define Max(x, y)
Definition: main_fbw.c:53
jevois_state
Definition: jevois.c:58
char extra[JEVOIS_MAX_LEN]
Definition: jevois.c:54
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
Definition: uart_arch.c:972
#define JEVOIS_MSG_T3
Definition: jevois.h:39
uint8_t n
Definition: jevois.c:75
unsigned char uint8_t
Definition: types.h:14
uint8_t nb
Definition: jevois.c:50
#define JEVOIS_NORM
Normalized data from JEVOIS are between -1000 and 1000.
Definition: jevois.h:45
struct jevois_t jevois
Definition: jevois.c:80
#define JEVOIS_MSG_F3
Definition: jevois.h:42
Definition: jevois.c:62
#define JEVOIS_MSG_N3
Definition: jevois.h:40
#define JEVOIS_MAX_LEN
Definition: jevois.c:40
#define JEVOIS_VFOV
Camera vertical FOV Camera has a 4/3 ratio.
Definition: jevois.h:59
enum jevois_state state
Definition: jevois.c:72
struct jevois_msg_t msg
Definition: jevois.c:76
Definition: jevois.c:59
#define JEVOIS_MSG_D2
Definition: jevois.h:37
#define JEVOIS_HFOV
Camera horizontal FOV From datasheet it should be 65deg, but it seems that better results are acheive...
Definition: jevois.h:52
bool data_available
Definition: jevois.c:77
void jevois_report(void)
Definition: jevois.c:83