Airspeed: preflight check for bad offset, fixed calls to preflight checks (vtol & airspeed)

msg/airspeed.msg

@@ -3,3 +3,4 @@ float32 true_airspeed_m_s		# true filtered airspeed in meters per second, -1 if
 float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if unknown
 float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
 float32 confidence			# confidence value from 0 to 1 for this sensor
+float32 differential_pressure_filtered_pa # filtered differential pressure, can be negative

src/modules/commander/PreflightCheck.cpp

@@ -363,7 +363,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 	return success;
 }
 
-static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail)
+static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm, hrt_abstime time_since_boot)
 {
 	bool success = true;
 	int ret;
@@ -388,11 +388,24 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		goto out;
 	}
 
-	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
+	/**
+	 * Check if differential pressure is off by more than 15Pa which equals to 5m/s when measuring no airspeed.
+	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
+	 * might have been removed.
+	 */
+
+	if (time_since_boot < 1e6) {
+		// the airspeed driver filter doesn't deliver the actual value yet
+		success = false;
+		goto out;
+	}
+
+	if (fabsf(airspeed.differential_pressure_filtered_pa) > 15.0f && !prearm) {
 		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED CALIBRATION ISSUE");
 		}
-		// XXX do not make this fatal yet
+		success = false;
+		goto out;
 	}
 
 out:
@@ -511,7 +524,8 @@ out:
 }
 
 bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
-		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL, bool reportFailures)
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS,
+		    bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot)
 {
 
 #ifdef __PX4_QURT
@@ -651,7 +665,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
 
 	/* ---- AIRSPEED ---- */
 	if (checkAirspeed) {
-		if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
+		if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm, time_since_boot)) {
 			failed = true;
 		}
 	}

src/modules/commander/PreflightCheck.h

@@ -39,6 +39,8 @@
  * @author Johan Jansen <jnsn.johan@gmail.com>
  */
 
+#include <drivers/drv_hrt.h>
+
 #pragma once
 
 namespace Commander
@@ -67,7 +69,8 @@ namespace Commander
 *   true if the GNSS receiver should be checked
 **/
 bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
-    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL, bool reportFailures = false);
+    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS,
+    bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot);
 
 const unsigned max_mandatory_gyro_count = 1;
 const unsigned max_optional_gyro_count = 3;

src/modules/commander/commander.cpp

@@ -401,9 +401,9 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "check")) {
 		int checkres = 0;
-		checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false);
+		checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false, hrt_elapsed_time(&commander_boot_timestamp));
 		warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
-		checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true);
+		checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true, hrt_elapsed_time(&commander_boot_timestamp));
 		warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
 		return 0;
 	}
@@ -663,7 +663,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
 					     mavlink_log_pub_local,
 					     &status_flags,
 					     avionics_power_rail_voltage,
-					     can_arm_without_gps);
+					     can_arm_without_gps,
+					     hrt_elapsed_time(&commander_boot_timestamp));
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -1634,7 +1635,8 @@ int commander_thread_main(int argc, char *argv[])
 	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
-	if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+	if (!status_flags.circuit_breaker_engaged_airspd_check &&
+	    (!status.is_rotary_wing || status.is_vtol)) {
 		checkAirspeed = true;
 	}
 
@@ -1654,8 +1656,8 @@ int commander_thread_main(int argc, char *argv[])
 	} else {
 			// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
 			status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
-				checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				!can_arm_without_gps, /*checkDynamic */ false, is_vtol(&status), /* reportFailures */ false);
+				checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check,
+				/* checkDynamic */ false, is_vtol(&status), /* reportFailures */ false, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
@@ -1894,25 +1896,19 @@ int commander_thread_main(int argc, char *argv[])
 				    /* and the system is not already armed (and potentially flying) */
 				    !armed.armed) {
 
-					bool chAirspeed = false;
 					hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 
-					/* Perform airspeed check only if circuit breaker is not
-					 * engaged and it's not a rotary wing
-					 */
-					if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
-						chAirspeed = true;
-					}
-
 					/* provide RC and sensor status feedback to the user */
 					if (is_hil_setup(autostart_id)) {
 						/* HIL configuration: check only RC input */
 						(void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false,
-								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), /* checkGNSS */ false, /* checkDynamic */ true, /* reportFailures */ false);
+								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false,
+								 /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ false, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
 					} else {
 						/* check sensors also */
-						(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed,
-								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout);
+						(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
+								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check,
+								 /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
 					}
 				}
 
@@ -2022,7 +2018,8 @@ int commander_thread_main(int argc, char *argv[])
 										  &mavlink_log_pub,
 										  &status_flags,
 										  avionics_power_rail_voltage,
-										  can_arm_without_gps)) {
+										  can_arm_without_gps,
+										  hrt_elapsed_time(&commander_boot_timestamp))) {
 					mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -2322,7 +2319,8 @@ int commander_thread_main(int argc, char *argv[])
 							     &mavlink_log_pub,
 							     &status_flags,
 							     avionics_power_rail_voltage,
-							     can_arm_without_gps);
+							     can_arm_without_gps,
+							     hrt_elapsed_time(&commander_boot_timestamp));
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -2583,7 +2581,8 @@ int commander_thread_main(int argc, char *argv[])
 									     &mavlink_log_pub,
 									     &status_flags,
 									     avionics_power_rail_voltage,
