Some changes to NAV Hybrid (#3335)

* first push

* Addressed the comments

* address comments

* Correct prefix and variable name

* make radius circle limiting optional

* first push

* Addressed the comments

* address comments

* Correct prefix and variable name

* make radius circle limiting optional

* Added soft acceleration limit

* Remove nav goto where not used. Added logic for minimum no-transition distance. Code clean-up.

* Add possibility to force max goto speed if needed

* Removed nav_goto_max_speed

conf/airframes/tudelft/rotwing_v3c_oneloop.xml

@@ -120,8 +120,10 @@
             <define name="NAV_HYBRID_MAX_AIRSPEED"      value="19.0f"/>
             <define name="NAV_HYBRID_SPEED_MARGIN"      value="0.0f"/>
             <define name="NAV_HYBRID_MAX_ACCELERATION"  value="4.0f"/>
+            <define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
             <define name="NAV_HYBRID_MAX_DECELERATION"  value="1.0f"/>
             <define name="GUIDANCE_H_USE_REF"           value="FALSE"/>
+            <define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
         </module>
 
         <module name="oneloop" type="andi">

conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml

@@ -120,9 +120,11 @@
             <define name="NAV_HYBRID_MAX_AIRSPEED"      value="19.0f"/>
             <define name="NAV_HYBRID_SPEED_MARGIN"      value="0.0f"/>
             <define name="NAV_HYBRID_MAX_ACCELERATION"  value="4.0f"/>
+            <define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
             <define name="NAV_HYBRID_MAX_DECELERATION"  value="1.0f"/>
             <define name="GUIDANCE_H_USE_REF"           value="FALSE"/>
-            <define name="NAV_OPTITRACK"                value="TRUE"/>
+            <define name="NAV_HYBRID_EXT_VISION_SETPOINT_MODE" value="TRUE"/>
+            <define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
         </module>
 
         <module name="oneloop" type="andi">

conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml

@@ -112,9 +112,11 @@
             <define name="NAV_HYBRID_MAX_AIRSPEED"      value="19.0f"/>
             <define name="NAV_HYBRID_SPEED_MARGIN"      value="0.0f"/>
             <define name="NAV_HYBRID_MAX_ACCELERATION"  value="4.0f"/>
+            <define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
             <define name="NAV_HYBRID_MAX_DECELERATION"  value="1.0f"/>
             <define name="GUIDANCE_H_USE_REF"           value="FALSE"/>
-            <define name="NAV_OPTITRACK"                value="TRUE"/>
+            <define name="NAV_HYBRID_EXT_VISION_SETPOINT_MODE" value="TRUE"/>
+            <define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
         </module>
 
         <module name="oneloop" type="andi">

conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml

@@ -81,8 +81,10 @@
             <define name="NAV_HYBRID_MAX_SPEED_V"       value="3.0f"/>
             <define name="NAV_HYBRID_SPEED_MARGIN"      value="0.0f"/>
             <define name="NAV_HYBRID_MAX_ACCELERATION"  value="4.0f"/>
+            <define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
             <define name="NAV_HYBRID_MAX_DECELERATION"  value="1.0f"/>
             <define name="GUIDANCE_H_USE_REF"           value="FALSE"/>
+            <define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
         </module>
 
         <module name="oneloop" type="andi">

conf/modules/nav_hybrid.xml

@@ -7,6 +7,9 @@
     </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"/>
+      <define name="MAX_EXPECTED_WIND" value="5" description="Maximum expected constant wind. Used to calculate minimum circle radius to not exceed acceleration bound."/>
+      <define name="LIMIT_CIRCLE_RADIUS" value="FALSE" description="Limit the circle radius on max target groundspeed and max acceleration."/>
+      <define name="EXT_VISION_SETPOINT_MODE" value="FALSE" description="Use position setpoints when using external vision systems."/>
     </section>
     <section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
       <define name="MAX_AIRSPEED" value="15." description="maximum airspeed (required)"/>
@@ -19,8 +22,9 @@
   <settings>
     <dl_settings>
       <dl_settings NAME="nav_hybrid">
+        <dl_setting var="nav_hybrid_max_acceleration" min="1.0" step="0.1" max="9.0"/>
         <dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0"/>
-        <dl_setting var="nav_goto_max_speed" min="0.1" step="0.1" max="50.0"/>
+        <dl_setting var="nav_hybrid_max_expected_wind" min="0.0" step="0.1" max="20.0" shortname="max_expected_wind"/>
         <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_setting var="nav_hybrid_line_gain" min="0.1" step="0.1" max="3" shortname="nav_hybrid_line_gain" param="NAV_HYBRID_LINE_GAIN"/>
         <dl_setting var="nav_hybrid_pos_gain" min="0.1" step="0.1" max="10" shortname="nav_hybrid_pos_gain" param="NAV_HYBRID_POS_GAIN"/>

sw/airborne/modules/nav/nav_rotorcraft_hybrid.c

@@ -51,13 +51,27 @@ float nav_max_speed = NAV_MAX_SPEED;
 #define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED
 #endif
 
-float nav_goto_max_speed = NAV_HYBRID_GOTO_MAX_SPEED;
-
 #ifndef NAV_HYBRID_MAX_DECELERATION
 #define NAV_HYBRID_MAX_DECELERATION 1.0
 #endif
 
-float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION;
+#ifndef NAV_HYBRID_MAX_ACCELERATION
+#define NAV_HYBRID_MAX_ACCELERATION 4.0
+#endif
+
+#ifndef NAV_HYBRID_SOFT_ACCELERATION
+#define NAV_HYBRID_SOFT_ACCELERATION NAV_HYBRID_MAX_ACCELERATION
+#endif
+
+float nav_max_deceleration_sp      = NAV_HYBRID_MAX_DECELERATION;  //Maximum deceleration allowed for the set-point
+float nav_max_acceleration_sp      = NAV_HYBRID_MAX_ACCELERATION;  //Maximum acceleration allowed for the set-point
+float nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION; //Soft acceleration limit allowed for the set-point (Equal to the maximum acceleration by default)
+float nav_hybrid_max_acceleration  = NAV_HYBRID_MAX_ACCELERATION;  //Maximum general limit in acceleration allowed for the hybrid navigation
+
+#ifndef NAV_HYBRID_MAX_EXPECTED_WIND
+#define NAV_HYBRID_MAX_EXPECTED_WIND 5.0f
+#endif
+float nav_hybrid_max_expected_wind = NAV_HYBRID_MAX_EXPECTED_WIND;
 
 #ifdef NAV_HYBRID_LINE_GAIN
 float nav_hybrid_line_gain = NAV_HYBRID_LINE_GAIN;
@@ -83,15 +97,21 @@ float nav_hybrid_pos_gain = 1.0f;
 bool force_forward = 0.0f;
 #endif
 
-#ifndef NAV_HYBRID_GOTO_MODE
+/* When using external vision, run the nav functions in position mode for better tracking*/
-#define NAV_HYBRID_GOTO_MODE NAV_SETPOINT_MODE_SPEED
+#ifndef NAV_HYBRID_EXT_VISION_SETPOINT_MODE
+#define NAV_HYBRID_EXT_VISION_SETPOINT_MODE FALSE 
 #endif
 
+/* Use max target groundspeed and max acceleration to limit circle radius*/
+#ifndef NAV_HYBRID_LIMIT_CIRCLE_RADIUS
+#define NAV_HYBRID_LIMIT_CIRCLE_RADIUS FALSE 
+#endif
 /** Implement basic nav function for the hybrid case
  */
 
 static void nav_hybrid_goto(struct EnuCoor_f *wp)
 {
+  nav_max_acceleration_sp = nav_hybrid_soft_acceleration;
   nav_rotorcraft_base.goto_wp.to = *wp;
   nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
   VECT2_COPY(nav.target, *wp);
@@ -107,7 +127,7 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
   // 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_max_speed
+    // If not in force_forward, compute speed based on deceleration and nav_max_speed
     // Calculate distance to waypoint
     float dist_to_wp = float_vect2_norm(&pos_error);
     // Calculate max speed when decelerating at MAX capacity a_max
@@ -118,13 +138,19 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
     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
-    max_h_speed = Min(nav_goto_max_speed, max_speed_decel); // use hover max speed
+    max_h_speed = Min(nav_max_speed, max_speed_decel);
   }
   float_vect2_bound_in_2d(&speed_sp, max_h_speed);
 
   VECT2_COPY(nav.speed, speed_sp);
   nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
-  nav.setpoint_mode = NAV_HYBRID_GOTO_MODE;
+
+  // In optitrack tests use position mode for more precise hovering
+#if NAV_HYBRID_EXT_VISION_SETPOINT_MODE
+    nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
+#else
+    nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
+#endif
 }
 
 static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
