[INDI] Harmonize INDI Simple and Full downlink (#3101)

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

@@ -312,7 +312,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   //Calculate roll,pitch and thrust command
   MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
 
-  //printf("abi thrust %f\n", euler_cmd.z);
   AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
 
   // Coordinated turn
@@ -510,7 +509,6 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
   /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
   BoundAbs(accel_sp.z, 3.0);
 
-  //printf("accel_sp %f %f %f\n", accel_sp.x, accel_sp.y, accel_sp.z);
   return accel_sp;
 }
 

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

@@ -145,6 +145,7 @@ float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT-1] = 1.0};
 // variables needed for control
 float actuator_state_filt_vect[INDI_NUM_ACT];
 struct FloatRates angular_accel_ref = {0., 0., 0.};
+struct FloatRates angular_rate_ref = {0., 0., 0.};
 float angular_acceleration[3] = {0., 0., 0.};
 float actuator_state[INDI_NUM_ACT];
 float indi_u[INDI_NUM_ACT];
@@ -233,6 +234,34 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d
                               &(quat->qy),
                               &(quat->qz));
 }
+
+static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
+{
+  struct FloatRates *body_rates = stateGetBodyRates_f();
+  struct Int32Vect3 *body_accel_i = stateGetAccelBody_i();
+  struct FloatVect3 body_accel_f_telem;
+  ACCELS_FLOAT_OF_BFP(body_accel_f_telem, *body_accel_i);
+  float zero = 0;
+  pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
+                                        &body_accel_f_telem.x,    // input lin.acc
+                                        &body_accel_f_telem.y,
+                                        &body_accel_f_telem.z,
+                                        &body_rates->p,           // rate
+                                        &body_rates->q,
+                                        &body_rates->r,
+                                        &angular_rate_ref.p,      // rate.sp
+                                        &angular_rate_ref.q,
+                                        &angular_rate_ref.r,
+                                        &angular_acceleration[0], // ang.acc
+                                        &angular_acceleration[1],
+                                        &angular_acceleration[2],
+                                        &angular_accel_ref.p,     // ang.acc.sp
+                                        &angular_accel_ref.q,
+                                        &angular_accel_ref.r,
+                                        &zero, &zero,             // eff.mat
+                                        &zero, &zero,
+                                        INDI_NUM_ACT, indi_u);    // out
+}
 #endif
 
 /**
@@ -278,6 +307,7 @@ void stabilization_indi_init(void)
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INDI_G, send_indi_g);
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_full_indi);
 #endif
 }
 
@@ -616,6 +646,11 @@ 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;
 
+  // Store for telemetry
+  angular_rate_ref.p = rate_sp.p;
+  angular_rate_ref.q = rate_sp.q;
+  angular_rate_ref.r = rate_sp.r;
+
   // Possibly we can use some bounding here
   /*BoundAbs(rate_sp.r, 5.0);*/
 
@@ -846,14 +881,18 @@ void calc_g1g2_pseudo_inv(void)
   }
 }
 
-static void rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t UNUSED *rpm_msg, uint8_t UNUSED num_act)
+static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act UNUSED)
 {
 #if INDI_RPM_FEEDBACK
   int8_t i;
   for (i = 0; i < num_act; i++) {
-    act_obs[i] = (rpm_msg[i].rpm - get_servo_min(i));
-    act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
-    Bound(act_obs[i], 0, MAX_PPRZ);
+    // Sanity check that index is valid
+    if (rpm_msg[i].actuator_idx < INDI_NUM_ACT) {
+      int8_t idx = rpm_msg[i].actuator_idx;
+      act_obs[idx] = (rpm_msg[i].rpm - get_servo_min(idx));
+      act_obs[idx] *= (MAX_PPRZ / (float)(get_servo_max(idx) - get_servo_min(idx)));
+      Bound(act_obs[idx], 0, MAX_PPRZ);
+    }
   }
 #endif
 }

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

@@ -148,18 +148,25 @@ static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
   struct FloatRates g1_disp;
   RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
   float g2_disp = indi.est.g2 * INDI_EST_SCALE;
+  float zero = 0;
 
   pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
-                                   &indi.rate_d[0],
+                                   &zero, &zero, &zero,         // input lin.acc
+                                   &indi.rate[0].o[0],          // rate
+                                   &indi.rate[1].o[0],
+                                   &indi.rate[2].o[0],
+                                   &zero, &zero, &zero,         // rate.ref
+                                   &indi.rate_d[0],             // ang.acc = rate.diff
                                    &indi.rate_d[1],
                                    &indi.rate_d[2],
-                                   &indi.angular_accel_ref.p,
+                                   &indi.angular_accel_ref.p,   // ang.acc.ref
                                    &indi.angular_accel_ref.q,
                                    &indi.angular_accel_ref.r,
-                                   &g1_disp.p,
+                                   &g1_disp.p,                  // matrix
                                    &g1_disp.q,
                                    &g1_disp.r,
-                                   &g2_disp);
+                                   &g2_disp,
+                                   0, &zero);                   // outputs
 }
 
 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)