Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[module] support for jevois smart cameras

Browse Source
This commit is contained in:
Gautier Hattenberger
2017-11-22 00:54:44 +01:00
parent 857999e71d
commit b42caab15f
5 changed files with 515 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+27
View File
@@ -124,6 +124,33 @@
<field name="vel_body_z_FF" type="float" unit="m/s"/>
</message>
<message name="JEVOIS_MSG" id="21">
<!--description>
Standardized serial messages sent by the Jevois smart camera
http://jevois.org/doc/UserSerialStyle.html
</description-->
<field name="type" type="uint8_t">Standardized message type JEVOIS_MSG_[T1|N1|D1|T2|N2|D2|F2|T3|N3|D3|F3]</field>
<field name="id" type="char *">Text string describing the reported object</field>
<field name="nb" type="uint8_t">Number of elements in the coordinates array</field>
<field name="coord" type="int16_t *">List of coordinates corresponding to 1D, 2D or 3D messages</field>
<field name="dim" type="uint16_t *">1, 2 or 3D dimension</field>
<field name="quat" type="struct FloatQuat">Quaternion that relates the object's frame to the camera's frame, if appropriate</field>
<field name="extra" type="char *">Additional text information</field>
</message>
<message name="FOLLOW_TARGET" id="22">
<field name="id" type="uint32_t">Object ID to track</field>
<field name="frame" type="uint8_t">
bitmask to select relative coordinates (0) or global coordinates (1)
bit 0: bearing
bit 1: height
bit 2: distance
</field>
<field name="bearing" type="float" unit="rad">Direction of object to track</field>
<field name="height" type="float" unit="m">Height of object to track</field>
<field name="distance" type="float" unit="m">Distance of object to track</field>
</message>
</msg_class>
</protocol>
conf/modules/jevois.xml
+35
View File
@@ -0,0 +1,35 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="jevois" dir="sensors/cameras">
<doc>
<description>
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
</description>
<configure name="JEVOIS_UART" value="UARTX" description="UART on which Jevois camera is connected"/>
<configure name="JEVOIS_BAUD" value="B115200" description="UART Baudrate, default to 115200"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="JeVois">
<dl_setting MAX="20" MIN="0" STEP="1" VAR="jevois_mapping_setting" shortname="mapping" module="modules/sensors/cameras/jevois" handler="setmapping"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="jevois_stream_setting" shortname="stream" module="modules/sensors/cameras/jevois" handler="stream" values="FALSE|TRUE"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="jevois.h"/>
</header>
<init fun="jevois_init()"/>
<event fun="jevois_event()"/>
<makefile>
<configure name="JEVOIS_UART" case="upper|lower"/>
<configure name="JEVOIS_BAUD" default="B115200"/>
<file name="jevois.c"/>
<define name="USE_$(JEVOIS_UART_UPPER)"/>
<define name="JEVOIS_DEV" value="$(JEVOIS_UART_LOWER)"/>
<define name="$(JEVOIS_UART_UPPER)_BAUD" value="$(JEVOIS_BAUD)"/>
</makefile>
</module>
sw/airborne/modules/sensors/cameras/jevois.c
+362
View File
@@ -0,0 +1,362 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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
* <http://www.gnu.org/licenses/>.
*/
/**
* @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 <stdio.h>
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);
}
sw/airborne/modules/sensors/cameras/jevois.h
+82
View File
@@ -0,0 +1,82 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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
* <http://www.gnu.org/licenses/>.
*/
/**
* @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
sw/airborne/subsystems/abi_sender_ids.h
+9
View File
@@ -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 */