[pfc] Actuator motion pre-flight-check (#3202)

* PFC for actuator deflections * [checks] Preflight check actuators [checks] PFC actuators merge-error * Fixes for other airframes * [pfc] Fix better debug information for actuator checks * [modules] Fix warnings in test build * Update conf/modules/imu_mpu9250_i2c.xml * Update conf/modules/imu_mpu9250_spi.xml * [uavcan] Fix possible misconfiguration and overflow --------- Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
2026-05-09 22:49:53 +08:00 · 2023-12-18 15:47:40 +01:00
parent 751edc5170
commit 5fd7d56630
12 changed files with 515 additions and 70 deletions
@@ -125,6 +125,7 @@
        <module name="ground_detect_sensor"/>
        <module name="rotwing_state"/>
        <module name="preflight_checks"/>
+        <module name="pfc_actuators"/>
        <module name="agl_dist"/>
        <module name="approach_moving_target"/>

@@ -236,25 +237,26 @@
        <let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/>

        <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[2])" SERVO="MOTOR_BACK"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[3])" SERVO="MOTOR_LEFT"/>
-        <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[4])" SERVO="SERVO_RUDDER"/>
-        <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
-        <set VALUE="($th_hold? -9600 : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
-        <set VALUE="rotwing_state_skewing.servo_pprz_cmd"  SERVO="ROTATION_MECH"/>
-        <set VALUE="$ail_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
-        <set VALUE="$ail_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
-        <set VALUE="$flap_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
-        <set VALUE="$flap_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_RIGHT"/>
+        <call fun="pfc_actuators_run()"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(7, -9600)) : actuators_pprz[0])" SERVO="MOTOR_FRONT"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="MOTOR_BACK"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="MOTOR_LEFT"/>
+        <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
+        <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
+        <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
+        <set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)"  SERVO="ROTATION_MECH"/>
+        <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
+        <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
+        <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
+        <set VALUE="$flap_limit_hit? pfc_actuators_value(4, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_RIGHT"/>

        <!-- Backup commands -->
-        <set VALUE="($hover_off? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
-        <set VALUE="($hover_off? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
-        <set VALUE="rotwing_state_skewing.servo_pprz_cmd"    SERVO="BROTATION_MECH"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(7, -9600)) : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
+        <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
+        <set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)"  SERVO="BROTATION_MECH"/>
    </command_laws>

    <section PREFIX="SYS_ID_" NAME="SYS_ID">
@@ -303,6 +305,9 @@
        <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
        <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

+        <!-- Preflight check actuators (ELE, RUD, AIL_L, FLAP_L, FLAP_R, AIL_R, ROT_M, M_FRONT, M_RIGHT_, M_BACK, M_LEFT, M_PUSH) -->
+        <define name="PFC_ACTUATORS" value="{{.feedback_id=SERVO_SERVO_ELEVATOR_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=0.85, .high_feedback=0.3, .timeout=1},{.feedback_id=SERVO_SERVO_RUDDER_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_ROTATION_MECH_IDX, .feedback_id2=SERVO_BROTATION_MECH_IDX, .low=-9600, .high=9600, .low_feedback=1.57, .high_feedback=0, .timeout=5},{.feedback_id=SERVO_MOTOR_FRONT_IDX, .feedback_id2=SERVO_BMOTOR_FRONT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_RIGHT_IDX, .feedback_id2=SERVO_BMOTOR_RIGHT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_BACK_IDX, .feedback_id2=SERVO_BMOTOR_BACK_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_LEFT_IDX, .feedback_id2=SERVO_BMOTOR_LEFT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_PUSH_IDX, .feedback_id2=255, .low=-9600, .high=2000, .low_feedback=0, .high_feedback=2200, .timeout=3}}"/>
+
        <!-- Others -->
        <define name="NO_RC_THRUST_LIMIT"                   value="TRUE"/>
        <define name="NAV_CLIMB_VSPEED"                     value="2.0" />
@@ -23,7 +23,7 @@
      <configure name="SRC_BOARD" value="boards/bebop"/>
      <define name="BEBOP_ACTUATORS_I2C_DEV" value="i2c1"/>
      <define name="USE_I2C1"/>
-      <define name="SERVOS_DEFAULT_OFFSET" value="0"/>
+      <define name="get_servo_idx(X)" value="X"/>
    </test>
  </makefile>
 </module>
