INDI hybrid updates (#2954)

- Two interpolation segments during transition
- Quadratic lift effectiveness with airspeed
- Velocity setpoint input possible from module via ABI message

conf/abi.xml

@@ -107,7 +107,7 @@
       <field name="noise_y" type="float"/>
       <field name="noise_z" type="float"/>
     </message>
-    
+
     <message name="UWB_COMMUNICATION" id="19">
       <field name="id" type="uint8_t"/>
       <field name="range" type="float" unit="m"/>
@@ -190,7 +190,7 @@
     </message>
 
     <message name="JOYSTICK" id="28">
-      <!-- 
+      <!--
            This message can be used to send command inputs from a joystick or equivalent
            either for the control of the airframe or the control of the payload
       -->
@@ -242,6 +242,10 @@
       <field name="dt" type="uint16_t" unit="us"/>
     </message>
 
+    <message name="VEL_SP" id="36">
+      <field name="vel_sp" type="struct FloatVect3 *" unit="m/s"/>
+    </message>
+
   </msg_class>
 
 </protocol>

conf/airframes/ENAC/cyfoam.xml

@@ -91,7 +91,7 @@
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
       <define name="GUIDANCE_INDI_POS_GAINZ" value="0.5"/>
       <define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.8"/>
-      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
+      <define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.20"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
       <define name="GUIDANCE_INDI_MIN_THROTTLE" value="4000"/>

conf/airframes/examples/cube_orange.xml

@@ -86,7 +86,7 @@
     <module name="stabilization" type="indi_simple"/>
     <module name="stabilization" type="rate_indi"/>
     <module name="ins"           type="ekf2"/>
-    
+
     <module name="air_data"/>
 
     <!-- Internal MAG -->
@@ -112,7 +112,7 @@
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
       <define name="GUIDANCE_INDI_POS_GAINZ" value="0.2"/>
       <define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.0"/>
-      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
+      <define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.20"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
       <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>

conf/airframes/tudelft/cyfoam.xml

@@ -1,6 +1,6 @@
 <!--Mini-Cyclone EPP Airframe
   Chimera AP
-	Xbee API 
+	Xbee API
 	Ublox M8T
 	SBUS Futaba -->
 
@@ -296,7 +296,7 @@
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
       <define name="GUIDANCE_INDI_POS_GAINZ" value="0.5"/>
       <define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.8"/>
-      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
+      <define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.20"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
       <define name="GUIDANCE_INDI_MIN_THROTTLE" value="3000"/>

conf/modules/guidance_indi_hybrid.xml

@@ -9,6 +9,9 @@
   <settings>
     <dl_settings>
       <dl_settings NAME="guidance_indi_hybrid">
+        <dl_setting var="gih_params.liftd_p50" min="0.1" step="0.1" max="10.0" shortname="liftd_p50" param="GUIDANCE_INDI_LIFTD_P50" persistent="true"/>
+        <dl_setting var="gih_params.liftd_p80" min="1.0" step="0.1" max="20.0" shortname="liftd_p80" param="GUIDANCE_INDI_LIFTD_P50" persistent="true"/>
+        <dl_setting var="gih_params.liftd_asq" min="0.01" step="0.01" max="1.0" shortname="liftd_asq" param="GUIDANCE_INDI_LIFTD_ASQ" persistent="true"/>
         <dl_setting var="gih_params.pos_gain" min="0" step="0.1" max="10.0" shortname="kp" param="GUIDANCE_INDI_POS_GAIN" persistent="true"/>
         <dl_setting var="gih_params.pos_gainz" min="0" step="0.1" max="10.0" shortname="kp_z" param="GUIDANCE_INDI_POS_GAINZ" persistent="true"/>
         <dl_setting var="gih_params.speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c

@@ -66,6 +66,19 @@
 #define GUIDANCE_INDI_MAX_PITCH 25
 #endif
 
+#ifndef GUIDANCE_INDI_LIFTD_ASQ
+#define GUIDANCE_INDI_LIFTD_ASQ 0.20
+#endif
+
+/* If lift effectiveness at low airspeed not defined,
+ * just make one interpolation segment that connects to
+ * the quadratic part from 12 m/s onward
+ */
+#ifndef GUIDANCE_INDI_LIFTD_P50
+#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12)
+#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
+#endif
+
 struct guidance_indi_hybrid_params gih_params = {
   .pos_gain = GUIDANCE_INDI_POS_GAIN,
   .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
@@ -74,14 +87,17 @@ struct guidance_indi_hybrid_params gih_params = {
   .speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
 
   .heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
+  .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared
+  .liftd_p80 = GUIDANCE_INDI_LIFTD_P80,
+  .liftd_p50 = GUIDANCE_INDI_LIFTD_P50,
 };
 
 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
 #error "You must have an airspeed sensor to use this guidance"
 #endif
 float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
 
-// Tell the guidance that the airspeed needs to be zeroed. 
+// Tell the guidance that the airspeed needs to be zeroed.
 // Recomended to also put GUIDANCE_INDI_NAV_SPEED_MARGIN low in this case.
 #ifndef GUIDANCE_INDI_ZERO_AIRSPEED
 #define GUIDANCE_INDI_ZERO_AIRSPEED FALSE
@@ -137,8 +153,6 @@ float guidance_indi_line_gain = 1.0;
 
 float inv_eff[4];
 
-float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF;
-
 // Max bank angle in radians
 float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK;
 
@@ -165,6 +179,14 @@ float thrust_in;
 
 struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0};
 
+#ifndef GUIDANCE_INDI_VEL_SP_ID
+#define GUIDANCE_INDI_VEL_SP_ID ABI_BROADCAST
+#endif
+abi_event vel_sp_ev;
+static void vel_sp_cb(uint8_t sender_id, struct FloatVect3 *vel_sp);
+struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
+float time_of_vel_sp = 0.0;
+
 void guidance_indi_propagate_filters(void);
 static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
 static float guidance_indi_get_liftd(float pitch, float theta);
@@ -198,6 +220,7 @@ static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_de
 void guidance_indi_init(void)
 {
   /*AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);*/
+  AbiBindMsgVEL_SP(GUIDANCE_INDI_VEL_SP_ID, &vel_sp_ev, vel_sp_cb);
 
   float tau = 1.0/(2.0*M_PI*filter_cutoff);
   float sample_time = 1.0/PERIODIC_FREQUENCY;
@@ -259,9 +282,16 @@ void guidance_indi_run(float *heading_sp) {
   float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
   float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
 
-  if(autopilot.mode == AP_MODE_NAV) {
+  // First check for velocity setpoint from module
+  float dt = get_sys_time_float() - time_of_vel_sp;
+  // If the input command is not updated after a timeout, switch back to flight plan control
+  if (dt < 0.5) {
+    gi_speed_sp.x = indi_vel_sp.x;
+    gi_speed_sp.y = indi_vel_sp.y;
+    gi_speed_sp.z = indi_vel_sp.z;
+  } else if(autopilot.mode == AP_MODE_NAV) {
     gi_speed_sp = nav_get_speed_setpoint(gih_params.pos_gain);
-  } else{
+  } else {
     gi_speed_sp.x = pos_x_err * gih_params.pos_gain;
     gi_speed_sp.y = pos_y_err * gih_params.pos_gain;
     gi_speed_sp.z = pos_z_err * gih_params.pos_gainz;
@@ -352,6 +382,7 @@ void guidance_indi_run(float *heading_sp) {
         speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
       }
     }
+
     gi_speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
     gi_speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
 
@@ -582,14 +613,29 @@ void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
  */
 float guidance_indi_get_liftd(float airspeed, float theta) {
   float liftd = 0.0;
+
   if(airspeed < 12) {
+  /* Assume the airspeed is too low to be measured accurately
+    * Use scheduling based on pitch angle instead.
+    * You can define two interpolation segments
+    */
     float pitch_interp = DegOfRad(theta);
-    Bound(pitch_interp, -80.0, -40.0);
-    float ratio = (pitch_interp + 40.0)/(-40.);
-    liftd = -24.0*ratio*lift_pitch_eff/0.12;
+    const float min_pitch = -80.0;
+    const float middle_pitch = -50.0;
+    const float max_pitch = -20.0;
+
+    Bound(pitch_interp, min_pitch, max_pitch);
+    if (pitch_interp > middle_pitch) {
+      float ratio = (pitch_interp - max_pitch)/(middle_pitch - max_pitch);
+      liftd = -gih_params.liftd_p50*ratio;
+    } else {
+      float ratio = (pitch_interp - middle_pitch)/(min_pitch - middle_pitch);
+      liftd = -(gih_params.liftd_p80-gih_params.liftd_p50)*ratio - gih_params.liftd_p50;
+    }
   } else {
-    liftd = -(airspeed - 8.5)*lift_pitch_eff/M_PI*180.0;
+    liftd = -gih_params.liftd_asq*airspeed*airspeed;
   }
+
   //TODO: bound liftd
   return liftd;
 }
@@ -713,7 +759,7 @@ struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_ga
   struct FloatVect3 speed_sp_return;
   struct NedCoor_f ned_target;
   // Target in NED instead of ENU
-  VECT3_ASSIGN(ned_target, POS_FLOAT_OF_BFP(target.y), POS_FLOAT_OF_BFP(target.x), -POS_FLOAT_OF_BFP(target.z));
+  VECT3_ASSIGN(ned_target, POS_FLOAT_OF_BFP(target.y), POS_FLOAT_OF_BFP(target.x), POS_FLOAT_OF_BFP(-nav_flight_altitude));
 
   // Calculate position error
   struct FloatVect3 pos_error;
@@ -756,3 +802,14 @@ struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_ga
 
   return speed_sp_return;
 }
+
+/**
+ * ABI callback that obtains the velocity setpoint from a module
+  */
+static void vel_sp_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *vel_sp)
+{
+  indi_vel_sp.x = vel_sp->x;
+  indi_vel_sp.y = vel_sp->y;
+  indi_vel_sp.z = vel_sp->z;
+  time_of_vel_sp = get_sys_time_float();
+}

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h

@@ -46,6 +46,9 @@ struct guidance_indi_hybrid_params {
   float speed_gain;
   float speed_gainz;
   float heading_bank_gain;
+  float liftd_asq;
+  float liftd_p80;
+  float liftd_p50;
 };
 
 extern struct FloatVect3 sp_accel;

sw/airborne/modules/core/abi_sender_ids.h

@@ -604,4 +604,12 @@
 #define RADIO_CONTROL_INTERMCU_ID 9
 #endif
 
+/*
+ * IDs of VEL_SP senders
+ */
+
+#ifndef VEL_SP_FCR_ID
+#define VEL_SP_FCR_ID 1 // Approach Moving Target
+#endif
+
 #endif /* ABI_SENDER_IDS_H */