[indi] ABI thrust message to 3D vector (#3116)

* [indi] ABI thrust message to 3D vector Common WLS for innerloop and outerloop * fix both guidance * cleanup * cleanup
paparazzi · Sep 28, 2023 · ae1f8ed · ae1f8ed
1 parent b9bcd65
commit ae1f8ed
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 9 deletions.
diff --git a/conf/abi.xml b/conf/abi.xml
@@ -89,7 +89,7 @@
     </message>
 
     <message name="THRUST" id="16">
-      <field name="thrust_increment" type="float" unit="m/s^2"/>
+      <field name="thrust_increment_vect" type="struct FloatVect3" unit="m/s^2"/>
     </message>
 
     <message name="OBSTACLE_DETECTION" id="17">

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -256,7 +256,11 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   //Calculate roll,pitch and thrust command
   MAT33_VECT3_MUL(control_increment, Ga_inv, a_diff);
 
-  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, control_increment.z);
+  struct FloatVect3 thrust_vect;
+  thrust_vect.x = 0.0;  // Fill for quadplanes
+  thrust_vect.y = 0.0;
+  thrust_vect.z = control_increment.z;
+  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
 
   guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
   guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -312,7 +312,11 @@ 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);
 
-  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
+  struct FloatVect3 thrust_vect;
+  thrust_vect.x = 0.0;  // Fill for quadplanes
+  thrust_vect.y = 0.0;
+  thrust_vect.z = euler_cmd.z;
+  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
 
   // Coordinated turn
   // feedforward estimate angular rotation omega = g*tan(phi)/v

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -195,8 +195,8 @@ abi_event rpm_ev;
 static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act);
 
 abi_event thrust_ev;
-static void thrust_cb(uint8_t sender_id, float thrust_increment);
-float indi_thrust_increment;
+static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment);
+struct FloatVect3 indi_thrust_increment;
 bool indi_thrust_increment_set = false;
 
 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
@@ -540,7 +540,10 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
     use_increment = 1.0;
   }
 
-  float v_thrust = 0.0;
+  struct FloatVect3 v_thrust;
+  v_thrust.x = 0.0;
+  v_thrust.y = 0.0;
+  v_thrust.z = 0.0;
   if (indi_thrust_increment_set) {
     v_thrust = indi_thrust_increment;
 
@@ -554,7 +557,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   } else {
     // incremental thrust
     for (i = 0; i < INDI_NUM_ACT; i++) {
-      v_thrust +=
+      v_thrust.z +=
         (stabilization_cmd[COMMAND_THRUST] - use_increment*actuator_state_filt_vect[i]) * Bwls[3][i];
     }
   }
@@ -563,7 +566,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   indi_v[0] = (angular_accel_ref.p - use_increment*angular_acceleration[0]);
   indi_v[1] = (angular_accel_ref.q - use_increment*angular_acceleration[1]);
   indi_v[2] = (angular_accel_ref.r - use_increment*angular_acceleration[2] + g2_times_du);
-  indi_v[3] = v_thrust;
+  indi_v[3] = v_thrust.z;
 
 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
   // Calculate the increment for each actuator
@@ -951,7 +954,7 @@ PRINT_CONFIG_MSG("INDI_RPM_FEEDBACK");
 /**
  * ABI callback that obtains the thrust increment from guidance INDI
  */
-static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
+static void thrust_cb(uint8_t UNUSED sender_id, struct FloatVect3 thrust_increment)
 {
   indi_thrust_increment = thrust_increment;
   indi_thrust_increment_set = true;