@@ -225,7 +251,10 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
   float sign_radius = radius > 0.f ? 1.f : -1.f;
   // absolute radius
   float abs_radius = fabsf(radius);
-
+#if NAV_HYBRID_LIMIT_CIRCLE_RADIUS
+  float min_radius =  pow(nav_max_speed+nav_hybrid_max_expected_wind,2) / (nav_hybrid_max_acceleration * 0.8);
+  abs_radius = Max(abs_radius, min_radius);
+#endif
   if (abs_radius > 0.1f) {
     // store last qdr
     float last_qdr = nav_rotorcraft_base.circle.qdr;
@@ -248,20 +277,22 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
     VECT2_COPY(nav.target, *wp_center);
   }
   // compute desired speed
+  float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
+  if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
+    // far from circle, speed proportional to diff
+    desired_speed = radius_diff * nav_hybrid_pos_gain;
+    nav_max_acceleration_sp = nav_hybrid_soft_acceleration;
+  } else {
+    // close to circle, speed function of radius for a feasible turn
+    // 0.8 * MAX_BANK gives some margins for the turns
+    desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * nav_hybrid_max_bank));
+    nav_max_acceleration_sp = nav_hybrid_max_acceleration ;
+  }
   if (force_forward) {
     desired_speed = nav_max_speed;
-  } else {
+    nav_max_acceleration_sp = nav_hybrid_max_acceleration ;
-    float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
-    if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
-      // far from circle, speed proportional to diff
-      desired_speed = radius_diff * nav_hybrid_pos_gain;
-    } else {
-      // close to circle, speed function of radius for a feasible turn
-      // 0.8 * MAX_BANK gives some margins for the turns
-      desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * nav_hybrid_max_bank));
-    }
-    Bound(desired_speed, 0.0f, nav_max_speed);
   }
