[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>
paparazzi · Dec 18, 2023 · 5fd7d56 · 5fd7d56
1 parent 751edc5
commit 5fd7d56
Show file tree

Hide file tree

Showing 12 changed files with 515 additions and 70 deletions.
diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml
@@ -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" />

diff --git a/conf/modules/actuators_bebop.xml b/conf/modules/actuators_bebop.xml
@@ -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>

diff --git a/conf/modules/actuators_disco.xml b/conf/modules/actuators_disco.xml
@@ -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>

diff --git a/conf/modules/pfc_actuators.xml b/conf/modules/pfc_actuators.xml
@@ -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>
diff --git a/conf/modules/preflight_checks.xml b/conf/modules/preflight_checks.xml
@@ -18,6 +18,9 @@
       </dl_settings>
     </dl_settings>
   </settings>
+  <dep>
+    <depends>@datalink</depends>
+  </dep>
   <header>
     <file name="preflight_checks.h"/>
   </header>

diff --git a/conf/modules/sensors_hitl.xml b/conf/modules/sensors_hitl.xml
@@ -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>

diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c
@@ -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;
   }

diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c
@@ -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);