From 4b6b0acf87991f4e1b3a6c869362cb76e5a37a7f Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 3 Oct 2022 22:57:01 +0200
Subject: [PATCH] [module] add tag speed control

---
 conf/modules/tag_tracking.xml                 |  6 +++
 .../modules/computer_vision/tag_tracking.c    | 45 ++++++++++++++++++-
 .../modules/computer_vision/tag_tracking.h    |  5 +++
 3 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/conf/modules/tag_tracking.xml b/conf/modules/tag_tracking.xml
index 1c9f7172a0..c45b88d415 100644
--- a/conf/modules/tag_tracking.xml
+++ b/conf/modules/tag_tracking.xml
@@ -22,6 +22,10 @@
       <define name="TIMEOUT" value="5." description="timeout for lost tracking in seconds"/>
       <define name="PREDICT_TIME" value="1." description="position prediction time when moving waypoint"/>
       <define name="MAX_OFFSET" value="2." description="maximum position offset for predicted positin compared to estimated position"/>
+      <define name="MAX_SPEED" value="4." description="maximum horiontal speed for the tracking control"/>
+      <define name="MAX_VZ" value="2." description="maximum vertical speed for the tracking control"/>
+      <define name="KP" value="0.5" description="horizontal gain on position error"/>
+      <define name="KPZ" value="0.2" description="vertical gain on position error"/>
     </section>
   </doc>
   <settings target="ap|nps">
@@ -29,6 +33,8 @@
       <dl_settings NAME="Tag Tracking">
         <dl_setting var="tag_tracking.motion_type" min="0" step="1" max="1" shortname="motion" values="FIXED|MOVING" module="modules/computer_vision/tag_tracking"/>
         <dl_setting var="tag_tracking.predict_time" min="0." step="0.1" max="5." shortname="pred_time"/>
+        <dl_setting var="tag_tracking.kp" min="0." step="0.01" max="5." shortname="kp"/>
+        <dl_setting var="tag_tracking.kpz" min="0." step="0.01" max="5." shortname="kpz"/>
       </dl_settings>
     </dl_settings>
   </settings>
diff --git a/sw/airborne/modules/computer_vision/tag_tracking.c b/sw/airborne/modules/computer_vision/tag_tracking.c
index 526982e95f..96cfeccfa1 100644
--- a/sw/airborne/modules/computer_vision/tag_tracking.c
+++ b/sw/airborne/modules/computer_vision/tag_tracking.c
@@ -127,13 +127,29 @@ float speed_circle = 0.03;
 #endif
 
 #ifndef TAG_TRACKING_PREDICT_TIME
-#define TAG_TRACKING_PREDICT_TIME 1.5f
+#define TAG_TRACKING_PREDICT_TIME 1.f
 #endif
 
 #ifndef TAG_TRACKING_MAX_OFFSET
 #define TAG_TRACKING_MAX_OFFSET 2.0f
 #endif
 
+#ifndef TAG_TRACKING_KP
+#define TAG_TRACKING_KP 0.5f
+#endif
+
+#ifndef TAG_TRACKING_KPZ
+#define TAG_TRACKING_KPZ 0.2f
+#endif
+
+#ifndef TAG_TRACKING_MAX_SPEED
+#define TAG_TRACKING_MAX_SPEED 4.f
+#endif
+
+#ifndef TAG_TRACKING_MAX_VZ
+#define TAG_TRACKING_MAX_VZ 2.f
+#endif
+
 // generated in modules.h
 static const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD;
 
@@ -233,7 +249,7 @@ void tag_tracking_parse_target_pos(uint8_t *buf)
 }
 
 // Update and display tracking WP
-static void update_wp(bool report)
+static void update_wp(bool report UNUSED)
 {
 #ifdef TAG_TRACKING_WP
   struct FloatVect3 target_pos_enu, target_pos_pred;
@@ -263,6 +279,7 @@ void tag_tracking_init()
   FLOAT_VECT3_ZERO(tag_track_private.meas);
   FLOAT_VECT3_ZERO(tag_tracking.pos);
   FLOAT_VECT3_ZERO(tag_tracking.speed);
+  FLOAT_VECT3_ZERO(tag_tracking.speed_cmd);
   struct FloatEulers euler = {
     TAG_TRACKING_BODY_TO_CAM_PHI,
     TAG_TRACKING_BODY_TO_CAM_THETA,
@@ -273,6 +290,8 @@ void tag_tracking_init()
       TAG_TRACKING_CAM_POS_X,
       TAG_TRACKING_CAM_POS_Y,
       TAG_TRACKING_CAM_POS_Z);
+  tag_tracking.kp = TAG_TRACKING_KP;
+  tag_tracking.kpz = TAG_TRACKING_KPZ;
 
   // Bind to ABI message
   AbiBindMsgJEVOIS_MSG(TAG_TRACKING_ID, &tag_track_ev, tag_track_cb);
@@ -365,6 +384,28 @@ void tag_tracking_report()
   }
 }
 
+/** Control function
+ *
+ * calling this function only updates the command vector
+ * it can be applied to the guidance control using the guided mode
+ * or from the flight plan with 'guided' instruction
+ */
+void tag_tracking_compute_speed(void)
+{
+  if (tag_tracking.status == TAG_TRACKING_RUNNING) {
+    // compute speed command as estimated tag speed + gain * position error
+    struct NedCoor_f pos = *stateGetPositionNed_f();
+    tag_tracking.speed_cmd.x = tag_tracking.speed.x + tag_tracking.kp * (tag_tracking.pos.x - pos.x);
+    tag_tracking.speed_cmd.y = tag_tracking.speed.y + tag_tracking.kp * (tag_tracking.pos.y - pos.y);
+    tag_tracking.speed_cmd.z = tag_tracking.speed.z + tag_tracking.kpz * (tag_tracking.pos.z - pos.z);
+    VECT2_STRIM(tag_tracking.speed_cmd, -TAG_TRACKING_MAX_SPEED, TAG_TRACKING_MAX_SPEED); // trim max horizontal speed
+    BoundAbs(tag_tracking.speed_cmd.z, TAG_TRACKING_MAX_VZ); // tim max vertical speed
+  }
+  else {
+    // filter is not runing, set speed command to zero
+    FLOAT_VECT3_ZERO(tag_tracking.speed_cmd);
+  }
+}
 
 // Simulate detection using a WP coordinate
 #if defined SITL && defined TAG_TRACKING_SIM_WP
diff --git a/sw/airborne/modules/computer_vision/tag_tracking.h b/sw/airborne/modules/computer_vision/tag_tracking.h
index 2c895606cb..be0a80faca 100644
--- a/sw/airborne/modules/computer_vision/tag_tracking.h
+++ b/sw/airborne/modules/computer_vision/tag_tracking.h
@@ -31,6 +31,7 @@
 
 #include "std.h"
 #include "math/pprz_algebra_float.h"
+#include "math/pprz_geodetic_float.h"
 
 // Searching status
 #define TAG_TRACKING_SEARCHING  0
@@ -49,6 +50,9 @@ struct tag_tracking_public {
   uint8_t status;               ///< tracking status flag
   uint8_t motion_type;          ///< type of tag motion
   float predict_time;           ///< prediction time for WP tag
+  struct NedCoor_f speed_cmd;   ///< speed command to track the tag position
+  float kp;                     ///< horizontal tracking command gain
+  float kpz;                    ///< vertical tracking command gain
 };
 
 extern struct tag_tracking_public tag_tracking;
@@ -58,6 +62,7 @@ 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);
 
 #endif  // TAG_TRACKING_H