+  Bound(desired_speed, 0.0f, nav_max_speed);
   // compute speed vector from target position
   struct FloatVect2 speed_unit;
   VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f());
@@ -269,7 +300,7 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
   VECT2_SMUL(nav.speed, speed_unit, desired_speed);
 
   nav_rotorcraft_base.circle.center = *wp_center;
-  nav_rotorcraft_base.circle.radius = radius;
+  nav_rotorcraft_base.circle.radius = sign_radius * abs_radius;
   nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE;
   nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
 }

sw/airborne/modules/nav/nav_rotorcraft_hybrid.h

@@ -33,16 +33,17 @@
 
 // settings
 extern float nav_max_speed;   // max speed in route mode
-extern float nav_goto_max_speed; // max speed in goto/stay mode
 extern float nav_max_deceleration_sp;
+extern float nav_max_acceleration_sp; // Maximum limit that can vary depending on the function
+extern float nav_hybrid_max_acceleration; // General setting
 extern float nav_hybrid_line_gain; 
 extern float nav_hybrid_pos_gain; 
 extern float nav_hybrid_max_bank;
+extern float nav_hybrid_max_expected_wind;
 #ifndef GUIDANCE_INDI_HYBRID
 extern bool force_forward;
 #endif
 
-
 extern void nav_rotorcraft_hybrid_init(void);
 
 #endif

sw/airborne/modules/rotwing_drone/rotwing_state.c

@@ -552,7 +552,6 @@ void rotwing_state_set_state_settings(void)
   force_forward = rotwing_state_settings.force_forward;
 
   nav_max_speed = rotwing_state_settings.nav_max_speed;
-  nav_goto_max_speed = rotwing_state_settings.nav_max_speed;
 
   // TO DO: pitch angle now hard coded scheduled by wing angle
 

sw/airborne/modules/rotwing_drone/rotwing_state_V2.c

@@ -603,7 +603,6 @@ void rotwing_state_set_state_settings(void)
   force_forward = rotwing_state_settings.force_forward;
 
   nav_max_speed = rotwing_state_settings.nav_max_speed;
-  nav_goto_max_speed = rotwing_state_settings.nav_max_speed;
 
   // TO DO: pitch angle now hard coded scheduled by wing angle