[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>

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" />

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>

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>

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>

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>

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>