UAVCAN telemetry (#3129)

* UAVCAN telemetry * [modules] Actuator feedback rewrite --------- Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
paparazzi · Oct 4, 2023 · 5b25426 · 5b25426
1 parent db4c67d
commit 5b25426
Show file tree

Hide file tree

Showing 33 changed files with 221 additions and 90 deletions.
diff --git a/conf/abi.xml b/conf/abi.xml
@@ -83,8 +83,8 @@
       <field name="eas" type="float" unit="m/s"/>
     </message>
 
-    <message name="RPM" id="15">
-      <field name="rpm" type="struct rpm_act_t *" unit="rpm"/>
+    <message name="ACT_FEEDBACK" id="15">
+      <field name="feedback" type="struct act_feedback_t *"/>
       <field name="num_act" type="uint8_t"/>
     </message>
 

diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
@@ -28,7 +28,7 @@
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="udp"/>
-      <define name="INDI_RPM_FEEDBACK" value="FALSE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="FALSE"/>
       <define name="CLIMB_VSPEED" value="0.8"/>
       <define name="STABILIZATION_INDI_G1_P" value="0.05"/>
       <define name="STABILIZATION_INDI_G1_Q" value="0.025"/>

diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
@@ -24,7 +24,7 @@
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="udp"/>
-      <define name="INDI_RPM_FEEDBACK" value="FALSE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="FALSE"/>
       <define name="CLIMB_VSPEED" value="0.8"/>
       <define name="STABILIZATION_INDI_G1_P" value="0.05"/>
       <define name="STABILIZATION_INDI_G1_Q" value="0.025"/>

diff --git a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
@@ -185,7 +185,7 @@
       <module name="udp"/>
       <!-- INDI RPM feedback not implemented in sim, will it be YOU who adds this? TIA! -->
       <module name="stabilization" type="indi_simple">
-        <define name="INDI_RPM_FEEDBACK" value="FALSE"/>
+        <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="FALSE"/>
       </module>
       <!--<module name="uart"/>--><!-- TODO: Exteral HITL PC debugging e.g test external device triggering while mission flying in sim -->
 
@@ -231,7 +231,7 @@
     <module name="stabilization" type="indi_simple"/>
       <!-- for later if simple works we then need to add RPM feedback... a big effort :(
       <module name="stabilization" type="indi">
-        <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+        <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
       </module>-->
       <!--<module name="guidance" type="indi">
         <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>

diff --git a/conf/airframes/OPENUAS/openuas_eflite_umx_sbach_342.xml b/conf/airframes/OPENUAS/openuas_eflite_umx_sbach_342.xml
@@ -61,6 +61,7 @@
        <module name="pwm_meas.xml"/> <!--  To be able to measure PWM pulses -->
        <module name="rpm_sensor.xml">
          <define name="RPM_PULSE_PER_RND" value="18"/><!-- TODO: determine correct value for this specific setup -->
+         <define name="RPM_SENSOR_ACTUATOR_IDX" value="0"/>
        </module>
        <module name="ins"/>
      </target>

diff --git a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
@@ -145,7 +145,7 @@
     <module name="stabilization" type="indi_simple"/>
     <!-- for later if simple works
     <module name="stabilization" type="indi">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </module>-->
     <!--<module name="guidance" type="indi"/>--><!-- for later if classic works-->
 

diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
@@ -65,7 +65,7 @@
       <module name="stabilization" type="rate_indi"/>
       <!--<module name="stabilization" type="indi_simple"/>--><!-- not used ATM -->
       <module name="stabilization" type="indi">
-        <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+        <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
         <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
         <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
         <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>

diff --git a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
@@ -134,7 +134,7 @@
       <module name="udp"/>
       <!-- INDI RPM feedback not implemented in sim, will it be YOU who adds this? TIA! -->
       <module name="stabilization" type="indi_simple">
-        <define name="INDI_RPM_FEEDBACK" value="FALSE"/>
+        <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="FALSE"/>
       </module>
       <!--<module name="uart"/>--><!-- TODO: Exteral HITL PC debugging e.g test external device triggering while mission flying in sim -->
 
@@ -182,7 +182,7 @@
     <module name="stabilization" type="indi_simple"/>
     <!-- for later if simple works we then need to add RPM feedback... a big effort :(
     <module name="stabilization" type="indi">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </module>-->
     <!--<module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>

diff --git a/conf/airframes/tudelft/bebop2_indi.xml b/conf/airframes/tudelft/bebop2_indi.xml
@@ -19,7 +19,7 @@
     <module name="imu" type="bebop"/>
     <module name="gps" type="ublox"/>
     <module name="stabilization" type="indi">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </module>
     <module name="ahrs" type="int_cmpl_quat">
       <configure name="USE_MAGNETOMETER" value="TRUE"/>

diff --git a/conf/airframes/tudelft/bebop2_no_damping.xml b/conf/airframes/tudelft/bebop2_no_damping.xml
@@ -6,7 +6,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="bebop2">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </target>
 
     <target name="nps" board="pc">

diff --git a/conf/airframes/tudelft/bebop2_no_damping_WLS.xml b/conf/airframes/tudelft/bebop2_no_damping_WLS.xml
@@ -6,7 +6,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="bebop2">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </target>
 
     <target name="nps" board="pc">

diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml
@@ -6,13 +6,13 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="bebop">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </target>
 
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="udp"/>
-      <define name="INDI_RPM_FEEDBACK" value="FALSE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="FALSE"/>
     </target>
 
     <!--define name="USE_SONAR" value="TRUE"/-->

diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor.xml b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
@@ -18,7 +18,7 @@
     <module name="actuators" type="bebop"/>
     <module name="imu" type="bebop"/>
     <module name="stabilization" type="indi">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </module>
     <module name="ahrs" type="int_cmpl_quat">
       <configure name="USE_MAGNETOMETER" value="FALSE"/>

diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
@@ -26,7 +26,7 @@ pyramid level 2: 21 fps average, min=11fps
     <module name="actuators" type="bebop"/>
     <module name="imu" type="bebop"/>
     <module name="stabilization" type="indi">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </module>
     <module name="ahrs" type="int_cmpl_quat">
       <configure name="USE_MAGNETOMETER" value="FALSE"/>

diff --git a/conf/airframes/tudelft/bebop_optitrack.xml b/conf/airframes/tudelft/bebop_optitrack.xml
@@ -20,7 +20,7 @@
     <module name="stabilization" type="indi">
       <define name="WLS_N_U" value="4"/>
       <define name="WLS_N_V" value="4"/>
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
     </module>
     <module name="ahrs" type="int_cmpl_quat">
       <!-- Use the optitrack heading instead of magnetometer -->

diff --git a/conf/airframes/tudelft/fan_demo.xml b/conf/airframes/tudelft/fan_demo.xml
@@ -19,7 +19,7 @@
     <module name="imu" type="bebop"/>
     <module name="gps" type="datalink"/>
     <module name="stabilization" type="indi">
-      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+      <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
       <define name="WLS_N_U" value="4"/>
       <define name="WLS_N_V" value="4"/>
     </module>

diff --git a/conf/airframes/tudelft/outback.xml b/conf/airframes/tudelft/outback.xml
@@ -38,7 +38,9 @@
       <module name="opa_controller"/>
       <module name="mag_pitot_uart" />
       <module name="pwm_meas"/>
-      <module name="rpm_sensor"/>
+      <module name="rpm_sensor">
+        <define name="RPM_SENSOR_ACTUATOR_IDX" value="0"/>
+      </module>
 
       <module name="geo_mag"/>
       <module name="air_data">

diff --git a/conf/modules/actuators_dshot.xml b/conf/modules/actuators_dshot.xml
@@ -22,6 +22,7 @@
     <file_arch name="actuators_dshot_arch.c"/>
     <file_arch name="esc_dshot.c" cond="ifeq ($(RTOS),chibios)"/>
     <file_arch name="hal_stm32_dma.c" dir="mcu_periph" cond="ifeq ($(RTOS),chibios)"/>
+    <test/>
   </makefile>
 </module>
 
diff --git a/conf/modules/heli_throttle_curve.xml b/conf/modules/heli_throttle_curve.xml
@@ -6,8 +6,8 @@
     <define name="THROTTLE_CURVE_RPM_FB_P" value="0" description="The RPM controller feeadback P gain of the PI controller"/>
     <define name="THROTTLE_CURVE_RPM_FB_I" value="0" description="The RPM controller feeadback I gain of the PI controller"/>
     <define name="THROTTLE_CURVE_RPM_INC_LIMIT" value="512" description="Amount of RPM the controller can increase or decrease per second"/>
-    <define name="THROTTLE_CURVE_RPM_ID" value="ABI_BROADCAST" description="The ABI sender ID to listen for RPM messages"/>
-    <define name="THROTTLE_CURVE_RPM_ACT" value="0" description="The ABI RPM message motor index"/>
+    <define name="THROTTLE_CURVE_ACT_FEEDBACK_ID" value="ABI_BROADCAST" description="The ABI sender ID to listen for RPM messages"/>
+    <define name="THROTTLE_CURVE_RPM_ACT" value="0" description="The RPM message motor index (generated from airframe.h)"/>
   </doc>
   <settings>
     <dl_settings>
@@ -25,6 +25,15 @@
   <makefile target="ap">
     <file name="throttle_curve.c"/>
     <define name="USE_THROTTLE_CURVES" value="TRUE"/>
+    <test firmware="rotorcraft">
+      <define name="THROTTLE_CURVE_RPM_ACT" value="0"/>
+      <define name="THROTTLE_POINTS_NB" value="3"/>
+      <define name="THROTTLE_CURVES_NB" value="2"/>
+      <define name="THROTTLE_CURVES" value="{{0}}"/>
+      <define name="THROTTLE_CURVE_MODE_INIT" value="0"/>
+      <define name="RADIO_FMODE" value="0"/>
+      <define name="COMMAND_COLLECTIVE" value="0"/>
+    </test>
   </makefile>
 </module>
 
diff --git a/conf/modules/ins_flow.xml b/conf/modules/ins_flow.xml
@@ -38,10 +38,14 @@
     <!-- <file name="ahrs.c" dir="subsystems"/> -->
     <file name="ahrs_aligner.c" dir="modules/ahrs"/>
     <!-- <file name="pprz_algebra_float.c" dir="math"/> -->
-	<raw>
-	$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_flow.h\"
-	$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq
-	</raw>
+    <raw>
+      $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_flow.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq
+    </raw>
+    <test firmware="rotorcraft">
+      <define name="MOTOR_MIXING_NB_MOTOR" value="4"/>
+      <define name="USE_MAGNETOMETER" value="true"/>
+    </test>
   </makefile>
 
 </module>
diff --git a/conf/modules/rpm_sensor.xml b/conf/modules/rpm_sensor.xml
@@ -8,6 +8,7 @@
     <configure name="RPM_PWM_CHANNEL" value="PWM_INPUTX" description="Select PWM input channel for RPM sensor"/>
     <define name="RPM_PULSE_PER_RND" value="14" description="Amount of pulses per round"/>
     <define name="RPM_FILTER_TAU" value="0.3" description="1/cut-off-frequency = filter time"/>
+    <define name="RPM_SENSOR_ACTUATOR_IDX" value="" description="the actuator which we are sensing the RPM for"/>
   </doc>
   <dep>
     <depends>pwm_meas</depends>

diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml
@@ -47,6 +47,8 @@
       <define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
       <define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}" description="WLS control objective priorities: roll, pitch, thrust, yaw, thrust_x"/>
       <define name="WLS_WU" value="{1, 1, 1, 1}" description="WLS actuator cost (size of INDI_NUM_ACT)"/>
+      <define name="RPM_FEEDBACK" value="FALSE" description="enable RPM feedback"/>
+      <define name="ACT_FEEDBACK_ID" value="ABI_BROADCAST" description="listening for RPM feedback on this ABI id"/>
     </section>
   </doc>
   <settings>

diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
@@ -328,14 +328,14 @@ void actuators_dshot_arch_commit(void)
 #endif
 
 
-  struct rpm_act_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 };
+  struct rpm_act_t feedback[ACTUATORS_DSHOT_NB] = { 0 };
   for (uint8_t i = 0; i < ACTUATORS_DSHOT_NB; i++) {
-    rpm_list[i].actuator_idx = ACTUATORS_DSHOT_OFFSET + i;
-    rpm_list[i].rpm = 0;
+    feedback[i].idx = ACTUATORS_DSHOT_OFFSET + i;
     if (actuators_dshot_values[i].activated) {
       const DshotTelemetry *dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
-      rpm_list[i].rpm = dtelem->rpm;
+      feedback[i].rpm = dtelem->rpm;
+      feedback[i].set.rpm = true;
     }
   }
-  AbiSendMsgRPM(RPM_DSHOT_ID, rpm_list, ACTUATORS_DSHOT_NB);
+  AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_DSHOT_ID, feedback, ACTUATORS_DSHOT_NB);
 }
diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c
@@ -137,17 +137,19 @@ void actuators_bebop_commit(void)
 
     actuators_bebop.led = led_hw_values & 0x3;
   }
+
   // Send ABI message
-  struct rpm_act_t rpm_message[4];
+  struct act_feedback_t feedback[4];
   for (int i=0;i<4;i++) {
 #ifdef SERVOS_BEBOP_OFFSET
-    rpm_message[i].actuator_idx = SERVOS_BEBOP_OFFSET + i;
+    feedback[i].idx = SERVOS_BEBOP_OFFSET + i;
 #else
-    rpm_message[i].actuator_idx = SERVOS_DEFAULT_OFFSET + i;
+    feedback[i].idx = SERVOS_DEFAULT_OFFSET + i;
 #endif
-    rpm_message[i].rpm = actuators_bebop.rpm_obs[i];
+    feedback[i].rpm = actuators_bebop.rpm_obs[i];
+    feedback[i].set.rpm = true;
   }
-  AbiSendMsgRPM(RPM_SENSOR_ID, rpm_message, 4);
+  AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, feedback, 4);
 }
 
 static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size)

diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c
@@ -169,14 +169,15 @@ void actuators_disco_commit(void)
   }
 
   // Send ABI message
-  struct rpm_act_t rpm_message = {0};
+  struct act_feedback_t feedback = {0};
 #ifdef SERVOS_DISCO_OFFSET
