Preflight checks: Airspeed checks update

-check for valid airspeed_validated (declared valid plus updated less than 1s ago)
-added param (COM_ARM_ARSP_EN) to enable/disable check for max airspeed for arming set max
airspeed limit to half of stall airspeed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -180,7 +180,17 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
 		const bool optional = (airspeed_mode == 1);
 
-		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm) && !optional) {
+		int32_t max_airspeed_check_en = 0;
+		param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);
+
+		float airspeed_stall = 10.0f;
+		param_get(param_find("ASPD_STALL"), &airspeed_stall);
+
+		const float arming_max_airspeed_allowed = airspeed_stall / 2.0f; // set to half of stall speed
+
+		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en,
+				   arming_max_airspeed_allowed)
+		    && !(bool)optional) {
 			failed = true;
 		}
 	}

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -104,7 +104,7 @@ private:
 			      const bool optional, int32_t &device_id, const bool report_fail);
 	static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
 	static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-				  const bool report_fail, const bool prearm);
+				  const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);
 	static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL);
 	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
 			       const bool prearm);

src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp

@@ -38,23 +38,26 @@
 #include <math.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
 
 using namespace time_literals;
 
 bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-				   const bool report_fail, const bool prearm)
+				   const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed)
 {
 	bool present = true;
 	bool success = true;
 
-	uORB::SubscriptionData<airspeed_s> airspeed_sub{ORB_ID(airspeed)};
-	airspeed_sub.update();
-	const airspeed_s &airspeed = airspeed_sub.get();
+	uORB::SubscriptionData<airspeed_validated_s> airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	airspeed_validated_sub.update();
+	const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get();
 
-	if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) {
+	/*
+	 * Check if Airspeed Selector is up and running.
+	 */
+	if (hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
 		if (report_fail && !optional) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing");
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down.");
 		}
 
 		present = false;
@@ -63,14 +66,11 @@ bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status
 	}
 
 	/*
-	 * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise
-	 * on the bench and trigger false positives of the voter. Therefore only fail this
-	 * for a pre-arm check, as then the cover is off and the natural airflow in the field
-	 * will ensure there is not zero noise.
+	 * Check if airspeed is declared valid or not by airspeed selector.
 	 */
-	if (prearm && fabsf(airspeed.confidence) < 0.95f) {
+	if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
 		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck");
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid.");
 		}
 
 		present = true;
@@ -79,13 +79,14 @@ bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status
 	}
 
 	/*
-	 * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying
+	 * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
 	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
 	 * might have been removed.
 	 */
-	if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && prearm) {
+	if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed
+	    && prearm) {
 		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot");
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot");
 		}
 
 		present = true;

src/modules/commander/commander_params.c

@@ -956,3 +956,15 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1);
  * @decimal 3
  */
 PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f);
+
+/**
+* Enable preflight check for maximal allowed airspeed when arming.
+*
+* Deny arming if the current airspeed measurement is greater than half the stall speed (ASPD_STALL).
+* Excessive airspeed measurements on ground are either caused by wind or bad airspeed calibration.
+*
+* @group Commander
+* @value 0 Disabled
+* @value 1 Enabled
+*/
+PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);