Feed forward guidance (#3068)

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

@@ -351,6 +351,16 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN;
 #endif
 
+  // We can pre-compute the required rates to achieve this turn rate:
+  // NOTE: there *should* not be any problems possible with Euler singularities here
+  struct FloatEulers *euler_zyx = stateGetNedToBodyEulers_f();
+
+  struct FloatRates ff_rates;
+
+  ff_rates.p = -sinf(euler_zyx->theta) * omega;
+  ff_rates.q =  cosf(euler_zyx->theta) * sinf(euler_zyx->phi) * omega;
+  ff_rates.r =  cosf(euler_zyx->theta) * cosf(euler_zyx->phi) * omega;
+
   // For a hybrid it is important to reduce the sideslip, which is done by changing the heading.
   // For experiments, it is possible to fix the heading to a different value.
   if (take_heading_control) {
@@ -399,7 +409,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   float_quat_of_eulers_zxy(&sp_quat, &guidance_euler_cmd);
   float_quat_normalize(&sp_quat);
 
-  return stab_sp_from_quat_f(&sp_quat);
+  return stab_sp_from_quat_ff_rates_f(&sp_quat, &ff_rates);
 }
 
 // compute accel setpoint from speed setpoint (use global variables ! FIXME)

sw/airborne/firmwares/rotorcraft/stabilization.c

@@ -122,7 +122,7 @@ void stabilization_filter_commands(void)
 
 struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
 {
-  if (sp->type == STAB_SP_QUAT) {
+  if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
     if (sp->format == STAB_SP_INT) {
       return sp->sp.quat_i;
     } else {
@@ -164,7 +164,7 @@ struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
 
 struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
 {
-  if (sp->type == STAB_SP_QUAT) {
+  if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
     if (sp->format == STAB_SP_FLOAT) {
       return sp->sp.quat_f;
     } else {
@@ -214,7 +214,7 @@ struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
       EULERS_BFP_OF_REAL(eulers, sp->sp.eulers_f);
       return eulers;
     }
-  } else if (sp->type == STAB_SP_QUAT) {
+  } else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
     if (sp->format == STAB_SP_INT) {
       struct Int32Eulers eulers;
       int32_eulers_of_quat(&eulers, &sp->sp.quat_i);
@@ -253,7 +253,7 @@ struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
       EULERS_FLOAT_OF_BFP(eulers, sp->sp.eulers_i);
       return eulers;
     }
-  } else if (sp->type == STAB_SP_QUAT) {
+  } else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
     if (sp->format == STAB_SP_FLOAT) {
       struct FloatEulers eulers;
       float_eulers_of_quat(&eulers, &sp->sp.quat_f);
@@ -284,7 +284,7 @@ struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
 
 struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
 {
-  if (sp->type == STAB_SP_RATES) {
+  if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
     if (sp->format == STAB_SP_INT) {
       return sp->sp.rates_i;
     } else {
@@ -294,14 +294,14 @@ struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
     }
   } else {
     // error, attitude setpoint
-    struct Int32Rates rates = {0};
+    struct Int32Rates rates = {0, 0, 0};
     return rates;
   }
 }
 
 struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
 {
-  if (sp->type == STAB_SP_RATES) {
+  if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
     if (sp->format == STAB_SP_FLOAT) {
       return sp->sp.rates_f;
     } else {
@@ -311,7 +311,7 @@ struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
     }
   } else {
     // error, attitude setpoint
-    struct FloatRates rates = {0};
+    struct FloatRates rates = {0, 0, 0};
     return rates;
   }
 }
@@ -336,6 +336,17 @@ struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
   return sp;
 }
 
+struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates)
+{
+  struct StabilizationSetpoint sp = {
+    .type = STAB_SP_QUAT_FF_RATE,
+    .format = STAB_SP_FLOAT,
+    .sp.quat_f = *quat,
+    .sp.rates_f = *rates
+  };
+  return sp;
+}
+
 struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
 {
   struct StabilizationSetpoint sp = {

sw/airborne/firmwares/rotorcraft/stabilization.h

@@ -44,7 +44,8 @@ struct StabilizationSetpoint {
     STAB_SP_QUAT,     ///< LTP to Body orientation in unit quaternion
     STAB_SP_EULERS,   ///< LTP to Body orientation in euler angles
     STAB_SP_LTP,      ///< banking and heading in LTP (NED) frame
-    STAB_SP_RATES     ///< body rates
+    STAB_SP_RATES,     ///< body rates
+    STAB_SP_QUAT_FF_RATE  ///< LTP to Body orientation in unit quaternion with precomputed feedforward rates
   } type;
   enum {
     STAB_SP_INT,
@@ -73,6 +74,7 @@ extern struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp);
 // helper make functions
 extern struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat);
 extern struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat);
+extern struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates);
 extern struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers);
 extern struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers);
 extern struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading);

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c

@@ -189,6 +189,7 @@ int32_t num_thrusters;
 
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
+struct Int32Rates  stab_att_ff_rates;
 
 abi_event rpm_ev;
 static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act);
@@ -413,6 +414,7 @@ void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
   stab_att_sp_euler = *rpy;
 
   int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
+  INT_RATES_ZERO(stab_att_ff_rates);
 }
 
 /**
@@ -422,6 +424,7 @@ void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
 {
   stab_att_sp_quat = *quat;
   int32_eulers_of_quat(&stab_att_sp_euler, quat);
+  INT_RATES_ZERO(stab_att_ff_rates);
 }
 
 /**
@@ -445,6 +448,7 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
   stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
 
   quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
+  INT_RATES_ZERO(stab_att_ff_rates);
 }
 
 /**
@@ -456,6 +460,7 @@ void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
 {
   stab_att_sp_euler = stab_sp_to_eulers_i(sp);
   stab_att_sp_quat = stab_sp_to_quat_i(sp);
+  stab_att_ff_rates = stab_sp_to_rates_i(sp);
 }
 
 /**
@@ -686,6 +691,9 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
   rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
   rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
 
+  // Add feed-forward rates to the attitude feedback part
+  RATES_ADD(rate_sp, stab_att_ff_rates);
+
   // Store for telemetry
   angular_rate_ref.p = rate_sp.p;
   angular_rate_ref.q = rate_sp.q;

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c

@@ -92,6 +92,7 @@
 
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
+struct Int32Rates  stab_att_ff_rates;
 
 static struct FirstOrderLowPass rates_filt_fo[3];
 
@@ -304,6 +305,7 @@ void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
 {
   stab_att_sp_euler = stab_sp_to_eulers_i(sp);
   stab_att_sp_quat = stab_sp_to_quat_i(sp);
+  stab_att_ff_rates = stab_sp_to_rates_i(sp);
 }
 
 /**
@@ -485,6 +487,9 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __
   rate_sp.q = indi.gains.att.q * att_fb.y / indi.gains.rate.q;
   rate_sp.r = indi.gains.att.r * att_fb.z / indi.gains.rate.r;
 
+  // Add feed-forward rates to the attitude feedback part
+  RATES_ADD(rate_sp, stab_att_ff_rates);
+
   /* compute the INDI command */
   stabilization_indi_rate_run(rate_sp, in_flight);
 }