-									     can_arm_without_gps);
+									     can_arm_without_gps,
+									     hrt_elapsed_time(&commander_boot_timestamp));
 
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
@@ -2631,7 +2630,8 @@ int commander_thread_main(int argc, char *argv[])
 										     &mavlink_log_pub,
 										     &status_flags,
 										     avionics_power_rail_voltage,
-										     can_arm_without_gps);
+										     can_arm_without_gps,
+										     hrt_elapsed_time(&commander_boot_timestamp));
 
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
@@ -3920,7 +3920,8 @@ void *commander_low_prio_loop(void *arg)
 											 &mavlink_log_pub,
 											 &status_flags,
 											 avionics_power_rail_voltage,
-											 can_arm_without_gps)) {
+											 can_arm_without_gps,
+											 hrt_elapsed_time(&commander_boot_timestamp))) {
 						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
 						break;
 					} else {
@@ -3999,12 +4000,14 @@ void *commander_low_prio_loop(void *arg)
 						bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 						/* Perform airspeed check only if circuit breaker is not
 						 * engaged and it's not a rotary wing */
-						if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+						if (!status_flags.circuit_breaker_engaged_airspd_check &&
+						    (!status.is_rotary_wing || status.is_vtol)) {
 							checkAirspeed = true;
 						}
 
 						status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout);
+							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status_flags.circuit_breaker_engaged_gpsfailure_check,
+							/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
 
 						arming_state_transition(&status,
 									&battery,
@@ -4015,7 +4018,8 @@ void *commander_low_prio_loop(void *arg)
 									&mavlink_log_pub,
 									&status_flags,
 								        avionics_power_rail_voltage,
-									can_arm_without_gps);
+									can_arm_without_gps,
+									hrt_elapsed_time(&commander_boot_timestamp));
 
 					} else {
 						tune_negative(true);

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -293,10 +293,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
         // Attempt transition
         transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed,
-        		false /* no pre-arm checks */,
-        		nullptr /* no mavlink_log_pub */,
-        		&status_flags,
-        		5.0f, check_gps);
+				false /* no pre-arm checks */,
+				nullptr /* no mavlink_log_pub */,
+				&status_flags,
+				5.0f, check_gps, 2e6);
 
         // Validate result of transition
         ut_compare(test->assertMsg, test->expected_transition_result, result);

src/modules/commander/state_machine_helper.cpp

@@ -126,7 +126,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 		orb_advert_t *mavlink_log_pub,	///< uORB handle for mavlink log
 		status_flags_s *status_flags,
 		float avionics_power_rail_voltage,
-		bool can_arm_without_gps)
+		bool can_arm_without_gps,
+		hrt_abstime time_since_boot)
 {
 	// Double check that our static arrays are still valid
 	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -152,7 +153,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 		    && status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
-						     status_flags, battery, can_arm_without_gps);
+						     status_flags, battery, can_arm_without_gps, time_since_boot);
 		}
 
 		/* re-run the pre-flight check as long as sensors are failing */
@@ -163,7 +164,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
 				prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */,
-							     status_flags, battery, can_arm_without_gps);
+							     status_flags, battery, can_arm_without_gps, time_since_boot);
 				status_flags->condition_system_sensors_initialized = !prearm_ret;
 				last_preflight_check = hrt_absolute_time();
 				last_prearm_ret = prearm_ret;
@@ -1102,7 +1103,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 }
 
 int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
-		    status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps)
+		    status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps, hrt_abstime time_since_boot)
 {
 	/*
 	 */
@@ -1119,7 +1120,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 
 	bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
 			    checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-			    !can_arm_without_gps, true, status->is_vtol, reportFailures);
+			    !can_arm_without_gps, true, status->is_vtol, reportFailures, prearm, time_since_boot);
 
 	if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) {
 		preflight_ok = false;

src/modules/commander/state_machine_helper.h

@@ -42,6 +42,8 @@
 #ifndef STATE_MACHINE_HELPER_H_
 #define STATE_MACHINE_HELPER_H_
 
+#include <drivers/drv_hrt.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/battery_status.h>
@@ -106,7 +108,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 					    orb_advert_t *mavlink_log_pub,	///< uORB handle for mavlink log
 					    status_flags_s *status_flags,
 					    float avionics_power_rail_voltage,
-					    bool can_arm_without_gps);
+					    bool can_arm_without_gps,
+					    hrt_abstime time_since_boot);
 
 transition_result_t
 main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
@@ -124,6 +127,8 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
 		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
 
-int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps);
+int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
+	bool force_report, status_flags_s *status_flags, battery_status_s *battery,
+	bool can_arm_without_gps, hrt_abstime time_since_boot);
 
 #endif /* STATE_MACHINE_HELPER_H_ */

src/modules/sensors/sensors.cpp

@@ -1409,6 +1409,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 						   _last_best_baro_pressure * 1e2f, air_temperature_celsius));
 
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
+		_airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub != nullptr) {