From 4d88bb90386518a1e1834ef58702c0ea32699d17 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 6 Nov 2023 10:00:13 +0100
Subject: [PATCH] [nav] use a low speed for goto with hybrid (#3148)

* [nav] use a low speed for goto with hybrid

* [nav] change name and add documentation

* [nav] fix names in nav_hybrid
---
 conf/modules/nav_hybrid.xml                   | 10 +++++++-
 .../modules/nav/nav_rotorcraft_hybrid.c       | 24 ++++++++++++-------
 .../modules/nav/nav_rotorcraft_hybrid.h       |  3 ++-
 3 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/conf/modules/nav_hybrid.xml b/conf/modules/nav_hybrid.xml
index c40d3c384d..c5e2ccc2ad 100644
--- a/conf/modules/nav_hybrid.xml
+++ b/conf/modules/nav_hybrid.xml
@@ -8,11 +8,19 @@
     <section name="NAV_HYBRID" prefix="NAV_HYBRID_">
       <define name="MAX_DECELERATION" value="1.0" description="Maximum deceleration in [m/s2] when arriving to hover at a WP"/>
     </section>
+    <section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
+      <define name="MAX_AIRSPEED" value="15." description="maximum airspeed (required)"/>
+      <define name="NAV_SPEED_MARGIN" value="10." description="maximum commanded ground speed is MAX_AIRSPEED + NAV_SPEED_MARGIN"/>
+      <define name="GOTO_SPEED" value="nav_max_speed" description="maximum speed when flying goto/stay routines (default is max speed)"/>
+      <define name="NAV_LINE_DIST" value="50" description="distance coefficient for line routine"/>
+      <define name="NAV_CIRCLE_DIST" value="40" description="distance coefficient for circle routine"/>
+    </section>
   </doc>
   <settings>
     <dl_settings>
       <dl_settings NAME="nav_hybrid">
-        <dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0" shortname="nav_max_speed"/>
+        <dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0"/>
+        <dl_setting var="nav_hover_speed" min="0.1" step="0.1" max="10.0"/>
         <dl_setting var="nav_max_deceleration_sp" min="0.5" step="0.1" max="10.0" shortname="max_deceleration" param="NAV_HYBRID_MAX_DECELERATION"/>
       </dl_settings>
     </dl_settings>
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
index 6429cc702a..b2c7d7efcf 100644
--- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
+++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
@@ -35,6 +35,13 @@
 #define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
 float nav_max_speed = NAV_MAX_SPEED;
 
+// Max ground speed in with goto/stay instruction
+// by default, same as route speed
+#ifndef GUIDANCE_INDI_GOTO_SPEED
+#define GUIDANCE_INDI_GOTO_SPEED NAV_MAX_SPEED
+#endif
+float nav_goto_speed = GUIDANCE_INDI_GOTO_SPEED;
+
 #ifndef NAV_HYBRID_MAX_DECELERATION
 #define NAV_HYBRID_MAX_DECELERATION 1.0
 #endif
@@ -72,22 +79,23 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
   struct FloatVect2 speed_sp;
   VECT2_SMUL(speed_sp, pos_error, gih_params.pos_gain);
 
-  if (force_forward) {
-    float_vect2_scale_in_2d(&speed_sp, nav_max_speed);
-  } else {
+  // Bound the setpoint velocity vector
+  float max_h_speed = nav_max_speed;
+  if (!force_forward) {
+    // If not in force_forward, compute speed based on decceleration and nav_goto_speed
     // Calculate distance to waypoint
     float dist_to_wp = float_vect2_norm(&pos_error);
     // Calculate max speed when decelerating at MAX capacity a_max
     // distance travelled d = 1/2 a_max t^2
     // The time in which it does this is: T = V / a_max
-    // The maximum speed at which to fly to still allow arriving with zero 
+    // The maximum speed at which to fly to still allow arriving with zero
     // speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d)
     float max_speed_decel2 = fabsf(2.f * dist_to_wp * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
     float max_speed_decel = sqrtf(max_speed_decel2);
     // Bound the setpoint velocity vector
-    float max_h_speed = Min(nav_max_speed, max_speed_decel);
-    float_vect2_bound_in_2d(&speed_sp, max_h_speed);
+    max_h_speed = Min(nav_goto_speed, max_speed_decel); // use hover max speed
   }
+  float_vect2_bound_in_2d(&speed_sp, max_h_speed);
 
   VECT2_COPY(nav.speed, speed_sp);
   nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
@@ -224,8 +232,8 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
       desired_speed = radius_diff * gih_params.pos_gain;
     } else {
       // close to circle, speed function of radius for a feasible turn
-      // MAX_BANK / 2 gives some margins for the turns
-      desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(GUIDANCE_H_MAX_BANK / 2.f));
+      // 0.8 * MAX_BANK gives some margins for the turns
+      desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * GUIDANCE_H_MAX_BANK));
     }
     Bound(desired_speed, 0.0f, nav_max_speed);
   }
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
index dbde32cba9..7e6a73fda2 100644
--- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
+++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
@@ -32,7 +32,8 @@
 #include "modules/nav/nav_rotorcraft_base.h"
 
 // settings
-extern float nav_max_speed;
+extern float nav_max_speed;   // max speed in route mode
+extern float nav_hover_speed; // max speed in goto/stay mode
 extern float nav_max_deceleration_sp;
 
 extern void nav_rotorcraft_hybrid_init(void);