Collection of changes after IMAV2024 (#3370)

- update airframe and add competition flight plans
- fix flight plan DTD
- mag params for Bristol
- add a set_expo function (might be weak) for digital cam driver
- fix tag tracking for simulation, add getter functions
- add option to skip initial circle in nav survey
- fix jevois driver parsing

sw/airborne/modules/computer_vision/tag_tracking.c

@@ -61,6 +61,10 @@ static void tag_motion_sim(void);
 #define TAG_MOTION_RANGE_X 4.f
 #define TAG_MOTION_RANGE_Y 4.f
 
+#ifndef TAG_TRACKING_SIM_ID
+#define TAG_TRACKING_SIM_ID "U1"
+#endif
+
 static uint8_t tag_motion_sim_type = TAG_MOTION_NONE;
 static struct FloatVect3 tag_motion_speed = { TAG_MOTION_SPEED_X, TAG_MOTION_SPEED_Y, 0.f };
 
@@ -179,8 +183,8 @@ struct tag_info {
 };
 
 struct wp_tracking {
-  uint8_t wp_id;
   int16_t tag_id;
+  uint8_t wp_id;
 };
 
 
@@ -215,7 +219,7 @@ struct tag_tracking_public* tag_tracking_get(int16_t tag_id) {
     }
 
     // tag_id == TAG_TRACKING_ANY, returns the first running tag.
-    if(tag_id == TAG_TRACKING_ANY && tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
+    if (tag_id == TAG_TRACKING_ANY && tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
       return &tag_infos[i].tag_tracking;
     }
   }
@@ -225,6 +229,16 @@ struct tag_tracking_public* tag_tracking_get(int16_t tag_id) {
   return &dummy;
 }
 
+uint8_t tag_tracking_get_status(int16_t tag_id)
+{
+  return tag_tracking_get(tag_id)->status;
+}
+
+uint8_t tag_tracking_get_motion_type(int16_t tag_id)
+{
+  return tag_tracking_get(tag_id)->motion_type;
+}
+
 float tag_tracking_get_heading(int16_t tag_id) {
   struct tag_tracking_public* tag = tag_tracking_get(tag_id);
   struct FloatEulers e;
@@ -278,13 +292,13 @@ static void tag_track_cb(uint8_t sender_id UNUSED,
 {
   if (type == JEVOIS_MSG_D3) {
     int16_t tag_id = (int16_t)jevois_extract_nb(id);
-    for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
+    for (int i=0; i<TAG_TRACKING_NB_MAX; i++) {
       // free slot, store tag ID
-      if(tag_infos[i].tag_track_private.id == TAG_UNUSED_ID) {
+      if (tag_infos[i].tag_track_private.id == TAG_UNUSED_ID) {
         tag_infos[i].tag_track_private.id = tag_id;
       }
 
-      if(tag_infos[i].tag_track_private.id == tag_id) {
+      if (tag_infos[i].tag_track_private.id == tag_id) {
         // store data from Jevois detection
         tag_infos[i].tag_track_private.meas.x = coord[0] * TAG_TRACKING_COORD_TO_M;
         tag_infos[i].tag_track_private.meas.y = coord[1] * TAG_TRACKING_COORD_TO_M;
@@ -324,9 +338,7 @@ void tag_tracking_parse_target_pos(uint8_t *buf)
 // Update and display tracking WP
 static void update_wp(struct tag_info* tag_info UNUSED, bool report UNUSED)
 {
-#ifdef TAG_TRACKING_WPS
-
-  if(tag_info->wp_id == 0) {
+  if (tag_info->wp_id == 0) {
     // not associated with any WP
     return;
   }
@@ -348,8 +360,6 @@ static void update_wp(struct tag_info* tag_info UNUSED, bool report UNUSED)
   } else {
     waypoint_set_enu_i(tag_info->wp_id, &pos_i);
   }
-
-#endif
 }
 
 // Init function
@@ -498,7 +508,7 @@ void tag_tracking_report()
  */
 void tag_tracking_compute_speed(void)
 {
-  for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
+  for (int i = 0; i < TAG_TRACKING_NB_MAX; i++) {
     if (tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
       // compute speed command as estimated tag speed + gain * position error
       struct NedCoor_f pos = *stateGetPositionNed_f();
@@ -515,6 +525,18 @@ void tag_tracking_compute_speed(void)
   }
 }
 
+bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id)
+{
+  for (int i = 0; i < TAG_TRACKING_NB_MAX; i++) {
+    if (tag_infos[i].tag_track_private.id == TAG_UNUSED_ID || tag_infos[i].tag_track_private.id == tag_id) {
+      tag_infos[i].tag_track_private.id = tag_id;
+      tag_infos[i].wp_id = wp_id;
+      return true;
+    }
+  }
+  return false; // fail to set tracker id
+}
+
 // Simulate detection using a WP coordinate
 #if defined SITL && defined TAG_TRACKING_SIM_WP
 static void tag_tracking_sim(void)
@@ -551,7 +573,7 @@ static void tag_tracking_sim(void)
       uint16_t dim[3] = { 100, 100, 0 };
       struct FloatQuat quat; // TODO
       float_quat_identity(&quat);
-      AbiSendMsgJEVOIS_MSG(42, JEVOIS_MSG_D3, "1", 3, coord, dim, quat, "");
+      AbiSendMsgJEVOIS_MSG(42, JEVOIS_MSG_D3, TAG_TRACKING_SIM_ID, 3, coord, dim, quat, "");
     }
   }
 }

sw/airborne/modules/computer_vision/tag_tracking.h

@@ -78,13 +78,16 @@ void tag_tracking_set_kp(float kp);
 void tag_tracking_set_kpz(float kpz);
 
 extern struct tag_tracking_public* tag_tracking_get(int16_t tag_id);
+extern uint8_t tag_tracking_get_status(int16_t tag_id);
+extern uint8_t tag_tracking_get_motion_type(int16_t tag_id);
 extern void tag_tracking_init(void);
 extern void tag_tracking_propagate(void);
 extern void tag_tracking_propagate_start(void);
 extern void tag_tracking_report(void);
 extern void tag_tracking_parse_target_pos(uint8_t *buf);
 extern void tag_tracking_compute_speed(void);
-float tag_tracking_get_heading(int16_t tag_id);
+extern bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id);
+extern float tag_tracking_get_heading(int16_t tag_id);
 
 #endif  // TAG_TRACKING_H
 

sw/airborne/modules/digital_cam/dc.c

@@ -69,6 +69,8 @@ uint16_t dc_gps_count = 0;
 uint8_t dc_cam_tracing = 1;
 float dc_cam_angle = 0;
 
+float dc_exposure;
+
 float dc_circle_interval = 0;
 float dc_circle_start_angle = 0;
 float dc_circle_last_block = 0;
@@ -155,6 +157,7 @@ void dc_send_shot_position(void)
 
 void dc_init(void)
 {
+  dc_exposure = 0.f;
   dc_autoshoot = DC_AUTOSHOOT_STOP;
   dc_autoshoot_period = DC_AUTOSHOOT_PERIOD;
   dc_distance_interval = DC_AUTOSHOOT_DISTANCE_INTERVAL;
@@ -195,6 +198,8 @@ void dc_send_command_common(uint8_t cmd __attribute__((unused)))
 #endif
 }
 
+// weak function for set_expo, to be implemented by drivers if available
+void WEAK dc_set_expo(float expo UNUSED) {}
 
 /* shoot on distance */
 uint8_t dc_distance(float interval)

sw/airborne/modules/digital_cam/dc.h

@@ -48,6 +48,8 @@ extern uint16_t dc_photo_nr;
 /** number of images taken since the last change of dc_mode */
 extern uint16_t dc_gps_count;
 
+/** camera exposure */
+extern float dc_exposure;
 
 /*
  * Variables for PERIODIC mode.
@@ -123,6 +125,9 @@ extern void dc_send_command(uint8_t cmd);
 /** Command sending function */
 extern void dc_send_command_common(uint8_t cmd);
 
+/** Set camera exposure */
+extern void dc_set_expo(float expo);
+
 /** Auotmatic Digital Camera Photo Triggering modes */
 typedef enum {
   DC_AUTOSHOOT_STOP = 0,

sw/airborne/modules/digital_cam/pprzlink_cam_ctrl.c

@@ -29,11 +29,9 @@
 // Include Standard Camera Control Interface
 #include "dc.h"
 
-float digital_cam_exposure;
-
 void pprzlink_cam_ctrl_init(void)
 {
-  digital_cam_exposure = 0.f; // auto expo
+  dc_exposure = PPRZLINK_CAM_AUTO_EXPO;
 }
 
 void pprzlink_cam_ctrl_periodic(void)
@@ -57,9 +55,9 @@ void dc_send_command(uint8_t cmd)
 #include "modules/datalink/downlink.h"
 #include "modules/datalink/extra_pprz_dl.h"
 
-void pprzlink_cam_ctrl_set_expo(float expo)
+void dc_set_expo(float expo)
 {
-  digital_cam_exposure = expo;
+  dc_exposure = expo;
   uint8_t tab[2];
   tab[0] = 'e';
   tab[1] = (uint8_t)(expo * 10.f);
@@ -72,7 +70,7 @@ void dc_expo_cb(uint8_t* buf) {
 
   // feedback from camera
   if (DL_PAYLOAD_COMMAND_command_length(buf) == 2 && DL_PAYLOAD_COMMAND_command(buf)[0] == 'e') {
-    digital_cam_exposure = DL_PAYLOAD_COMMAND_command(buf)[1] / 10.0;
+    dc_exposure = DL_PAYLOAD_COMMAND_command(buf)[1] / 10.0;
   }
 }
 

sw/airborne/modules/digital_cam/pprzlink_cam_ctrl.h

@@ -32,7 +32,6 @@ extern void pprzlink_cam_ctrl_init(void);
 extern void pprzlink_cam_ctrl_periodic(void);
 
 /** Set expo setting */
-extern float digital_cam_exposure;
 extern void pprzlink_cam_ctrl_set_expo(float expo);
 extern void dc_expo_cb(uint8_t* buf);
 #define PPRZLINK_CAM_AUTO_EXPO 0.f

sw/airborne/modules/nav/common_flight_plan.h

@@ -60,4 +60,7 @@ void nav_goto_block(uint8_t block_id);
 /** Time in s since the entrance in the current block */
 #define NavBlockTime() (block_time)
 
+/** macro to use pointers in flight plans XML */
+#define Ptr(x) (&(x))
+
 #endif /* COMMON_FLIGHT_PLAN_H */

sw/airborne/modules/nav/nav_survey_hybrid.c

@@ -69,6 +69,11 @@
 #define SURVEY_HYBRID_ENTRY_DISTANCE (survey_private.sweep_distance / 2.f)
 #endif
 
+// make a circle at entry point if radius is not 0
+#ifndef SURVEY_HYBRID_ENTRY_CIRCLE
+#define SURVEY_HYBRID_ENTRY_CIRCLE TRUE
+#endif
+
 struct Line {float m; float b; float x;};
 enum SurveyStatus { Init, Entry, Sweep, Turn };
 
@@ -417,7 +422,7 @@ bool nav_survey_hybrid_run(void)
       RotateAndTranslateToWorld(&C, 0.f, &survey_private.smallest_corner);
       RotateAndTranslateToWorld(&C, survey_private.orientation, &zero);
 
-      if (survey_private.circle_turns) {
+      if (survey_private.circle_turns && SURVEY_HYBRID_ENTRY_CIRCLE) {
         // align segment at entry point with a circle
         survey_private.circle.x = C.x - (cosf(survey_private.orientation + M_PI_2) * survey_private.radius);
         survey_private.circle.y = C.y - (sinf(survey_private.orientation + M_PI_2) * survey_private.radius);

sw/airborne/modules/sensors/cameras/jevois.c

@@ -376,7 +376,10 @@ static void jevois_parse(struct jevois_t *jv, char c)
         if (c == '\n') {
           jevois_handle_msg(jv);
           jv->state = JV_TYPE;
-        } else {
+        } else if( c=='\r') {
+          // do nothing, just to consume '\r'
+        }
+        else {
           jv->state = JV_SYNC;
         }
       }