-  rpm_message.actuator_idx = SERVOS_DISCO_OFFSET;
+  feedback.idx = SERVOS_DISCO_OFFSET;
 #else
-  rpm_message.actuator_idx = SERVOS_DEFAULT_OFFSET;
+  feedback.idx = SERVOS_DEFAULT_OFFSET;
 #endif
-  rpm_message.rpm = actuators_disco.rpm_obs;
-  AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1);//FIXME & or not
+  feedback.rpm = actuators_disco.rpm_obs;
+  feedback.set.rpm = true;
+  AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, &feedback, 1);
 }
 
 static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size)

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -223,8 +223,17 @@ struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
 struct FloatRates  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);
+// Register actuator feedback if we rely on RPM information
+#if STABILIZATION_INDI_RPM_FEEDBACK
+#ifndef STABILIZATION_INDI_ACT_FEEDBACK_ID
+#define STABILIZATION_INDI_ACT_FEEDBACK_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(STABILIZATION_INDI_ACT_FEEDBACK_ID)
+
+abi_event act_feedback_ev;
+static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
+PRINT_CONFIG_MSG("STABILIZATION_INDI_RPM_FEEDBACK")
+#endif
 
 abi_event thrust_ev;
 static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment);
@@ -327,7 +336,9 @@ void stabilization_indi_init(void)
   // Initialize filters
   init_filters();
 
