From 7b643460dc051a0294df870eba97b207daadbf39 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 23 Oct 2019 11:04:34 +0200 Subject: [PATCH] [jevois] move some functionnality to jevois cam driver --- conf/modules/cv_target_localization.xml | 1 - conf/modules/jevois.xml | 1 + .../computer_vision/cv_target_localization.c | 23 -------- .../computer_vision/cv_target_localization.h | 3 - sw/airborne/modules/sensors/cameras/jevois.c | 59 ++++++++++++++++++- sw/airborne/modules/sensors/cameras/jevois.h | 4 ++ 6 files changed, 63 insertions(+), 28 deletions(-) diff --git a/conf/modules/cv_target_localization.xml b/conf/modules/cv_target_localization.xml index 849447476b2..8cdf18439d0 100644 --- a/conf/modules/cv_target_localization.xml +++ b/conf/modules/cv_target_localization.xml @@ -40,7 +40,6 @@ - diff --git a/conf/modules/jevois.xml b/conf/modules/jevois.xml index ea3e3677dc9..6e57e8c910f 100644 --- a/conf/modules/jevois.xml +++ b/conf/modules/jevois.xml @@ -24,6 +24,7 @@ + diff --git a/sw/airborne/modules/computer_vision/cv_target_localization.c b/sw/airborne/modules/computer_vision/cv_target_localization.c index 698ac9e5750..104fcc99fac 100644 --- a/sw/airborne/modules/computer_vision/cv_target_localization.c +++ b/sw/airborne/modules/computer_vision/cv_target_localization.c @@ -241,26 +241,3 @@ void cv_target_localization_report_mark(uint8_t mark) &lat_deg, &lon_deg); } - -#if TARGET_LOC_JEVOIS_ALT -// send current altitude to the jevois camera to help detection algorithm -// this should be moved to the jevois camera driver as an option - -#include "modules/sensors/cameras/jevois.h" -#include "stdio.h" - -void target_localization_send_pos_to_cam(void) -{ - char str[32]; - int alt_mm = (int)(stateGetPositionEnu_f()->z * 1000.f); - Bound(alt_mm, 0, 999999); - sprintf(str, "alt %d\r\n", alt_mm); -#ifndef SITL - jevois_send_string(str); -#endif -} - -#else -void target_localization_send_pos_to_cam(void) {} -#endif - diff --git a/sw/airborne/modules/computer_vision/cv_target_localization.h b/sw/airborne/modules/computer_vision/cv_target_localization.h index 7e9a91654e6..431ea057b9a 100644 --- a/sw/airborne/modules/computer_vision/cv_target_localization.h +++ b/sw/airborne/modules/computer_vision/cv_target_localization.h @@ -39,8 +39,5 @@ extern uint8_t target_localization_mark; extern void cv_target_localization_report_mark(uint8_t mark); extern bool target_localization_update_wp; -// TODO move functionality to the camera driver -extern void target_localization_send_pos_to_cam(void); - #endif diff --git a/sw/airborne/modules/sensors/cameras/jevois.c b/sw/airborne/modules/sensors/cameras/jevois.c index ec3222f35e8..325522e6bcb 100644 --- a/sw/airborne/modules/sensors/cameras/jevois.c +++ b/sw/airborne/modules/sensors/cameras/jevois.c @@ -124,6 +124,26 @@ void jevois_init(void) memset(jevois.buf, 0, JEVOIS_MAX_LEN); } +// extrat a number (int) from an idea string +// this might be needed as jevois ID can start with a letter +// this will extract the first substring with a number +// and return the result of atoi function +static int jevois_extract_nb(char *in) { + unsigned int i, j = 0; + bool first = false; + char out[JEVOIS_MAX_LEN]; + for (i = 0; i < strlen(in)+1; i++) { + if ((in[i] > '0' && in[i] < '9') || in[i] == '-') { + out[j++] = in[i]; + first = true; + } else if (first || in[i] == '\0') { + out[j] = '\0'; + break; + } + } + return atoi(out); +} + // send specific message if requested static void jevois_send_message(void) { @@ -144,7 +164,7 @@ static void jevois_send_message(void) jevois.msg.dim[0], jevois.msg.dim[1], 0, - (int16_t)atoi(jevois.msg.id)); + (int16_t)jevois_extract_nb(jevois.msg.id)); #endif } @@ -413,3 +433,40 @@ void jevois_setmapping(int number) jevois_stream(true); } +void jevois_send_state(void) +{ + char str[32] __attribute__((unused)); +#if JEVOIS_SEND_ALT + // send current altitude in millimeter + int alt_mm = (int)(stateGetPositionEnu_f()->z * 1000.f); + Bound(alt_mm, -999999, 999999); + sprintf(str, "alt %d\r\n", alt_mm); + jevois_send_string(str); +#endif +#if JEVOIS_SEND_POS + // send current position in millimeter + struct EnuCoor_f pos = *stateGetPositionEnu_f(); + int x_mm = (int)(pos.x * 1000.f); + int y_mm = (int)(pos.y * 1000.f); + int z_mm = (int)(pos.z * 1000.f); + Bound(x_mm, -999999, 999999); + Bound(y_mm, -999999, 999999); + Bound(z_mm, -999999, 999999); + sprintf(str, "pos %d %d %d\r\n", x_mm, y_mm, z_mm); + jevois_send_string(str); +#endif +#if JEVOIS_SEND_QUAT + // send quaternion + struct FloatQuat quat = *stateGetNedToBodyQuat_f(); + int qi = (int)(quat.qi * 1000.f); + int qx = (int)(quat.qx * 1000.f); + int qy = (int)(quat.qy * 1000.f); + int qz = (int)(quat.qz * 1000.f); + Bound(qi, -9999, 9999); + Bound(qx, -9999, 9999); + Bound(qy, -9999, 9999); + Bound(qz, -9999, 9999); + sprintf(str, "quat %d %d %d %d\r\n", qi, qx, qy, qz); + jevois_send_string(str); +#endif +} diff --git a/sw/airborne/modules/sensors/cameras/jevois.h b/sw/airborne/modules/sensors/cameras/jevois.h index 39f60bfc8a5..99cc51056a0 100644 --- a/sw/airborne/modules/sensors/cameras/jevois.h +++ b/sw/airborne/modules/sensors/cameras/jevois.h @@ -85,5 +85,9 @@ extern void jevois_setmapping(int number); // dummy variable to change mapping from setting extern int jevois_mapping_setting; +/** Send state to camera + */ +extern void jevois_send_state(void); + #endif