@@ -21,7 +21,7 @@
      <define name="USE_I2C1"/>
      <include name="arch/linux"/>
      <configure name="SRC_BOARD" value="boards/disco"/>
-      <define name="SERVOS_DEFAULT_OFFSET" value="0"/>
+      <define name="SERVO_MOTOR_IDX" value="0"/>
    </test>
  </makefile>
 </module>
@@ -0,0 +1,36 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="pfc_actuators" dir="checks">
+  <doc>
+    <description>
+      Preform a pre flight check of the actuators and validate by looking at the feedback.
+    </description>
+    <define name="PFC_ACTUATORS" value="{}" description="Struct containing the setup of the preflight check"/>
+    <define name="PFC_ACTUATORS_MAX_ANGLE_ERROR" value="0.1" description="Maximum allowed angle error in radians +/-"/>
+    <define name="PFC_ACTUATORS_MAX_RPM_ERROR" value="250" description="Maximum allowed RPM error +/-"/>
+    <define name="PFC_ACTUATORS_DEBUG" value="false" description="Enable debug output in the GCS"/>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="Checks">
+        <dl_setting var="act_start" min="0" step="1" max="1" values="OFF|START" handler="start" module="checks/pfc_actuators" type="fun"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <dep>
+    <depends>@datalink,preflight_checks</depends>
+  </dep>
+  <header>
+    <file name="pfc_actuators.h"/>
+  </header>
+  <init fun="pfc_actuators_init()"/>
+  <makefile>
+    <file name="pfc_actuators.c"/>
+    <test>
+      <define name="PFC_ACTUATORS" value="{{}}"/>
+      <define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
+      <define name="DOWNLINK_DEVICE" value="uart0"/>
+      <define name="USE_UART0"/>
+    </test>
+  </makefile>
+</module>
@@ -18,6 +18,9 @@
      </dl_settings>
    </dl_settings>
  </settings>
+  <dep>
+    <depends>@datalink</depends>
+  </dep>
  <header>
    <file name="preflight_checks.h"/>
  </header>
@@ -33,7 +33,7 @@
    <file name="sensors_hitl.c"/>
    <test firmware="rotorcraft">
      <include name="../../conf/simulator/nps"/>
-      <define name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
+      <define name="PERIODIC_FREQUENCY" value="500"/>
      <define name="HITL_DEVICE" value="usb_serial"/>
      <define name="USE_USB_SERIAL"/>
    </test>
@@ -141,11 +141,7 @@ void actuators_bebop_commit(void)
  // Send ABI message
  struct act_feedback_t feedback[4];
  for (int i=0;i<4;i++) {
-#ifdef SERVOS_BEBOP_OFFSET
-    feedback[i].idx = SERVOS_BEBOP_OFFSET + i;
-#else
-    feedback[i].idx = SERVOS_DEFAULT_OFFSET + i;
-#endif
+    feedback[i].idx = get_servo_idx(i);
    feedback[i].rpm = actuators_bebop.rpm_obs[i];
    feedback[i].set.rpm = true;
  }
@@ -40,6 +40,11 @@

 #include <stdio.h>

+
+#ifndef SERVO_MOTOR_IDX
+#warning "Disco actuators require a <servo name=MOTOR>"
+#endif
+
 /**
 * private observation structure
 */
@@ -170,11 +175,7 @@ void actuators_disco_commit(void)

  // Send ABI message
  struct act_feedback_t feedback = {0};
-#ifdef SERVOS_DISCO_OFFSET
-  feedback.idx = SERVOS_DISCO_OFFSET;
-#else
-  feedback.idx = SERVOS_DEFAULT_OFFSET;
-#endif
+  feedback.idx = SERVO_MOTOR_IDX;
  feedback.rpm = actuators_disco.rpm_obs;
  feedback.set.rpm = true;
  AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, &feedback, 1);
@@ -49,13 +49,12 @@ struct actuators_uavcan_telem_t {
  float position;
 };

+/* The transmitted actuator values */
 #ifdef SERVOS_UAVCAN1_NB
 int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB];
-static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB] = {0};
 #endif
 #ifdef SERVOS_UAVCAN2_NB
 int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB];
-static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0};
 #endif
 #ifdef SERVOS_UAVCAN1CMD_NB
 int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB];
