Extra preflight checks

paparazzi · Nov 3, 2023 · 4a37e31 · 4a37e31
1 parent 624214f
commit 4a37e31
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 0 deletions.
diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c
@@ -159,6 +159,20 @@ static struct i2c_transaction ms45xx_trans;
 static Butterworth2LowPass ms45xx_filter;
 #endif
 
+#if PREFLIGHT_CHECKS
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t ms45xx_i2c_pfc;
+bool preflight_checks_ms45xx_offset_set = false;
+static void ms45xx_preflight(struct preflight_result_t *result) {
+  if(preflight_checks_ms45xx_offset_set) {
+    preflight_success(result, "ms45xx_i2c airspeed sensor nulled ok");
+  } else {
+    preflight_error(result, "ms45xx_i2c airspeed sensor not nulled");
+  }
+}
+#endif // PREFLIGHT_CHECKS
+
 static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
 {
   uint8_t dev_id = MS45XX_SENDER_ID;
@@ -192,6 +206,11 @@ void ms45xx_i2c_init(void)
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, ms45xx_downlink);
 #endif
+
+#if PREFLIGHT_CHECKS
+  /* Register preflight checks */
+  preflight_check_register(&ms45xx_i2c_pfc, ms45xx_preflight);
+#endif // AGL_DIST_PREFLIGHT_CHECK
 }
 
 void ms45xx_i2c_periodic(void)
@@ -256,6 +275,10 @@ void ms45xx_i2c_event(void)
           autoset_offset = 0.f;
           autoset_nb = 0;
           ms45xx.autoset_offset = false;
+
+          #if PREFLIGHT_CHECKS
+          preflight_checks_ms45xx_offset_set = true;
+          #endif // PREFLIGHT_CHECKS
         }
       }
 

diff --git a/sw/airborne/modules/sonar/agl_dist.c b/sw/airborne/modules/sonar/agl_dist.c
@@ -53,6 +53,25 @@ abi_event agl_ev;
 
 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
 
+
+#if PREFLIGHT_CHECKS
+#ifdef AGL_DIST_MIN_DISTANCE_CHECK
+#ifdef AGL_DIST_MAX_DISTANCE_CHECK
+#define AGL_DIST_PREFLIGHT_CHECK
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+  static struct preflight_check_t agl_dist_pfc;
+  static void agl_dist_preflight(struct preflight_result_t *result) {
+    if(agl_dist_valid && agl_dist_value > AGL_DIST_MIN_DISTANCE_CHECK && agl_dist_value < AGL_DIST_MAX_DISTANCE_CHECK) {
+      preflight_success(result, "agl data ok");
+    } else {
+      preflight_error(result, "No valid agl data");
+    }
+  }
+#endif // AGL_DIST_MIN_DISTANCE_CHECK
+#endif // AGL_DIST_MAX_DISTANCE_CHECK
+#endif // PREFLIGHT_CHECKS
+
 void agl_dist_init(void)
 {
   agl_dist_valid = false;
@@ -62,6 +81,11 @@ void agl_dist_init(void)
 
   // Bind to AGL message
   AbiBindMsgAGL(AGL_DIST_ID, &agl_ev, agl_cb);
+
+  #ifdef AGL_DIST_PREFLIGHT_CHECK
+   /* Register preflight checks */
+    preflight_check_register(&agl_dist_pfc, agl_dist_preflight);
+  #endif // AGL_DIST_PREFLIGHT_CHECK
 }
 
 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t __attribute__((unused)) stamp, float distance)