Paparazzi UAS  v5.15_devel-113-g1b57ff1
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 #if JEVOIS_SEND_VISUAL_DETECTION
141  AbiSendMsgVISUAL_DETECTION(CAM_JEVOIS_ID,
142  jevois.msg.coord[0],
143  jevois.msg.coord[1],
144  jevois.msg.dim[0],
145  jevois.msg.dim[1],
146  0,
147  (int16_t)atoi(jevois.msg.id));
148 #endif
149 }
150 
151 // raw message parsing function
152 static void jevois_parse(struct jevois_t *jv, char c)
153 {
154  switch (jv->state) {
155  case JV_SYNC:
156  // wait for sync (newline character)
157  if (c == '\n') {
158  jv->state = JV_TYPE;
159  jv->idx = 0;
160  jv->n = 0;
161  }
162  break;
163  case JV_TYPE:
164  jv->buf[jv->idx++] = c; // fill buffer
165  // parse type
166  if (jv->idx > 2) { // msg type + white space
167  if (jv->buf[0] == 'T' && jv->buf[1] == '1') {
168  jv->state = JV_COORD;
169  jv->msg.type = JEVOIS_MSG_T1;
170  jv->msg.nb = 1;
171  } else if (jv->buf[0] == 'N' && jv->buf[1] == '1') {
172  jv->state = JV_ID;
173  jv->msg.type = JEVOIS_MSG_N1;
174  jv->msg.nb = 1;
175  } else if (jv->buf[0] == 'D' && jv->buf[1] == '1') {
176  jv->state = JV_ID;
177  jv->msg.type = JEVOIS_MSG_D1;
178  jv->msg.nb = 2;
179  } else if (jv->buf[0] == 'T' && jv->buf[1] == '2') {
180  jv->state = JV_COORD;
181  jv->msg.type = JEVOIS_MSG_T2;
182  jv->msg.nb = 2;
183  } else if (jv->buf[0] == 'N' && jv->buf[1] == '2') {
184  jv->state = JV_ID;
185  jv->msg.type = JEVOIS_MSG_N2;
186  jv->msg.nb = 2;
187  } else if (jv->buf[0] == 'D' && jv->buf[1] == '2') {
188  jv->state = JV_ID;
189  jv->msg.type = JEVOIS_MSG_D2;
190  jv->msg.nb = 8;
191  } else if (jv->buf[0] == 'F' && jv->buf[1] == '2') {
192  jv->state = JV_ID;
193  jv->msg.type = JEVOIS_MSG_F2;
194  jv->msg.nb = 0;
195  } else if (jv->buf[0] == 'T' && jv->buf[1] == '3') {
196  jv->state = JV_COORD;
197  jv->msg.type = JEVOIS_MSG_T3;
198  jv->msg.nb = 3;
199  } else if (jv->buf[0] == 'N' && jv->buf[1] == '3') {
200  jv->state = JV_ID;
201  jv->msg.type = JEVOIS_MSG_N3;
202  jv->msg.nb = 3;
203  } else if (jv->buf[0] == 'D' && jv->buf[1] == '3') {
204  jv->state = JV_ID;
205  jv->msg.type = JEVOIS_MSG_D3;
206  jv->msg.nb = 3;
207  } else if (jv->buf[0] == 'F' && jv->buf[1] == '3') {
208  jv->state = JV_ID;
209  jv->msg.type = JEVOIS_MSG_F3;
210  jv->msg.nb = 0;
211  } else {
212  jv->state = JV_SYNC; // error
213  }
214  jv->idx = 0;
215  }
216  break;
217  case JV_ID:
218  if (JEVOIS_CHECK_DELIM(c)) {
219  jv->msg.id[jv->idx] = '\0'; // end string
220  if (jv->msg.type == JEVOIS_MSG_F2 ||
221  jv->msg.type == JEVOIS_MSG_F3) {
222  jv->state = JV_SIZE; // parse n before coordinates
223  } else {
224  jv->state = JV_COORD; // parse directly coordinates
225  }
226  jv->idx = 0;
227  break;
228  }
229  else {
230  jv->msg.id[jv->idx++] = c;
231  if (jv->idx > JEVOIS_MAX_LEN - 1) {
232  jv->state = JV_SYNC; // too long, return to sync
233  }
234  }
235  break;
236  case JV_SIZE:
237  if (JEVOIS_CHECK_DELIM(c)) {
238  jv->buf[jv->idx] = '\0'; // end string
239  jv->msg.nb = (uint8_t)atoi(jv->buf); // store size
240  jv->state = JV_COORD;
241  jv->idx = 0;
242  }
243  else {
244  jv->buf[jv->idx++] = c; // fill buffer
245  }
246  break;
247  case JV_COORD:
248  if (JEVOIS_CHECK_DELIM(c)) {
249  jv->buf[jv->idx] = '\0'; // end string
250  jv->msg.coord[jv->n++] = (int16_t)atoi(jv->buf); // store value
251  if (jv->n == jv->msg.nb) {
252  // got all coordinates, go to next state
253  jv->n = 0; // reset number of received elements
254  jv->idx = 0; // reset index
255  switch (jv->msg.type) {
256  case JEVOIS_MSG_T1:
257  case JEVOIS_MSG_T2:
258  case JEVOIS_MSG_T3:
259  jv->state = JV_SEND_MSG;
260  break;
261  case JEVOIS_MSG_N1:
262  case JEVOIS_MSG_N2:
263  case JEVOIS_MSG_N3:
264  case JEVOIS_MSG_D3:
265  jv->state = JV_DIM;
266  break;
267  case JEVOIS_MSG_D1:
268  case JEVOIS_MSG_D2:
269  case JEVOIS_MSG_F2:
270  case JEVOIS_MSG_F3:
271  jv->state = JV_EXTRA;
272  break;
273  default:
274  jv->state = JV_SYNC; // error
275  break;
276  }
277  }
278  jv->idx = 0; // reset index
279  }
280  else {
281  jv->buf[jv->idx++] = c; // fill buffer
282  }
283  break;
284  case JV_DIM:
285  if (JEVOIS_CHECK_DELIM(c)) {
286  jv->buf[jv->idx] = '\0'; // end string
287  jv->msg.dim[jv->n++] = (uint16_t)atoi(jv->buf); // store dimension
288  if (jv->n == jv->msg.nb) {
289  // got all dimensions, go to next state
290  jv->n = 0; // reset number of received elements
291  jv->idx = 0; // reset index
292  if (jv->msg.type == JEVOIS_MSG_D3) {
293  jv->state = JV_QUAT;
294  } else {
295  jv->state = JV_SEND_MSG;
296  }
297  break;
298  }
299  jv->idx = 0; // reset index
300  }
301  else {
302  jv->buf[jv->idx++] = c; // fill buffer
303  }
304  break;
305  case JV_QUAT:
306  if (JEVOIS_CHECK_DELIM(c)) {
307  jv->buf[jv->idx] = '\0';
308  float q = (float)atof(jv->buf);
309  switch (jv->n) {
310  case 0:
311  jv->msg.quat.qi = q;
312  break;
313  case 1:
314  jv->msg.quat.qx = q;
315  break;
316  case 2:
317  jv->msg.quat.qy = q;
318  break;
319  case 3:
320  jv->msg.quat.qz = q;
321  break;
322  case 4:
323  jv->state = JV_EXTRA;
324  break;
325  default:
326  jv->state = JV_SYNC; // error
327  break;
328  }
329  jv->n++;
330  jv->idx = 0; // reset index
331  }
332  else {
333  jv->buf[jv->idx++] = c; // fill buffer
334  }
335  break;
336  case JV_EXTRA:
337  if (JEVOIS_CHECK_DELIM(c)) {
338  jv->msg.extra[jv->idx] = '\0'; // end string
339  jv->state = JV_SEND_MSG;
340  jv->idx = 0; // reset index
341  }
342  else {
343  jv->msg.extra[jv->idx++] = c; // store extra string
344  if (jv->idx > JEVOIS_MAX_LEN - 1) {
345  jv->state = JV_SYNC; // too long, return to sync
346  }
347  }
348  break;
349  case JV_SEND_MSG:
350  // send ABI message
351  AbiSendMsgJEVOIS_MSG(CAM_JEVOIS_ID,
352  jv->msg.type,
353  jv->msg.id,
354  jv->msg.nb,
355  jv->msg.coord,
356  jv->msg.dim,
357  jv->msg.quat,
358  jv->msg.extra);
359  // also send specific messages if needed
361  jv->data_available = true;
362  jv->state = JV_SYNC;
363  break;
364  default:
365  // error, back to SYNC
366  jv->state = JV_SYNC;
367  break;
368  }
369 }
370 
371 
372 // UART polling function
373 void jevois_event(void)
374 {
375  // Look for data on serial link and send to parser
376  while (uart_char_available(&(JEVOIS_DEV))) {
377  uint8_t ch = uart_getch(&(JEVOIS_DEV));
378  jevois_parse(&jevois, ch);
379  }
380 }
381 
382 // utility function to send a string
384 {
385  uint8_t i = 0;
386  while (s[i]) {
387  uart_put_byte(&(JEVOIS_DEV), 0, (uint8_t)(s[i]));
388  i++;
389  }
390 }
391 
392 void jevois_stream(bool activate)
393 {
394  jevois_stream_setting = activate;
395  if (activate) {
396  jevois_send_string("streamon\r\n");
397  } else {
398  jevois_send_string("streamoff\r\n");
399  }
400 }
401 
402 void jevois_setmapping(int number)
403 {
404  jevois_mapping_setting = number;
405  jevois_stream(false);
406  jevois_send_string("setmapping ");
407  char s[4];
408 #ifndef SITL
409  itoa(number, s, 10);
410 #endif
412  jevois_send_string("\r\n");
413  jevois_stream(true);
414 }
415 
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:402
Definition: jevois.c:64
#define JEVOIS_MSG_D3
Definition: jevois.h:41
void jevois_event(void)
Definition: jevois.c:373
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:383
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:152
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:392
#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