-  AbiBindMsgRPM(RPM_SENSOR_ID, &rpm_ev, rpm_cb);
+#if STABILIZATION_INDI_RPM_FEEDBACK 
+  AbiBindMsgACT_FEEDBACK(STABILIZATION_INDI_ACT_FEEDBACK_ID, &act_feedback_ev, act_feedback_cb);
+#endif
   AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
 
   float_vect_zero(actuator_state_filt_vectd, INDI_NUM_ACT);
@@ -806,7 +817,7 @@ void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinat
  */
 void get_actuator_state(void)
 {
-#if INDI_RPM_FEEDBACK
+#if STABILIZATION_INDI_RPM_FEEDBACK
   float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
 #else
   //actuator dynamics
@@ -1008,22 +1019,21 @@ void calc_g1g2_pseudo_inv(void)
 }
 #endif
 
-static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act UNUSED)
+#if STABILIZATION_INDI_RPM_FEEDBACK
+static void act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
 {
-#if INDI_RPM_FEEDBACK
-  PRINT_CONFIG_MSG("INDI_RPM_FEEDBACK");
   int8_t i;
   for (i = 0; i < num_act; i++) {
     // 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));
+    if (feedback[i].idx < INDI_NUM_ACT && feedback[i].set.rpm) {
+      int8_t idx = feedback[i].idx;
+      act_obs[idx] = (feedback[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
 }
+#endif
 
 /**
  * ABI callback that obtains the thrust increment from guidance INDI