[nav_rotorcraft_hybrid] tune deceleration (#3102)

Updated comments
paparazzi · Sep 25, 2023 · 0e50371 · 0e50371
1 parent b65e1da
commit 0e50371
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 4 deletions.
diff --git a/conf/modules/nav_hybrid.xml b/conf/modules/nav_hybrid.xml
@@ -5,11 +5,15 @@
     <description>
       Navigation patterns and flight plan handling for hybrid airframes
     </description>
+    <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>
   </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_deceleration_sp" min="0.5" step="0.1" max="10.0" shortname="max_deceleration" param="NAV_HYBRID_MAX_DECELERATION"/>
       </dl_settings>
     </dl_settings>
   </settings>

diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
@@ -35,10 +35,12 @@
 #define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
 float nav_max_speed = NAV_MAX_SPEED;
 
-#ifndef MAX_DECELERATION
-#define MAX_DECELERATION 1.f
+#ifndef NAV_HYBRID_MAX_DECELERATION
+#define NAV_HYBRID_MAX_DECELERATION 1.0
 #endif
 
+float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION;
+
 #ifdef GUIDANCE_INDI_LINE_GAIN
 static float guidance_indi_line_gain = GUIDANCE_INDI_LINE_GAIN;
 #else
@@ -75,8 +77,12 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
   } else {
     // Calculate distance to waypoint
     float dist_to_wp = float_vect2_norm(&pos_error);
-    // Calculate max speed to decelerate from
-    float max_speed_decel2 = fabsf(2.f * dist_to_wp * MAX_DECELERATION); // dist_to_wp can only be positive, but just in case
+    // 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 
+    // 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);

diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
@@ -33,6 +33,7 @@
 
 // settings
 extern float nav_max_speed;
+extern float nav_max_deceleration_sp;
 
 extern void nav_rotorcraft_hybrid_init(void);