diff --git a/conf/abi.xml b/conf/abi.xml index f358de58f4..269d937287 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -124,6 +124,33 @@ + + + Standardized message type JEVOIS_MSG_[T1|N1|D1|T2|N2|D2|F2|T3|N3|D3|F3] + Text string describing the reported object + Number of elements in the coordinates array + List of coordinates corresponding to 1D, 2D or 3D messages + 1, 2 or 3D dimension + Quaternion that relates the object's frame to the camera's frame, if appropriate + Additional text information + + + + Object ID to track + + bitmask to select relative coordinates (0) or global coordinates (1) + bit 0: bearing + bit 1: height + bit 2: distance + + Direction of object to track + Height of object to track + Distance of object to track + + diff --git a/conf/modules/jevois.xml b/conf/modules/jevois.xml new file mode 100644 index 0000000000..0b5fc7548f --- /dev/null +++ b/conf/modules/jevois.xml @@ -0,0 +1,35 @@ + + + + + + Decoder for standardized messages from the JEVOIS smart camera (http://jevois.org) + + Data are extracted from a serial link and produce a JEVOIS_MSG ABI message + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + diff --git a/sw/airborne/modules/sensors/cameras/jevois.c b/sw/airborne/modules/sensors/cameras/jevois.c new file mode 100644 index 0000000000..16c9dc8b99 --- /dev/null +++ b/sw/airborne/modules/sensors/cameras/jevois.c @@ -0,0 +1,362 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/sensors/cameras/jevois.c" + * @author Gautier Hattenberger + * Decoder for standardized messages from the JEVOIS smart camera (http://jevois.org) + */ + +#include "modules/sensors/cameras/jevois.h" + +#include "std.h" +#include "mcu_periph/uart.h" +#include "subsystems/abi.h" +#include "math/pprz_algebra_float.h" +#include + +int jevois_mapping_setting; +bool jevois_stream_setting; + +// max string length +#define JEVOIS_MAX_LEN 32 +// max number of coordinates +#define JEVOIS_MAX_COORD 18 +// check delimiter +#define JEVOIS_CHECK_DELIM(_c) (_c == ' ' || _c == '\n' || _c == '\r' || _c == '\0') + +// generic JEVOIS message structure +struct jevois_msg_t { + uint8_t type; + char id[JEVOIS_MAX_LEN]; + uint8_t nb; + int16_t coord[JEVOIS_MAX_COORD]; + uint16_t dim[3]; + struct FloatQuat quat; + char extra[JEVOIS_MAX_LEN]; +}; + +// decoder state +enum jevois_state { + JV_SYNC = 0, + JV_TYPE, + JV_ID, + JV_SIZE, + JV_COORD, + JV_DIM, + JV_QUAT, + JV_EXTRA, + JV_SEND_MSG +}; + +// jevois struct +struct jevois_t { + enum jevois_state state; // decoder state + char buf[JEVOIS_MAX_LEN]; // temp buffer + uint8_t idx; // temp buffer index + uint8_t n; // temp coordinates/dimension index + struct jevois_msg_t msg; // last decoded message +}; + +struct jevois_t jevois; + +// initialization +void jevois_init(void) +{ + // dummy settings + jevois_mapping_setting = 0; + jevois_stream_setting = false; + + jevois.state = JV_SYNC; + jevois.idx = 0; + jevois.n = 0; + memset(jevois.buf, 0, JEVOIS_MAX_LEN); +} + +// send specific message if requested +static void jevois_send_message(void) +{ +#if JEVOIS_SEND_FOLLOW_TARGET + float cam_heading = (JEVOIS_HFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[0]); + float cam_height = (JEVOIS_VFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[1]); + // send a FOLLOW_TARGET message in camera frame + AbiSendMsgFOLLOW_TARGET(CAM_JEVOIS_ID, 0, 0, cam_heading, cam_height, 0.f); +#endif +} + +// raw message parsing function +static void jevois_parse(struct jevois_t *jv, char c) +{ + switch (jv->state) { + case JV_SYNC: + // wait for sync (newline character) + if (c == '\n') { + jv->state = JV_TYPE; + jv->idx = 0; + jv->n = 0; + } + break; + case JV_TYPE: + jv->buf[jv->idx++] = c; // fill buffer + // parse type + if (jv->idx > 2) { // msg type + white space + if (jv->buf[0] == 'T' && jv->buf[1] == '1') { + jv->state = JV_COORD; + jv->msg.type = JEVOIS_MSG_T1; + jv->msg.nb = 1; + } else if (jv->buf[0] == 'N' && jv->buf[1] == '1') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_N1; + jv->msg.nb = 1; + } else if (jv->buf[0] == 'D' && jv->buf[1] == '1') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_D1; + jv->msg.nb = 2; + } else if (jv->buf[0] == 'T' && jv->buf[1] == '2') { + jv->state = JV_COORD; + jv->msg.type = JEVOIS_MSG_T2; + jv->msg.nb = 2; + } else if (jv->buf[0] == 'N' && jv->buf[1] == '2') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_N2; + jv->msg.nb = 2; + } else if (jv->buf[0] == 'D' && jv->buf[1] == '2') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_D2; + jv->msg.nb = 8; + } else if (jv->buf[0] == 'F' && jv->buf[1] == '2') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_F2; + jv->msg.nb = 0; + } else if (jv->buf[0] == 'T' && jv->buf[1] == '3') { + jv->state = JV_COORD; + jv->msg.type = JEVOIS_MSG_T3; + jv->msg.nb = 3; + } else if (jv->buf[0] == 'N' && jv->buf[1] == '3') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_N3; + jv->msg.nb = 3; + } else if (jv->buf[0] == 'D' && jv->buf[1] == '3') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_D3; + jv->msg.nb = 3; + } else if (jv->buf[0] == 'F' && jv->buf[1] == '3') { + jv->state = JV_ID; + jv->msg.type = JEVOIS_MSG_F3; + jv->msg.nb = 0; + } else { + jv->state = JV_SYNC; // error + } + jv->idx = 0; + } + break; + case JV_ID: + if (JEVOIS_CHECK_DELIM(c)) { + jv->buf[jv->idx] = '\0'; // end string + if (jv->msg.type == JEVOIS_MSG_F2 || + jv->msg.type == JEVOIS_MSG_F3) { + jv->state = JV_SIZE; // parse n before coordinates + } else { + jv->state = JV_COORD; // parse directly coordinates + } + break; + } + else { + jv->msg.id[jv->idx++] = c; + if (jv->idx > JEVOIS_MAX_LEN - 1) { + jv->state = JV_SYNC; // too long, return to sync + } + } + break; + case JV_SIZE: + if (JEVOIS_CHECK_DELIM(c)) { + jv->buf[jv->idx] = '\0'; // end string + jv->msg.nb = (uint8_t)atoi(jv->buf); // store size + jv->state = JV_COORD; + jv->idx = 0; + } + else { + jv->buf[jv->idx++] = c; // fill buffer + } + break; + case JV_COORD: + if (JEVOIS_CHECK_DELIM(c)) { + jv->buf[jv->idx] = '\0'; // end string + jv->msg.coord[jv->n++] = (int16_t)atoi(jv->buf); // store value + if (jv->n == jv->msg.nb) { + // got all coordinates, go to next state + jv->n = 0; // reset number of received elements + jv->idx = 0; // reset index + switch (jv->msg.type) { + case JEVOIS_MSG_T1: + case JEVOIS_MSG_T2: + case JEVOIS_MSG_T3: + jv->state = JV_SEND_MSG; + break; + case JEVOIS_MSG_N1: + case JEVOIS_MSG_N2: + case JEVOIS_MSG_N3: + case JEVOIS_MSG_D3: + jv->state = JV_DIM; + break; + case JEVOIS_MSG_D1: + case JEVOIS_MSG_D2: + case JEVOIS_MSG_F2: + case JEVOIS_MSG_F3: + jv->state = JV_EXTRA; + break; + default: + jv->state = JV_SYNC; // error + break; + } + } + jv->idx = 0; // reset index + } + else { + jv->buf[jv->idx++] = c; // fill buffer + } + break; + case JV_DIM: + if (JEVOIS_CHECK_DELIM(c)) { + jv->buf[jv->idx] = '\0'; // end string + jv->msg.dim[jv->n++] = (uint16_t)atoi(jv->buf); // store dimension + if (jv->n == jv->msg.nb) { + // got all dimensions, go to next state + jv->n = 0; // reset number of received elements + jv->idx = 0; // reset index + if (jv->msg.type == JEVOIS_MSG_D3) { + jv->state = JV_QUAT; + } else { + jv->state = JV_SEND_MSG; + } + break; + } + jv->idx = 0; // reset index + } + else { + jv->buf[jv->idx++] = c; // fill buffer + } + break; + case JV_QUAT: + if (JEVOIS_CHECK_DELIM(c)) { + jv->buf[jv->idx] = '\0'; + float q = 0.f;//(float) atof(jv->buf); + switch (jv->n) { + case 0: + jv->msg.quat.qi = q; // TODO check quaternion order + break; + case 1: + jv->msg.quat.qx = q; + break; + case 2: + jv->msg.quat.qy = q; + break; + case 3: + jv->msg.quat.qz = q; + break; + case 4: + jv->state = JV_EXTRA; + break; + default: + jv->state = JV_SYNC; // error + break; + } + jv->n++; + jv->idx = 0; // reset index + } + else { + jv->buf[jv->idx++] = c; // fill buffer + } + break; + case JV_EXTRA: + if (JEVOIS_CHECK_DELIM(c)) { + jv->buf[jv->idx] = '\0'; // end string + jv->state = JV_SEND_MSG; + } + else { + jv->msg.id[jv->idx++] = c; // store extra string + if (jv->idx > JEVOIS_MAX_LEN - 1) { + jv->state = JV_SYNC; // too long, return to sync + } + } + break; + case JV_SEND_MSG: + // send ABI message + AbiSendMsgJEVOIS_MSG(CAM_JEVOIS_ID, + jv->msg.type, + jv->msg.id, + jv->msg.nb, + jv->msg.coord, + jv->msg.dim, + jv->msg.quat, + jv->msg.extra); + // also send specific messages if needed + jevois_send_message(); + jv->state = JV_SYNC; + break; + default: + // error, back to SYNC + jv->state = JV_SYNC; + break; + } +} + + +// UART polling function +void jevois_event(void) +{ + // Look for data on serial link and send to parser + while (uart_char_available(&(JEVOIS_DEV))) { + uint8_t ch = uart_getch(&(JEVOIS_DEV)); + jevois_parse(&jevois, ch); + } +} + +// utility function to send a string +static void send_string(char *s) +{ + uint8_t i = 0; + while (s[i]) { + uart_put_byte(&(JEVOIS_DEV), 0, (uint8_t)(s[i])); + i++; + } +} + +void jevois_stream(bool activate) +{ + jevois_stream_setting = activate; + if (activate) { + send_string("streamon\r\n"); + } else { + send_string("streamoff\r\n"); + } +} + +void jevois_setmapping(int number) +{ + jevois_mapping_setting = number; + jevois_stream(false); + send_string("setmapping "); + char s[4]; + itoa(number, s, 10); + send_string(s); + send_string("\r\n"); + jevois_stream(true); +} + diff --git a/sw/airborne/modules/sensors/cameras/jevois.h b/sw/airborne/modules/sensors/cameras/jevois.h new file mode 100644 index 0000000000..18afe2acdb --- /dev/null +++ b/sw/airborne/modules/sensors/cameras/jevois.h @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/sensors/cameras/jevois.h" + * @author Gautier Hattenberger + * Decoder for standardized messages from the JEVOIS smart camera (http://jevois.org) + */ + +#ifndef JEVOIS_H +#define JEVOIS_H + +#include "std.h" + +/** JEVOIS messages types */ +#define JEVOIS_MSG_T1 10 +#define JEVOIS_MSG_N1 11 +#define JEVOIS_MSG_D1 12 +#define JEVOIS_MSG_T2 20 +#define JEVOIS_MSG_N2 21 +#define JEVOIS_MSG_D2 22 +#define JEVOIS_MSG_F2 23 +#define JEVOIS_MSG_T3 30 +#define JEVOIS_MSG_N3 31 +#define JEVOIS_MSG_D3 32 +#define JEVOIS_MSG_F3 33 + +/** Normalized data from JEVOIS are between -1000 and 1000 */ +#define JEVOIS_NORM 1000 + +/** Camera horizontal FOV + * From datasheet it should be 65deg, + * but it seems that better results are acheived with 45 + */ +#ifndef JEVOIS_HFOV +#define JEVOIS_HFOV RadOfDeg(45) +#endif + +/** Camera vertical FOV + * Camera has a 4/3 ratio + */ +#ifndef JEVOIS_VFOV +#define JEVOIS_VFOV (3*JEVOIS_HFOV/4) +#endif + +extern void jevois_init(void); +extern void jevois_event(void); + +/** Start and stop streaming + * @param[in] activate enable or disable streaming + */ +extern void jevois_stream(bool activate); + +// dummy variable to start/stop stream from setting +extern bool jevois_stream_setting; + +/** Set video mapping + * @param[in] number video mapping number + */ +extern void jevois_setmapping(int number); + +// dummy variable to change mapping from setting +extern int jevois_mapping_setting; + +#endif + diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 0dc94f1b93..c578b67750 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -362,4 +362,13 @@ #define RANGE_FORCEFIELD_ID 1 #endif +/* + * ID's for camera type sensors + */ + +#ifndef CAM_JEVOIS_ID +#define CAM_JEVOIS_ID 1 +#endif + + #endif /* ABI_SENDER_IDS_H */