Paparazzi UAS  v5.17_devel-15-geeac31e
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 // extrat a number (int) from an idea string
128 // this might be needed as jevois ID can start with a letter
129 // this will extract the first substring with a number
130 // and return the result of atoi function
131 static int jevois_extract_nb(char *in) {
132  unsigned int i, j = 0;
133  bool first = false;
134  char out[JEVOIS_MAX_LEN];
135  for (i = 0; i < strlen(in)+1; i++) {
136  if ((in[i] > '0' && in[i] < '9') || in[i] == '-') {
137  out[j++] = in[i];
138  first = true;
139  } else if (first || in[i] == '\0') {
140  out[j] = '\0';
141  break;
142  }
143  }
144  return atoi(out);
145 }
146 
147 // send specific message if requested
148 static void jevois_send_message(void)
149 {
150 #if JEVOIS_SEND_MSG
151  // send pprzlink JEVOIS message
152  jevois_report();
153 #endif
154 #if JEVOIS_SEND_FOLLOW_TARGET
155  float cam_heading = (JEVOIS_HFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[0]);
156  float cam_height = (JEVOIS_VFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[1]);
157  // send a FOLLOW_TARGET message in camera frame
158  AbiSendMsgFOLLOW_TARGET(CAM_JEVOIS_ID, 0, 0, cam_heading, cam_height, 0.f);
159 #endif
160 #if JEVOIS_SEND_VISUAL_DETECTION
161  AbiSendMsgVISUAL_DETECTION(CAM_JEVOIS_ID,
162  jevois.msg.coord[0],
163  jevois.msg.coord[1],
164  jevois.msg.dim[0],
165  jevois.msg.dim[1],
166  0,
168 #endif
169 }
170 
171 // raw message parsing function
172 static void jevois_parse(struct jevois_t *jv, char c)
173 {
174  switch (jv->state) {
175  case JV_SYNC:
176  // wait for sync (newline character)
177  if (c == '\n') {
178  jv->state = JV_TYPE;
179  jv->idx = 0;
180  jv->n = 0;
181  }
182  break;
183  case JV_TYPE:
184  jv->buf[jv->idx++] = c; // fill buffer
185  // parse type
186  if (jv->idx > 2) { // msg type + white space
187  if (jv->buf[0] == 'T' && jv->buf[1] == '1') {
188  jv->state = JV_COORD;
189  jv->msg.type = JEVOIS_MSG_T1;
190  jv->msg.nb = 1;
191  } else if (jv->buf[0] == 'N' && jv->buf[1] == '1') {
192  jv->state = JV_ID;
193  jv->msg.type = JEVOIS_MSG_N1;
194  jv->msg.nb = 1;
195  } else if (jv->buf[0] == 'D' && jv->buf[1] == '1') {
196  jv->state = JV_ID;
197  jv->msg.type = JEVOIS_MSG_D1;
198  jv->msg.nb = 2;
199  } else if (jv->buf[0] == 'T' && jv->buf[1] == '2') {
200  jv->state = JV_COORD;
201  jv->msg.type = JEVOIS_MSG_T2;
202  jv->msg.nb = 2;
203  } else if (jv->buf[0] == 'N' && jv->buf[1] == '2') {
204  jv->state = JV_ID;
205  jv->msg.type = JEVOIS_MSG_N2;
206  jv->msg.nb = 2;
207  } else if (jv->buf[0] == 'D' && jv->buf[1] == '2') {
208  jv->state = JV_ID;
209  jv->msg.type = JEVOIS_MSG_D2;
210  jv->msg.nb = 8;
211  } else if (jv->buf[0] == 'F' && jv->buf[1] == '2') {
212  jv->state = JV_ID;
213  jv->msg.type = JEVOIS_MSG_F2;
214  jv->msg.nb = 0;
215  } else if (jv->buf[0] == 'T' && jv->buf[1] == '3') {
216  jv->state = JV_COORD;
217  jv->msg.type = JEVOIS_MSG_T3;
218  jv->msg.nb = 3;
219  } else if (jv->buf[0] == 'N' && jv->buf[1] == '3') {
220  jv->state = JV_ID;
221  jv->msg.type = JEVOIS_MSG_N3;
222  jv->msg.nb = 3;
223  } else if (jv->buf[0] == 'D' && jv->buf[1] == '3') {
224  jv->state = JV_ID;
225  jv->msg.type = JEVOIS_MSG_D3;
226  jv->msg.nb = 3;
227  } else if (jv->buf[0] == 'F' && jv->buf[1] == '3') {
228  jv->state = JV_ID;
229  jv->msg.type = JEVOIS_MSG_F3;
230  jv->msg.nb = 0;
231  } else {
232  jv->state = JV_SYNC; // error
233  }
234  jv->idx = 0;
235  }
236  break;
237  case JV_ID:
238  if (JEVOIS_CHECK_DELIM(c)) {
239  jv->msg.id[jv->idx] = '\0'; // end string
240  if (jv->msg.type == JEVOIS_MSG_F2 ||
241  jv->msg.type == JEVOIS_MSG_F3) {
242  jv->state = JV_SIZE; // parse n before coordinates
243  } else {
244  jv->state = JV_COORD; // parse directly coordinates
245  }
246  jv->idx = 0;
247  break;
248  }
249  else {
250  jv->msg.id[jv->idx++] = c;
251  if (jv->idx > JEVOIS_MAX_LEN - 1) {
252  jv->state = JV_SYNC; // too long, return to sync
253  }
254  }
255  break;
256  case JV_SIZE:
257  if (JEVOIS_CHECK_DELIM(c)) {
258  jv->buf[jv->idx] = '\0'; // end string
259  jv->msg.nb = (uint8_t)atoi(jv->buf); // store size
260  jv->state = JV_COORD;
261  jv->idx = 0;
262  }
263  else {
264  jv->buf[jv->idx++] = c; // fill buffer
265  }
266  break;
267  case JV_COORD:
268  if (JEVOIS_CHECK_DELIM(c)) {
269  jv->buf[jv->idx] = '\0'; // end string
270  jv->msg.coord[jv->n++] = (int16_t)atoi(jv->buf); // store value
271  if (jv->n == jv->msg.nb) {
272  // got all coordinates, go to next state
273  jv->n = 0; // reset number of received elements
274  jv->idx = 0; // reset index
275  switch (jv->msg.type) {
276  case JEVOIS_MSG_T1:
277  case JEVOIS_MSG_T2:
278  case JEVOIS_MSG_T3:
279  jv->state = JV_SEND_MSG;
280  break;
281  case JEVOIS_MSG_N1:
282  case JEVOIS_MSG_N2:
283  case JEVOIS_MSG_N3:
284  case JEVOIS_MSG_D3:
285  jv->state = JV_DIM;
286  break;
287  case JEVOIS_MSG_D1:
288  case JEVOIS_MSG_D2:
289  case JEVOIS_MSG_F2:
290  case JEVOIS_MSG_F3:
291  jv->state = JV_EXTRA;
292  break;
293  default:
294  jv->state = JV_SYNC; // error
295  break;
296  }
297  }
298  jv->idx = 0; // reset index
299  }
300  else {
301  jv->buf[jv->idx++] = c; // fill buffer
302  }
303  break;
304  case JV_DIM:
305  if (JEVOIS_CHECK_DELIM(c)) {
306  jv->buf[jv->idx] = '\0'; // end string
307  jv->msg.dim[jv->n++] = (uint16_t)atoi(jv->buf); // store dimension
308  if (jv->n == jv->msg.nb) {
309  // got all dimensions, go to next state
310  jv->n = 0; // reset number of received elements
311  jv->idx = 0; // reset index
312  if (jv->msg.type == JEVOIS_MSG_D3) {
313  jv->state = JV_QUAT;
314  } else {
315  jv->state = JV_SEND_MSG;
316  }
317  break;
318  }
319  jv->idx = 0; // reset index
320  }
321  else {
322  jv->buf[jv->idx++] = c; // fill buffer
323  }
324  break;
325  case JV_QUAT:
326  if (JEVOIS_CHECK_DELIM(c)) {
327  jv->buf[jv->idx] = '\0';
328  float q = (float)atof(jv->buf);
329  switch (jv->n) {
330  case 0:
331  jv->msg.quat.qi = q;
332  break;
333  case 1:
334  jv->msg.quat.qx = q;
335  break;
336  case 2:
337  jv->msg.quat.qy = q;
338  break;
339  case 3:
340  jv->msg.quat.qz = q;
341  break;
342  case 4:
343  jv->state = JV_EXTRA;
344  break;
345  default:
346  jv->state = JV_SYNC; // error
347  break;
348  }
349  jv->n++;
350  jv->idx = 0; // reset index
351  }
352  else {
353  jv->buf[jv->idx++] = c; // fill buffer
354  }
355  break;
356  case JV_EXTRA:
357  if (JEVOIS_CHECK_DELIM(c)) {
358  jv->msg.extra[jv->idx] = '\0'; // end string
359  jv->state = JV_SEND_MSG;
360  jv->idx = 0; // reset index
361  }
362  else {
363  jv->msg.extra[jv->idx++] = c; // store extra string
364  if (jv->idx > JEVOIS_MAX_LEN - 1) {
365  jv->state = JV_SYNC; // too long, return to sync
366  }
367  }
368  break;
369  case JV_SEND_MSG:
370  // send ABI message
371  AbiSendMsgJEVOIS_MSG(CAM_JEVOIS_ID,
372  jv->msg.type,
373  jv->msg.id,
374  jv->msg.nb,
375  jv->msg.coord,
376  jv->msg.dim,
377  jv->msg.quat,
378  jv->msg.extra);
379  // also send specific messages if needed
381  jv->data_available = true;
382  jv->state = JV_SYNC;
383  break;
384  default:
385  // error, back to SYNC
386  jv->state = JV_SYNC;
387  break;
388  }
389 }
390 
391 
392 // UART polling function
393 void jevois_event(void)
394 {
395  // Look for data on serial link and send to parser
396  while (uart_char_available(&(JEVOIS_DEV))) {
397  uint8_t ch = uart_getch(&(JEVOIS_DEV));
398  jevois_parse(&jevois, ch);
399  }
400 }
401 
402 // utility function to send a string
404 {
405  uint8_t i = 0;
406  while (s[i]) {
407  uart_put_byte(&(JEVOIS_DEV), 0, (uint8_t)(s[i]));
408  i++;
409  }
410 }
411 
412 void jevois_stream(bool activate)
413 {
414  jevois_stream_setting = activate;
415  if (activate) {
416  jevois_send_string("streamon\r\n");
417  } else {
418  jevois_send_string("streamoff\r\n");
419  }
420 }
421 
422 void jevois_setmapping(int number)
423 {
424  jevois_mapping_setting = number;
425  jevois_stream(false);
426  jevois_send_string("setmapping ");
427  char s[4];
428 #ifndef SITL
429  itoa(number, s, 10);
430 #endif
432  jevois_send_string("\r\n");
433  jevois_stream(true);
434 }
435 
437 {
438  char str[32] __attribute__((unused));
439 #if JEVOIS_SEND_ALT
440  // send current altitude in millimeter
441  int alt_mm = (int)(stateGetPositionEnu_f()->z * 1000.f);
442  Bound(alt_mm, -999999, 999999);
443  sprintf(str, "alt %d\r\n", alt_mm);
444  jevois_send_string(str);
445 #endif
446 #if JEVOIS_SEND_POS
447  // send current position in millimeter
448  struct EnuCoor_f pos = *stateGetPositionEnu_f();
449  int x_mm = (int)(pos.x * 1000.f);
450  int y_mm = (int)(pos.y * 1000.f);
451  int z_mm = (int)(pos.z * 1000.f);
452  Bound(x_mm, -999999, 999999);
453  Bound(y_mm, -999999, 999999);
454  Bound(z_mm, -999999, 999999);
455  sprintf(str, "pos %d %d %d\r\n", x_mm, y_mm, z_mm);
456  jevois_send_string(str);
457 #endif
458 #if JEVOIS_SEND_QUAT
459  // send quaternion
460  struct FloatQuat quat = *stateGetNedToBodyQuat_f();
461  int qi = (int)(quat.qi * 1000.f);
462  int qx = (int)(quat.qx * 1000.f);
463  int qy = (int)(quat.qy * 1000.f);
464  int qz = (int)(quat.qz * 1000.f);
465  Bound(qi, -9999, 9999);
466  Bound(qx, -9999, 9999);
467  Bound(qy, -9999, 9999);
468  Bound(qz, -9999, 9999);
469  sprintf(str, "quat %d %d %d %d\r\n", qi, qx, qy, qz);
470  jevois_send_string(str);
471 #endif
472 }
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:422
Definition: jevois.c:64
#define JEVOIS_MSG_D3
Definition: jevois.h:41
void jevois_event(void)
Definition: jevois.c:393
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:867
bool jevois_stream_setting
Definition: jevois.c:37
uint8_t type
Definition: jevois.c:48
vector in East North Up coordinates Units: meters
uint16_t dim[3]
Definition: jevois.c:52
#define JEVOIS_MSG_T2
Definition: jevois.h:35
void jevois_send_string(char *s)
Generic function to send a string command to Jevois.
Definition: jevois.c:403
Main include for ABI (AirBorneInterface).
struct FloatQuat quat
Definition: jevois.c:53
void jevois_send_state(void)
Send state to camera.
Definition: jevois.c:436
#define JEVOIS_MAX_COORD
Definition: jevois.c:42
void jevois_init(void)
Definition: jevois.c:114
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
uint8_t idx
Definition: jevois.c:74
static void jevois_send_message(void)
Definition: jevois.c:148
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
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.
float x
in meters
Definition: jevois.c:65
static void jevois_parse(struct jevois_t *jv, char c)
Definition: jevois.c:172
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:412
#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
volatile int32_t alt_mm
Definition: exif_module.c:33
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:994
#define JEVOIS_MSG_T3
Definition: jevois.h:39
uint8_t n
Definition: jevois.c:75
static int jevois_extract_nb(char *in)
Definition: jevois.c:131
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
float z
in meters
#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
float y
in meters
#define JEVOIS_HFOV
Camera horizontal FOV From datasheet it should be 65deg, but it seems that better results are acheive...
Definition: jevois.h:52
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
bool data_available
Definition: jevois.c:77
void jevois_report(void)
Definition: jevois.c:83