@@ -64,6 +63,23 @@ int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB];
 int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB];
 #endif

+/* Set the actual telemetry length (ID's from actuators can't collide with the command version) */
+#if SERVOS_UAVCAN1CMD_NB > SERVOS_UAVCAN1_NB
+#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1CMD_NB
+static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1CMD_NB] = {0};
+#elif defined(SERVOS_UAVCAN1_NB)
+#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1_NB
+static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB] = {0};
+#endif
+
+#if SERVOS_UAVCAN2CMD_NB > SERVOS_UAVCAN2_NB
+#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2CMD_NB
+static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2CMD_NB] = {0};
+#elif defined(SERVOS_UAVCAN2_NB)
+#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2_NB
+static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0};
+#endif
+
 /* UNUSED value for CMD */
 #define UAVCAN_CMD_UNUSED (MIN_PPRZ-1)

@@ -112,8 +128,8 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) {

  // Find the next set telemetry
  uint8_t offset = 0;
-#ifdef SERVOS_UAVCAN1_NB
-  for(uint8_t i = esc_idx - offset; i < SERVOS_UAVCAN1_NB; i++) {
+#ifdef UAVCAN1_TELEM_NB
+  for(uint8_t i = esc_idx - offset; i < UAVCAN1_TELEM_NB; i++) {
    if(uavcan1_telem[i].set) {
      old_idx = i + offset;
      esc_idx = i + offset + add_idx;
@@ -122,10 +138,10 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) {
      esc_idx = i + offset + 1;
    }
  }
-  offset += SERVOS_UAVCAN1_NB;
+  offset += UAVCAN1_TELEM_NB;
 #endif
-#ifdef SERVOS_UAVCAN2_NB
-  for(uint8_t i = esc_idx - offset; i < SERVOS_UAVCAN2_NB; i++) {
+#ifdef UAVCAN2_TELEM_NB
+  for(uint8_t i = esc_idx - offset; i < UAVCAN2_TELEM_NB; i++) {
    if(uavcan2_telem[i].set) {
      old_idx = i + offset;
      esc_idx = i + offset + add_idx;
@@ -134,7 +150,7 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) {
      esc_idx = i + offset + 1;
    }
  }
-  offset += SERVOS_UAVCAN2_NB;
+  offset += UAVCAN2_TELEM_NB;
 #endif

  // Going round or no telemetry found
@@ -173,16 +189,16 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR

  struct actuators_uavcan_telem_t *telem = NULL;
  uint8_t max_id = 0;
-#ifdef SERVOS_UAVCAN1_NB
+#ifdef UAVCAN1_TELEM_NB
  if (iface == &uavcan1) {
    telem = uavcan1_telem;
-    max_id = SERVOS_UAVCAN1_NB;
+    max_id = UAVCAN1_TELEM_NB;
  }
 #endif
-#ifdef SERVOS_UAVCAN2_NB
+#ifdef UAVCAN2_TELEM_NB
  if (iface == &uavcan2) {
    telem = uavcan2_telem;
-    max_id = SERVOS_UAVCAN2_NB;
+    max_id = UAVCAN2_TELEM_NB;
  }
 #endif

@@ -206,37 +222,53 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
 #if UAVCAN_ACTUATORS_USE_CURRENT
  // Update total current
  electrical.current = 0;
-#ifdef SERVOS_UAVCAN1_NB
-  for (uint8_t i = 0; i < SERVOS_UAVCAN1_NB; ++i) {
+#ifdef UAVCAN1_TELEM_NB
+  for (uint8_t i = 0; i < UAVCAN1_TELEM_NB; ++i) {
    electrical.current += uavcan1_telem[i].current;
  }
 #endif
-#ifdef SERVOS_UAVCAN2_NB
-  for (uint8_t i = 0; i < SERVOS_UAVCAN2_NB; ++i) {
+#ifdef UAVCAN2_TELEM_NB
+  for (uint8_t i = 0; i < UAVCAN2_TELEM_NB; ++i) {
    electrical.current += uavcan2_telem[i].current;
  }
 #endif
 #endif

  // Feedback ABI RPM messages
-#ifdef SERVOS_UAVCAN1_NB
+#ifdef UAVCAN1_TELEM_NB
  if (iface == &uavcan1) {
-    struct act_feedback_t feedback;
-    feedback.idx =  SERVOS_UAVCAN1_OFFSET + esc_idx;
+    struct act_feedback_t feedback = {0};
    feedback.rpm = telem[esc_idx].rpm;
    feedback.set.rpm = true;

+#ifdef SERVOS_UAVCAN1_NB
+    feedback.idx = get_servo_idx_UAVCAN1(esc_idx);
+#endif
+#ifdef SERVOS_UAVCAN1CMD_NB
+    if(esc_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) {
+      feedback.idx = get_servo_idx_UAVCAN1CMD(esc_idx);
+    }
+#endif
+
    // Send ABI message
    AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
  }
 #endif
-#ifdef SERVOS_UAVCAN2_NB
+#ifdef UAVCAN2_TELEM_NB
  if (iface == &uavcan2) {
-    struct act_feedback_t feedback;
-    feedback.idx =  SERVOS_UAVCAN2_OFFSET + esc_idx;
+    struct act_feedback_t feedback = {0};
    feedback.rpm = telem[esc_idx].rpm;
    feedback.set.rpm = true;

+#ifdef SERVOS_UAVCAN2_NB
+    feedback.idx = get_servo_idx_UAVCAN2(esc_idx);
+#endif
+#ifdef SERVOS_UAVCAN2CMD_NB
+    if(esc_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) {
+      feedback.idx = get_servo_idx_UAVCAN2CMD(esc_idx);
+    }
+#endif
+
    // Send ABI message
    AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
  }
@@ -253,16 +285,16 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca

  struct actuators_uavcan_telem_t *telem = NULL;
  uint8_t max_id = 0;
-#ifdef SERVOS_UAVCAN1_NB
+#ifdef UAVCAN1_TELEM_NB
  if (iface == &uavcan1) {
    telem = uavcan1_telem;
-    max_id = SERVOS_UAVCAN1_NB;
+    max_id = UAVCAN1_TELEM_NB;
  }
 #endif
-#ifdef SERVOS_UAVCAN2_NB
+#ifdef UAVCAN2_TELEM_NB
  if (iface == &uavcan2) {
    telem = uavcan2_telem;
-    max_id = SERVOS_UAVCAN2_NB;
+    max_id = UAVCAN2_TELEM_NB;
  }
 #endif

@@ -276,25 +308,41 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca
  canardDecodeScalar(transfer, 8, 16, true, (void *)&tmp_float);
  telem[actuator_idx].position = canardConvertFloat16ToNativeFloat(tmp_float);

-#ifdef SERVOS_UAVCAN1_NB
+#ifdef UAVCAN1_TELEM_NB
  if (iface == &uavcan1) {
-    struct act_feedback_t feedback;
-    feedback.idx =  SERVOS_UAVCAN1_OFFSET + actuator_idx;
+    struct act_feedback_t feedback = {0};
    feedback.position = telem[actuator_idx].position;
    feedback.set.position = true;

+#ifdef SERVOS_UAVCAN1_NB
+    feedback.idx = get_servo_idx_UAVCAN1(actuator_idx);
+#endif
+#ifdef SERVOS_UAVCAN1CMD_NB
+    if(actuator_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) {
+      feedback.idx = get_servo_idx_UAVCAN1CMD(actuator_idx);
+    }
+#endif
+
    // Send ABI message
    AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
  }
 #endif

-#ifdef SERVOS_UAVCAN2_NB
+#ifdef UAVCAN2_TELEM_NB
  if (iface == &uavcan2) {
-    struct act_feedback_t feedback;
-    feedback.idx =  SERVOS_UAVCAN2_OFFSET + actuator_idx;
+    struct act_feedback_t feedback = {0};
    feedback.position = telem[actuator_idx].position;
    feedback.set.position = true;

+#ifdef SERVOS_UAVCAN2_NB
+    feedback.idx = get_servo_idx_UAVCAN2(actuator_idx);
+#endif
+#ifdef SERVOS_UAVCAN2CMD_NB
+    if(actuator_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) {
+      feedback.idx = get_servo_idx_UAVCAN2CMD(actuator_idx);
+    }
+#endif
+
    // Send ABI message
    AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
  }
@@ -311,16 +359,16 @@ static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface,

  struct actuators_uavcan_telem_t *telem = NULL;
  uint8_t max_id = 0;
-#ifdef SERVOS_UAVCAN1_NB
+#ifdef UAVCAN1_TELEM_NB
  if (iface == &uavcan1) {
    telem = uavcan1_telem;
-    max_id = SERVOS_UAVCAN1_NB;
+    max_id = UAVCAN1_TELEM_NB;
  }
 #endif
-#ifdef SERVOS_UAVCAN2_NB
+#ifdef UAVCAN2_TELEM_NB
  if (iface == &uavcan2) {
    telem = uavcan2_telem;
-    max_id = SERVOS_UAVCAN2_NB;
+    max_id = UAVCAN2_TELEM_NB;
  }
 #endif

@@ -0,0 +1,312 @@
+/*
+ * Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/checks/pfc_actuators.c"
+ * @author Freek van Tienen <freek.v.tienen@gmail.com>
+ * Checks the actuators with feedback before takeoff
+ */
+
+#include "preflight_checks.h"
+#include "core/abi.h"
+#include "modules/datalink/telemetry.h"
+#include <stdio.h>
+
+/**
+ * Maximum error for the angle of the actuators (rad)
+*/
+#ifndef PFC_ACTUATORS_MAX_ANGLE_ERROR
+#define PFC_ACTUATORS_MAX_ANGLE_ERROR 0.1f
+#endif
+
+/**
+ * Maximum error for the RPM of the actuators
+*/
+#ifndef PFC_ACTUATORS_MAX_RPM_ERROR
+#define PFC_ACTUATORS_MAX_RPM_ERROR 250.0f
+#endif
+
+/**
+ * Enable debugging to set the expected feedback values
+*/
+#ifndef PFC_ACTUATORS_DEBUG
+#define PFC_ACTUATORS_DEBUG false
+#endif
+
+/**
+ * @brief The status of the preflight checks
+*/
+enum pfc_actuators_state_t {
+  PFC_ACTUATORS_STATE_INIT,
+  PFC_ACTUATORS_STATE_RUNNING,
+  PFC_ACTUATORS_STATE_SUCCESS,
+  PFC_ACTUATORS_STATE_ERROR
+};
+
+/**
+ * @brief The state of the actuator during the test
+*/
+enum pfc_actuator_state_t {
+  PFC_ACTUATOR_STATE_WAIT,
+  PFC_ACTUATOR_STATE_LOW,
+  PFC_ACTUATOR_STATE_HIGH,
+};
+
+/**
+ * @brief The configuration struct of the actuator
+*/
+struct pfc_actuator_t {
+  uint8_t feedback_id;    ///< The feedback id of the actuator (255 for none)
+  uint8_t feedback_id2;   ///< The secondary feedback id of the actuator (255 for none)
+
+  int16_t low;            ///< The low value to set the actuator to
+  int16_t high;           ///< The high value to set the actuator to
+  float low_feedback;     ///< The expected feedback value when the actuator is low
+  float high_feedback;    ///< The expected feedback value when the actuator is high
+
+  float timeout;          ///< The timeout for the actuator to move
+};
+
+struct pfc_actuators_t {
+  enum pfc_actuators_state_t state;     ///< The state of the preflight checks
+
+  uint8_t act_idx;                      ///< The current actuator index
+  uint8_t act_nb;                       ///< The number of actuators
+  enum pfc_actuator_state_t act_state;  ///< The state of the actuator (during the test)
+  float act_start_time;                 ///< The start time of the actuator (during the test)
+
+  float last_feedback;                  ///< The last measured feedback value of the actuator
+  float last_feedback_err;              ///< The last expected feedback error of the actuator (based on RPM/angle)
+  float last_feedback2;                 ///< The last measured secondary feedback value of the actuator
+  float last_feedback_err2;             ///< The last expected secondary feedback error of the actuator (based on RPM/angle)
+};
+
+// Local variables and functions
+static struct pfc_actuator_t pfc_acts[] = PFC_ACTUATORS;
+static struct pfc_actuators_t pfc_actuators;
+static struct preflight_check_t actuators_pfc;
+static abi_event act_feedback_ev;
+static void pfc_actuators_cb(struct preflight_result_t *result);
+static void pfc_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
+
+/**
+ * @brief Send an error message to the ground station
+ * @param fmt The format of the message
+ * @param ... The arguments for the format
+ */
+static void pfc_actuators_error(const char *fmt, ...) {
+  char msg[200];
+
+  // Add the error
+  va_list args;
+  va_start(args, fmt);
+  int rc = vsnprintf(msg, 200, fmt, args);
+  va_end(args);
+
+  if(rc > 0) {
+    DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, msg);
+  }
+}
+
+/**
+ * @brief Send a debug message to the ground station
+*/
+#if PFC_ACTUATORS_DEBUG
+#define pfc_actuators_debug pfc_actuators_error
+#else
+#define pfc_actuators_debug(...)
+#endif
+
+/**
+ * @brief Register the preflight checks for the actuators
+ */
+void pfc_actuators_init(void) {
+  pfc_actuators.state = PFC_ACTUATORS_STATE_INIT;
+  pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
+  pfc_actuators.act_idx = 0;
+  pfc_actuators.act_nb = sizeof(pfc_acts) / sizeof(struct pfc_actuator_t);
+  pfc_actuators.last_feedback = NAN;
+  pfc_actuators.last_feedback2 = NAN;
+  
+  preflight_check_register(&actuators_pfc, pfc_actuators_cb);
+  AbiBindMsgACT_FEEDBACK(ABI_BROADCAST, &act_feedback_ev, pfc_act_feedback_cb);
+}
+
+/**
+ * @brief Move the actuators, should be put in the command laws
+ */
+void pfc_actuators_run(void) {
+  // Only actuate when running
+  if(pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING || pfc_actuators.act_idx >= pfc_actuators.act_nb) {
+    return;
+  }
+
+  // Get the actuators
+  struct pfc_actuator_t *act = &pfc_acts[pfc_actuators.act_idx];
+
+  // Verify the result and continue
+  if((get_sys_time_float() - pfc_actuators.act_start_time) > act->timeout) {
+    switch(pfc_actuators.act_state) {
+      case PFC_ACTUATOR_STATE_WAIT:
+        pfc_actuators_debug("Actuator %d starting LOW", pfc_actuators.act_idx);
+        pfc_actuators.act_state = PFC_ACTUATOR_STATE_LOW;
+        pfc_actuators.act_start_time = get_sys_time_float();
+        pfc_actuators.last_feedback = NAN;
+        pfc_actuators.last_feedback2 = NAN;
+        break;
+      case PFC_ACTUATOR_STATE_LOW:
+        // Check if the feedback is correct
+        if(act->feedback_id != 255 && 
+          (pfc_actuators.last_feedback < (act->low_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->low_feedback + pfc_actuators.last_feedback_err))) {
+          pfc_actuators_error("Actuator %d not responding correctly LOW %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->low_feedback - pfc_actuators.last_feedback_err, pfc_actuators.last_feedback, act->low_feedback + pfc_actuators.last_feedback_err);
+          pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
+        } else if(act->feedback_id2 != 255 && 
+          (pfc_actuators.last_feedback2 < (act->low_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->low_feedback + pfc_actuators.last_feedback_err2))) {
+          pfc_actuators_error("Actuator %d not responding correctly LOW2 %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->low_feedback - pfc_actuators.last_feedback_err2, pfc_actuators.last_feedback2, act->low_feedback + pfc_actuators.last_feedback_err2);
+          pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
+        }
+        if(pfc_actuators.state != PFC_ACTUATORS_STATE_ERROR || PFC_ACTUATORS_DEBUG) {
+          pfc_actuators_debug("Actuator %d LOW ok %.2f, %.2f starting HIGH", pfc_actuators.act_idx, pfc_actuators.last_feedback, pfc_actuators.last_feedback2);
+          pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING;
+          pfc_actuators.act_state = PFC_ACTUATOR_STATE_HIGH;
+          pfc_actuators.act_start_time = get_sys_time_float();
+          pfc_actuators.last_feedback = NAN;
+          pfc_actuators.last_feedback2 = NAN;
+        }
+        break;
+      case PFC_ACTUATOR_STATE_HIGH:
+        // Check if the feedback is correct
+        if(act->feedback_id != 255 && 
+          (pfc_actuators.last_feedback < (act->high_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->high_feedback + pfc_actuators.last_feedback_err))) {
+          pfc_actuators_error("Actuator %d not responding correctly HIGH %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->high_feedback - pfc_actuators.last_feedback_err, pfc_actuators.last_feedback, act->high_feedback + pfc_actuators.last_feedback_err);
+          pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
+        } else if(act->feedback_id2 != 255 && 
+          (pfc_actuators.last_feedback2 < (act->high_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->high_feedback + pfc_actuators.last_feedback_err2))) {
+          pfc_actuators_error("Actuator %d not responding correctly HIGH2 %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->high_feedback - pfc_actuators.last_feedback_err2, pfc_actuators.last_feedback2, act->high_feedback + pfc_actuators.last_feedback_err2);
+          pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
+        } 
+        if(pfc_actuators.state != PFC_ACTUATORS_STATE_ERROR || PFC_ACTUATORS_DEBUG) {
+          pfc_actuators_debug("Actuator %d HIGH ok %.2f, %.2f", pfc_actuators.act_idx, pfc_actuators.last_feedback, pfc_actuators.last_feedback2);
+          pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING;
+          pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
+          pfc_actuators.act_start_time = 0;
+          pfc_actuators.act_idx++;
+        }
+        break;
+    }
+  }
+
+  // Finished testing
+  if(pfc_actuators.act_idx >= pfc_actuators.act_nb) {
+    pfc_actuators.state = PFC_ACTUATORS_STATE_SUCCESS;
+    pfc_actuators_error("Actuators checks done %d", pfc_actuators.act_idx);
+  }
+}
+
+/**
+ * @brief Start the actuator testing
+*/
+void pfc_actuators_start(bool start) {
+  if(start && pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING) {
+    pfc_actuators.act_idx = 0;
+    pfc_actuators.act_start_time = 0;
+    pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING;
+    pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
+    pfc_actuators.last_feedback = NAN;
+    pfc_actuators.last_feedback2 = NAN;
+  }
+  else if(!start && pfc_actuators.state == PFC_ACTUATORS_STATE_RUNNING) {
+    pfc_actuators.act_idx = 0;
+    pfc_actuators.act_start_time = 0;
+    pfc_actuators.state = PFC_ACTUATORS_STATE_INIT;
+    pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
+  }
+}
+
+/**
+ * @brief Get the actuator value in the command laws to move the actuator during the preflight checks
+ * @param idx The index of the actuator in the preflight checks struct
+ * @param value The value if no checks are performed on this actuator
+*/
+int16_t pfc_actuators_value(uint8_t idx, int16_t value) {
+  if(idx != pfc_actuators.act_idx || pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING) {
+    return value;
+  }
+
+  if(pfc_actuators.act_state == PFC_ACTUATOR_STATE_LOW) {
+    return pfc_acts[pfc_actuators.act_idx].low;
+  }
+  else if(pfc_actuators.act_state == PFC_ACTUATOR_STATE_HIGH) {
+    return pfc_acts[pfc_actuators.act_idx].high;
+  }
+
+  return value;
+}
+
+/**
+ * @brief Check the actuators with feedback
+ * 
+ * @param result The result of the preflight checks
+ */
+static void pfc_actuators_cb(struct preflight_result_t *result)
+{
+  switch(pfc_actuators.state) {
+    case PFC_ACTUATORS_STATE_INIT:
+      preflight_error(result, "Actuators not checked perform checks first[%d]", pfc_actuators.act_nb);
+      break;
+    case PFC_ACTUATORS_STATE_RUNNING:
+      preflight_error(result, "Actuators are currently being checked[%d/%d]",pfc_actuators.act_idx, pfc_actuators.act_nb);
+      break;
+    case PFC_ACTUATORS_STATE_SUCCESS:
+      preflight_success(result, "Actuators checked and moved succesfully[%d]", pfc_actuators.act_nb);
+      break;
+    case PFC_ACTUATORS_STATE_ERROR:
+      preflight_error(result, "Actuators not responding correctly[%d/%d]",pfc_actuators.act_idx, pfc_actuators.act_nb);
+      break;
+  }
+}
+
+/**
+ * @brief Callback for the actuator feedback
+*/
+static void pfc_act_feedback_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *feedback, uint8_t num_act) {
+  if(pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING || pfc_actuators.act_idx >= pfc_actuators.act_nb) {
+    return;
+  }
+
+  // Save the last feedback values
+  for(uint8_t i = 0; i < num_act; i++) {
+    if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id && feedback[i].set.rpm) {
+      pfc_actuators.last_feedback = feedback[i].rpm;
+      pfc_actuators.last_feedback_err = PFC_ACTUATORS_MAX_RPM_ERROR;
+    } else if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id && feedback[i].set.position) {
+      pfc_actuators.last_feedback = feedback[i].position;
+      pfc_actuators.last_feedback_err = PFC_ACTUATORS_MAX_ANGLE_ERROR;
+    }
+
+    if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id2 && feedback[i].set.rpm) {
+      pfc_actuators.last_feedback2 = feedback[i].rpm;
+      pfc_actuators.last_feedback_err2 = PFC_ACTUATORS_MAX_RPM_ERROR;
+    } else if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id2 && feedback[i].set.position) {
+      pfc_actuators.last_feedback2 = feedback[i].position;
+      pfc_actuators.last_feedback_err2 = PFC_ACTUATORS_MAX_ANGLE_ERROR;
+    }
+  }
+}
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/checks/pfc_actuators.h"
+ * @author Freek van Tienen <freek.v.tienen@gmail.com>
+ * Checks the actuators with feedback before takeoff
+ */
+
+#ifndef PFC_ACTUATORS_H
+#define PFC_ACTUATORS_H
+
+#include "std.h"
+
+extern void pfc_actuators_init(void);
+extern void pfc_actuators_run(void);
+extern void pfc_actuators_start(bool start);
+extern int16_t pfc_actuators_value(uint8_t idx, int16_t value);
+
+#endif /* PFC_ACTUATORS_H */
@@ -164,14 +164,25 @@ let print_reverse_servo_table = fun out driver servos ->
  ) servos;
  fprintf out "    default: return 0;\n";
  fprintf out "  };\n";
+  fprintf out "}\n\n";
+  fprintf out "static inline int get_servo_idx%s(int _idx) {\n" d;
+  fprintf out "  switch (_idx) {\n";
+  List.iter (fun c ->
+    let name = ExtXml.attrib c "name" in
+    fprintf out "    case SERVO_%s_DRIVER_NO: return SERVO_%s_IDX;\n" name name;
+  ) servos;
+  fprintf out "    default: return 0;\n";
+  fprintf out "  };\n";
  fprintf out "}\n\n"

 let parse_servo = fun out driver c ->
  let shortname = ExtXml.attrib c "name" in
  let name = "SERVO_"^shortname
  and no_servo = int_of_string (ExtXml.attrib c "no") in
+  let global_idx = Hashtbl.length servos_drivers in

  define_out out (name^"_DRIVER_NO") (string_of_int no_servo);
+  define_out out (name^"_IDX") (string_of_int global_idx);

  let s_min = fos (ExtXml.attrib c "min" )
  and neutral = fos (ExtXml.attrib c "neutral")
@@ -194,7 +205,6 @@ let parse_servo = fun out driver c ->
  fprintf out "\n";

  (* Memorize the associated driver (if any) and global index (insertion order) *)
-  let global_idx = Hashtbl.length servos_drivers in
  Hashtbl.add servos_drivers shortname (driver, global_idx)

 (* Characters checked in Gen_radio.checl_function_name *)
@@ -207,7 +217,6 @@ let preprocess_value = fun s v prefix ->

 let print_actuators_idx = fun out ->
  Hashtbl.iter (fun s (d, i) ->
-    fprintf out "#define SERVO_%s_IDX %d\n" s i;
    (* Set servo macro *)
    fprintf out "#define Set_%s_Servo(_v) { \\\n" s;
    fprintf out "  actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
@@ -315,10 +324,8 @@ let rec parse_section = fun out ac_id s ->
      let driver = ExtXml.attrib_or_default s "driver" "Default" in
      let servos = Xml.children s in
      let nb_servos = List.fold_right (fun s m -> max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in
-      let servos_offset = Hashtbl.length servos_drivers in

      define_out out (sprintf "SERVOS_%s_NB" (String.uppercase_ascii driver)) (string_of_int nb_servos);
-      define_out out (sprintf "SERVOS_%s_OFFSET" (String.uppercase_ascii driver)) (string_of_int servos_offset);
      fprintf out "#include \"modules/actuators/actuators_%s.h\"\n" (String.lowercase_ascii driver);
      fprintf out "\n";
      List.iter (parse_servo out driver) servos;