move estimator_status preflight failure flags to estimator_status_flags

- shift around estimatorStatus() and estimatorStatusFlags() health
checks

boards/px4/fmu-v6c/src/i2c.cpp

@@ -36,5 +36,5 @@
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
 	initI2CBusExternal(2),
-	initI2CBusExternal(4),
+	initI2CBusInternal(4),
 };

msg/estimator_status.msg

@@ -113,12 +113,6 @@ uint8 reset_count_quat   # number of quaternion reset events (allow to wrap if c
 
 float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
 
-bool pre_flt_fail_innov_heading
-bool pre_flt_fail_innov_vel_horiz
-bool pre_flt_fail_innov_vel_vert
-bool pre_flt_fail_innov_height
-bool pre_flt_fail_mag_field_disturbed
-
 uint32 accel_device_id
 uint32 gyro_device_id
 uint32 baro_device_id

msg/estimator_status_flags.msg

@@ -73,3 +73,10 @@ bool reject_sideslip                      #  9 - true if the synthetic sideslip
 bool reject_hagl                          # 10 - true if the height above ground observation has been rejected
 bool reject_optflow_x                     # 11 - true if the X optical flow observation has been rejected
 bool reject_optflow_y                     # 12 - true if the Y optical flow observation has been rejected
+
+
+# preflight failure flags
+bool pre_flt_fail_innov_heading
+bool pre_flt_fail_innov_vel_horiz
+bool pre_flt_fail_innov_vel_vert
+bool pre_flt_fail_innov_height

src/drivers/barometer/bmp388/bmp388.cpp

@@ -68,11 +68,19 @@ BMP388::init()
 		return -EIO;
 	}
 
-	if (_interface->get_reg(BMP3_CHIP_ID_ADDR) != BMP3_CHIP_ID) {
-		PX4_WARN("id of your baro is not: 0x%02x", BMP3_CHIP_ID);
+	_chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR);
+
+	if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) {
+		PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID);
 		return -EIO;
 	}
 
+	if (_chip_id == BMP390_CHIP_ID) {
+		_interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
+	}
+
+	_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);
+
 	_cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR);
 
 	if (!_cal) {
@@ -99,6 +107,7 @@ void
 BMP388::print_status()
 {
 	I2CSPIDriverBase::print_status();
+	printf("chip id:  0x%02x rev id:  0x%02x\n", _chip_id, _chip_rev_id);
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_measure_perf);
 	perf_print_counter(_comms_errors);

src/drivers/barometer/bmp388/bmp388.h

@@ -51,7 +51,8 @@
 
 // From https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h
 
-#define BMP3_CHIP_ID                      (0x50)
+#define BMP388_CHIP_ID                    (0x50)
+#define BMP390_CHIP_ID                    (0x60)
 
 /* Over sampling macros */
 #define BMP3_NO_OVERSAMPLING              (0x00)
@@ -79,6 +80,7 @@
 
 /* Register Address */
 #define BMP3_CHIP_ID_ADDR                 (0x00)
+#define BMP3_REV_ID_ADDR                  (0x01)
 #define BMP3_ERR_REG_ADDR                 (0x02)
 #define BMP3_SENS_STATUS_REG_ADDR         (0x03)
 #define BMP3_DATA_ADDR                    (0x04)
@@ -304,6 +306,8 @@ public:
 	virtual uint32_t get_device_id() const = 0;
 
 	virtual uint8_t get_device_address() const = 0;
+
+	virtual void set_device_type(uint8_t devtype) = 0;
 };
 
 class BMP388 : public I2CSPIDriver<BMP388>
@@ -339,6 +343,9 @@ private:
 
 	bool			_collect_phase{false};
 
+	uint8_t                 _chip_id{0};
+	uint8_t                 _chip_rev_id{0};
+
 	void 			start();
 	int 			measure();
 	int			collect(); //get results and publish

src/drivers/barometer/bmp388/bmp388_i2c.cpp

@@ -58,6 +58,7 @@ public:
 
 	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
 
+	void set_device_type(uint8_t devtype);
 private:
 	struct calibration_s _cal;
 };
@@ -108,3 +109,8 @@ calibration_s *BMP388_I2C::get_calibration(uint8_t addr)
 		return nullptr;
 	}
 }
+
+void BMP388_I2C::set_device_type(uint8_t devtype)
+{
+	device::Device::set_device_type(devtype);
+}

src/drivers/barometer/bmp388/bmp388_spi.cpp

@@ -74,6 +74,8 @@ public:
 	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
 
 	uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
+
+	void set_device_type(uint8_t devtype);
 private:
 	spi_calibration_s _cal;
 };
@@ -124,3 +126,8 @@ calibration_s *BMP388_SPI::get_calibration(uint8_t addr)
 		return nullptr;
 	}
 }
+
+void BMP388_SPI::set_device_type(uint8_t devtype)
+{
+	device::Device::set_device_type(devtype);
+}

src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp

@@ -234,7 +234,6 @@ void AFBRS50::Run()
 		break;
 
 	case STATE::CONFIGURE: {
-			//status_t status = Argus_SetConfigurationFrameTime(_hnd, _measure_interval);
 			status_t status = set_rate(SHORT_RANGE_MODE_HZ);
 
 			if (status != STATUS_OK) {
@@ -259,8 +258,6 @@ void AFBRS50::Run()
 			_mode = ARGUS_MODE_B;
 			set_mode(_mode);
 
-			status = Argus_StartMeasurementTimer(_hnd, measurement_ready_callback);
-
 			if (status != STATUS_OK) {
 				PX4_ERR("CONFIGURE status not okay: %i", (int)status);
 				ScheduleNow();
@@ -273,7 +270,14 @@ void AFBRS50::Run()
 		break;
 
 	case STATE::COLLECT: {
-			// currently handeled by measurement_ready_callback
+			// Only start a new measurement if one is not ongoing
+			if (Argus_GetStatus(_hnd) == STATUS_IDLE) {
+				status_t status = Argus_TriggerMeasurement(_hnd, measurement_ready_callback);
+
+				if (status != STATUS_OK) {
+					PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status);
+				}
+			}
 
 			UpdateMode();
 		}
@@ -290,8 +294,7 @@ void AFBRS50::Run()
 		break;
 	}
 
-	// backup schedule
-	ScheduleDelayed(100_ms);
+	ScheduleDelayed(_measure_interval);
 }
 
 void AFBRS50::UpdateMode()

src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c

@@ -2,6 +2,7 @@
 #include <nuttx/irq.h>
 
 static volatile irqstate_t irqstate_flags;
+static volatile size_t _lock_count = 0;
 
 /*!***************************************************************************
 * @brief Enable IRQ Interrupts
@@ -10,7 +11,13 @@ static volatile irqstate_t irqstate_flags;
 *****************************************************************************/
 void IRQ_UNLOCK(void)
 {
-	leave_critical_section(irqstate_flags);
+	if (_lock_count > 0) {
+		_lock_count--;
+
+		if (_lock_count == 0) {
+			leave_critical_section(irqstate_flags);
+		}
+	}
 }
 
 /*!***************************************************************************
@@ -20,5 +27,9 @@ void IRQ_UNLOCK(void)
 *****************************************************************************/
 void IRQ_LOCK(void)
 {
-	irqstate_flags = enter_critical_section();
+	if (_lock_count == 0) {
+		irqstate_flags = enter_critical_section();
+	}
+
+	_lock_count++;
 }

src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c

@@ -138,22 +138,23 @@ status_t S2PI_SetBaudRate(uint32_t baudRate_Bps)
 status_t S2PI_CaptureGpioControl(void)
 {
 	/* Check if something is ongoing. */
-	irqstate_t irqstate_flags = px4_enter_critical_section();
+	IRQ_LOCK();
 	status_t status = s2pi_.Status;
 
 	if (status != STATUS_IDLE) {
-		px4_leave_critical_section(irqstate_flags);
+		IRQ_UNLOCK();
 		return status;
 	}
 
 	s2pi_.Status = STATUS_S2PI_GPIO_MODE;
-	px4_leave_critical_section(irqstate_flags);
 
 	// GPIO mode (output push pull)
 	px4_arch_configgpio(PX4_MAKE_GPIO_OUTPUT_SET(s2pi_.GPIOs[S2PI_CLK]));
-	px4_arch_configgpio(PX4_MAKE_GPIO_OUTPUT_SET(s2pi_.GPIOs[S2PI_MISO]));
+	px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(s2pi_.GPIOs[S2PI_MISO]));
 	px4_arch_configgpio(PX4_MAKE_GPIO_OUTPUT_SET(s2pi_.GPIOs[S2PI_MOSI]));
 
+	IRQ_UNLOCK();
+
 	return STATUS_OK;
 }
 
@@ -167,16 +168,15 @@ status_t S2PI_CaptureGpioControl(void)
 status_t S2PI_ReleaseGpioControl(void)
 {
 	/* Check if something is ongoing. */
-	irqstate_t irqstate_flags = px4_enter_critical_section();
+	IRQ_LOCK();
 	status_t status = s2pi_.Status;
 
 	if (status != STATUS_S2PI_GPIO_MODE) {
-		px4_leave_critical_section(irqstate_flags);
+		IRQ_UNLOCK();
 		return status;
 	}
 
 	s2pi_.Status = STATUS_IDLE;
-	px4_leave_critical_section(irqstate_flags);
 
 	// SPI alternate
 	stm32_configgpio(s2pi_.GPIOs[S2PI_CLK]);
@@ -186,6 +186,8 @@ status_t S2PI_ReleaseGpioControl(void)
 	// probably not necessary
 	stm32_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS);
 
+	IRQ_UNLOCK();
+
 	return STATUS_OK;
 }
 
@@ -254,22 +256,23 @@ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value)
 status_t S2PI_CycleCsPin(s2pi_slave_t slave)
 {
 	/* Check the driver status. */
-	irqstate_t irqstate_flags = px4_enter_critical_section();
+	IRQ_LOCK();
 	status_t status = s2pi_.Status;
 
 	if (status != STATUS_IDLE) {
-		px4_leave_critical_section(irqstate_flags);
+		IRQ_UNLOCK();
 		return status;
 	}
 
 	s2pi_.Status = STATUS_BUSY;
-	px4_leave_critical_section(irqstate_flags);
 
 	px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0);
 	px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1);
 
 	s2pi_.Status = STATUS_IDLE;
 
+	IRQ_UNLOCK();
+
 	return STATUS_OK;
 }
 
@@ -339,11 +342,11 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8
 	}
 
 	/* Check the driver status, lock if idle. */
-	irqstate_t irqstate_flags = px4_enter_critical_section();
+	IRQ_LOCK();
 	status_t status = s2pi_.Status;
 
 	if (status != STATUS_IDLE) {
-		px4_leave_critical_section(irqstate_flags);
+		IRQ_UNLOCK();
 		return status;
 	}
 
@@ -358,7 +361,7 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8
 	s2pi_.spi_frame_size = frameSize;
 	work_queue(HPWORK, &broadcom_s2pi_transfer_work, broadcom_s2pi_transfer_callout, NULL, 0);
 
-	px4_leave_critical_section(irqstate_flags);
+	IRQ_UNLOCK();
 
 	return STATUS_OK;
 }

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -94,7 +94,7 @@ static constexpr uint8_t LL40LS_UNIT_ID_3_V4	      = 0x19;
 static constexpr uint8_t LL40LS_DISTHIGH_REG_V4       = 0x10; /* High byte of distance register, auto increment */
 
 // Normal conversion wait time.
-static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms};
+static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{10_ms};
 
 // Maximum time to wait for a conversion to complete.
 static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms};

src/drivers/drv_hrt.h

@@ -61,8 +61,6 @@ __BEGIN_DECLS
  */
 typedef uint64_t	hrt_abstime;
 
-#define HRT_ABSTIME_INVALID UINT64_MAX
-
 /**
  * Callout function type.
  *

src/drivers/drv_sensor.h

@@ -128,18 +128,19 @@
 #define DRV_BARO_DEVTYPE_MPC2520	0x5F
 #define DRV_BARO_DEVTYPE_LPS22HB	0x60
 
-#define DRV_ACC_DEVTYPE_LSM303AGR	0x61
-#define DRV_MAG_DEVTYPE_LSM303AGR	0x62
-#define DRV_IMU_DEVTYPE_ADIS16497	0x63
-#define DRV_BARO_DEVTYPE_BAROSIM	0x65
-#define DRV_GYR_DEVTYPE_BMI088		0x66
-#define DRV_BARO_DEVTYPE_BMP388	0x67
-#define DRV_BARO_DEVTYPE_DPS310	0x68
-#define DRV_PWM_DEVTYPE_PCA9685     0x69
-#define DRV_ACC_DEVTYPE_BMI088		0x6a
-#define DRV_OSD_DEVTYPE_ATXXXX		0x6b
-#define DRV_ACC_DEVTYPE_BMI085		0x6C
-#define DRV_GYR_DEVTYPE_BMI085		0x6D
+#define DRV_ACC_DEVTYPE_LSM303AGR       0x61
+#define DRV_MAG_DEVTYPE_LSM303AGR       0x62
+#define DRV_IMU_DEVTYPE_ADIS16497       0x63
+#define DRV_BARO_DEVTYPE_BAROSIM        0x65
+#define DRV_GYR_DEVTYPE_BMI088          0x66
+#define DRV_BARO_DEVTYPE_BMP388         0x67
+#define DRV_BARO_DEVTYPE_DPS310         0x68
+#define DRV_PWM_DEVTYPE_PCA9685         0x69
+#define DRV_ACC_DEVTYPE_BMI088          0x6a
+#define DRV_OSD_DEVTYPE_ATXXXX          0x6b
+#define DRV_ACC_DEVTYPE_BMI085          0x6C
+#define DRV_GYR_DEVTYPE_BMI085          0x6D
+#define DRV_BARO_DEVTYPE_BMP390         0x6E
 
 #define DRV_DIST_DEVTYPE_LL40LS       0x70
 #define DRV_DIST_DEVTYPE_MAPPYDOT     0x71

src/drivers/gps/gps.cpp

@@ -783,9 +783,20 @@ GPS::run()
 
 		} else if (gps_ubx_mode == 4) {
 			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
+
+		} else if (gps_ubx_mode == 5) { // rover with static base on Uart2
+			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
+
 		}
 	}
 
+	handle = param_find("GPS_UBX_BAUD2");
+	int32_t f9p_uart2_baudrate = 57600;
+
+	if (handle != PARAM_INVALID) {
+		param_get(handle, &f9p_uart2_baudrate);
+	}
+
 	int32_t gnssSystemsParam = static_cast<int32_t>(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS);
 
 	if (_instance == Instance::Main) {
@@ -846,7 +857,7 @@ GPS::run()
 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
 			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
-						   gps_ubx_dynmodel, heading_offset, ubx_mode);
+						   gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
 #ifndef CONSTRAINED_FLASH

src/drivers/gps/params.c

@@ -87,6 +87,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
  * Select the u-blox configuration setup. Most setups will use the default, including RTK and
  * dual GPS without heading.
  *
+ * If rover has RTCM corrections from a static base (or other static correction source) coming in on UART2, then select Mode 5.
  * The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output
  * heading information, whereas the secondary will act as moving base.
  * Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the
@@ -101,6 +102,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
  * @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover)
  * @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
  * @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
+ * @value 5 Rover with Static Base on UART2 (similar to Default, except coming in on UART2)
  *
  * @reboot_required true
  * @group GPS
@@ -108,6 +110,22 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
 PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
 
 
+/**
+ * u-blox F9P UART2 Baudrate
+ *
+ * Select a baudrate for the F9P's UART2 port.
+ * In GPS_UBX_MODE 1, 2, and 3, the F9P's UART2 port is configured to send/receive RTCM corrections.
+ * Set this to 57600 if you want to attach a telemetry radio on UART2.
+ *
+ * @min 0
+ * @unit B/s
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400);
+
+
 /**
  * Heading/Yaw offset for dual antenna GPS
  *

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -88,14 +88,13 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 
 	if (!missing_data) {
-		estimator_status_s estimator_status;
+		estimator_status_flags_s estimator_status_flags;
 
-		if (_estimator_status_sub.copy(&estimator_status)) {
-			pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
-			pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
-
-			checkEstimatorStatus(context, reporter, estimator_status, required_groups);
-			checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
+		if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
+			pre_flt_fail_innov_heading = estimator_status_flags.pre_flt_fail_innov_heading;
+			pre_flt_fail_innov_vel_horiz = estimator_status_flags.pre_flt_fail_innov_vel_horiz;
+			checkEstimatorStatusFlags(context, reporter, estimator_status_flags, required_groups);
+			checkEstimatorStatus(context, reporter, lpos, required_groups);
 
 		} else {
 			missing_data = true;
@@ -129,325 +128,323 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 }
 
 void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
-		const estimator_status_s &estimator_status, NavModes required_groups)
+		const vehicle_local_position_s &lpos, NavModes required_groups)
 {
-	if (estimator_status.pre_flt_fail_innov_heading) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_heading_not_stable"),
-					    events::Log::Error, "Heading estimate not stable");
+	estimator_status_s estimator_status;
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
-		}
+	if (_estimator_status_sub.copy(&estimator_status)) {
 
-	} else if (estimator_status.pre_flt_fail_innov_vel_horiz) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_hor_vel_not_stable"),
-					    events::Log::Error, "Horizontal velocity estimate not stable");
+		// check vertical position innovation test ratio
+		if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_hgt_est_err"),
+					events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity estimate not stable");
-		}
-
-	} else if (estimator_status.pre_flt_fail_innov_vel_vert) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_vert_vel_not_stable"),
-					    events::Log::Error, "Vertical velocity estimate not stable");
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity estimate not stable");
-		}
-
-	} else if (estimator_status.pre_flt_fail_innov_height) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_hgt_not_stable"),
-					    events::Log::Error, "Height estimate not stable");
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
-		}
-	}
-
-
-	if ((_param_com_arm_mag_str.get() >= 1) && estimator_status.pre_flt_fail_mag_field_disturbed) {
-		NavModes required_groups_mag = required_groups;
-
-		if (_param_com_arm_mag_str.get() != 1) {
-			required_groups_mag = NavModes::None; // optional
-		}
-
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_mag_interference"),
-					    events::Log::Warning, "Strong magnetic interference detected");
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference detected");
-		}
-	}
-
-	// check vertical position innovation test ratio
-	if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_hgt_est_err"),
-				events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
-		}
-	}
-
-	// check velocity innovation test ratio
-	if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_vel_est_err"),
-				events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
-		}
-	}
-
-	// check horizontal position innovation test ratio
-	if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_pos_est_err"),
-				events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
-		}
-	}
-
-	// check magnetometer innovation test ratio
-	if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_yaw_est_err"),
-				events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
-		}
-	}
-
-	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
-	if (_param_sys_has_gps.get()) {
-		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
-		const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
-
-		if (ekf_gps_fusion) {
-			reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
-		}
-
-		if (ekf_gps_check_fail) {
-			NavModes required_groups_gps = required_groups;
-			events::Log log_level = events::Log::Error;
-
-			if (_param_com_arm_wo_gps.get()) {
-				required_groups_gps = NavModes::None; // optional
-				log_level = events::Log::Warning;
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
 			}
+		}
 
-			// Only report the first failure to avoid spamming
-			const char *message = nullptr;
+		// check velocity innovation test ratio
+		if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_vel_est_err"),
+					events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
 
-			if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
-				message = "Preflight%s: GPS fix too low";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_fix_too_low"),
-							    log_level, "GPS fix too low");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
+			}
+		}
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
-				message = "Preflight%s: not enough GPS Satellites";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_num_sats_too_low"),
-							    log_level, "Not enough GPS Satellites");
+		// check horizontal position innovation test ratio
+		if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_pos_est_err"),
+					events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
-				message = "Preflight%s: GPS PDOP too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_pdop_too_high"),
-							    log_level, "GPS PDOP too high");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
+			}
+		}
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
-				message = "Preflight%s: GPS Horizontal Pos Error too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_pos_err_too_high"),
-							    log_level, "GPS Horizontal Position Error too high");
+		// check magnetometer innovation test ratio
+		if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_yaw_est_err"),
+					events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
-				message = "Preflight%s: GPS Vertical Pos Error too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_pos_err_too_high"),
-							    log_level, "GPS Vertical Position Error too high");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
+			}
+		}
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
-				message = "Preflight%s: GPS Speed Accuracy too low";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_speed_acc_too_low"),
-							    log_level, "GPS Speed Accuracy too low");
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
-				message = "Preflight%s: GPS Horizontal Pos Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
-							    log_level, "GPS Horizontal Position Drift too high");
+		/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
+		* for a short time interval after takeoff.
+		* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
+		* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
+		* if this does not fix the issue we need to stop using a position controlled
+		* mode to prevent flyaway crashes.
+		*/
+		if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
-				message = "Preflight%s: GPS Vertical Pos Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
-							    log_level, "GPS Vertical Position Drift too high");
+			const hrt_abstime now = hrt_absolute_time();
 
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
-				message = "Preflight%s: GPS Hor Speed Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
-							    log_level, "GPS Horizontal Speed Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
-				message = "Preflight%s: GPS Vert Speed Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
-							    log_level, "GPS Vertical Speed Drift too high");
+			if (!context.isArmed()) {
+				_nav_test_failed = false;
+				_nav_test_passed = false;
 
 			} else {
-				if (!ekf_gps_fusion) {
-					// Likely cause unknown
-					message = "Preflight%s: Estimator not using GPS";
-					/* EVENT
-					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-								    events::ID("check_estimator_gps_not_fusing"),
-								    log_level, "Estimator not using GPS");
+				if (!_nav_test_passed) {
+					// Both test ratios need to pass/fail together to change the nav test status
+					const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
+								     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
 
-				} else {
-					// if we land here there was a new flag added and the code not updated. Show a generic message.
-					message = "Preflight%s: Poor GPS Quality";
-					/* EVENT
-					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-								    events::ID("check_estimator_gps_generic"),
-								    log_level, "Poor GPS Quality");
-				}
-			}
+					const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
 
-			if (message && reporter.mavlink_log_pub()) {
-				if (!_param_com_arm_wo_gps.get()) {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+					if (innovation_pass) {
+						_time_last_innov_pass = now;
 
-				} else {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+						// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
+						const bool sufficient_time = context.status().takeoff_time != 0
+									     && now - context.status().takeoff_time > 30_s;
+						const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+
+						// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
+						if (now - _time_last_innov_fail > 10_s && (sufficient_time || sufficient_speed)) {
+							_nav_test_passed = true;
+							_nav_test_failed = false;
+						}
+
+					} else if (innovation_fail) {
+						_time_last_innov_fail = now;
+
+						if (now - _time_last_innov_pass > 2_s) {
+							// if the innovation test has failed continuously, declare the nav as failed
+							_nav_test_failed = true;
+							/* EVENT
+							 * @description
+							 * Land and recalibrate the sensors.
+							 */
+							reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+										    events::ID("check_estimator_nav_failure"),
+										    events::Log::Emergency, "Navigation failure");
+
+							if (reporter.mavlink_log_pub()) {
+								mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
+							}
+						}
+					}
 				}
 			}
 		}
-	}
 
+		// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+		if (_param_sys_has_gps.get()) {
+			const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+			const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
+
+			if (ekf_gps_fusion) {
+				reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
+			}
+
+			if (ekf_gps_check_fail) {
+				NavModes required_groups_gps = required_groups;
+				events::Log log_level = events::Log::Error;
+
+				if (_param_com_arm_wo_gps.get()) {
+					required_groups_gps = NavModes::None; // optional
+					log_level = events::Log::Warning;
+				}
+
+				// Only report the first failure to avoid spamming
+				const char *message = nullptr;
+
+				if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
+					message = "Preflight%s: GPS fix too low";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_fix_too_low"),
+								    log_level, "GPS fix too low");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
+					message = "Preflight%s: not enough GPS Satellites";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_num_sats_too_low"),
+								    log_level, "Not enough GPS Satellites");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
+					message = "Preflight%s: GPS PDOP too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_pdop_too_high"),
+								    log_level, "GPS PDOP too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
+					message = "Preflight%s: GPS Horizontal Pos Error too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_pos_err_too_high"),
+								    log_level, "GPS Horizontal Position Error too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
+					message = "Preflight%s: GPS Vertical Pos Error too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_pos_err_too_high"),
+								    log_level, "GPS Vertical Position Error too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
+					message = "Preflight%s: GPS Speed Accuracy too low";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_speed_acc_too_low"),
+								    log_level, "GPS Speed Accuracy too low");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
+					message = "Preflight%s: GPS Horizontal Pos Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
+								    log_level, "GPS Horizontal Position Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
+					message = "Preflight%s: GPS Vertical Pos Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
+								    log_level, "GPS Vertical Position Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
+					message = "Preflight%s: GPS Hor Speed Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
+								    log_level, "GPS Horizontal Speed Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
+					message = "Preflight%s: GPS Vert Speed Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
+								    log_level, "GPS Vertical Speed Drift too high");
+
+				} else {
+					if (!ekf_gps_fusion) {
+						// Likely cause unknown
+						message = "Preflight%s: Estimator not using GPS";
+						/* EVENT
+						 */
+						reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+									    events::ID("check_estimator_gps_not_fusing"),
+									    log_level, "Estimator not using GPS");
+
+					} else {
+						// if we land here there was a new flag added and the code not updated. Show a generic message.
+						message = "Preflight%s: Poor GPS Quality";
+						/* EVENT
+						 */
+						reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+									    events::ID("check_estimator_gps_generic"),
+									    log_level, "Poor GPS Quality");
+					}
+				}
+
+				if (message && reporter.mavlink_log_pub()) {
+					if (!_param_com_arm_wo_gps.get()) {
+						mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+
+					} else {
+						mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+					}
+				}
+			}
+		}
+
+	}
 }
 
 void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups)
@@ -528,112 +525,113 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter,
 }
 
 void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &reporter,
-		const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos)
+		const estimator_status_flags_s &estimator_status_flags, NavModes required_groups)
 {
-	estimator_status_flags_s estimator_status_flags;
+	bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
+			      || estimator_status_flags.cs_inertial_dead_reckoning;
 
-	if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
+	if (!dead_reckoning) {
+		// position requirements (update if not dead reckoning)
+		bool gps             = estimator_status_flags.cs_gps;
+		bool optical_flow    = estimator_status_flags.cs_opt_flow;
+		bool vision_position = estimator_status_flags.cs_ev_pos;
 
-		bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
-				      || estimator_status_flags.cs_inertial_dead_reckoning;
+		_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
+	}
 
-		if (!dead_reckoning) {
-			// position requirements (update if not dead reckoning)
-			bool gps             = estimator_status_flags.cs_gps;
-			bool optical_flow    = estimator_status_flags.cs_opt_flow;
-			bool vision_position = estimator_status_flags.cs_ev_pos;
+	// Check for a magnetomer fault and notify the user
+	if (estimator_status_flags.cs_mag_fault) {
+		/* EVENT
+		 * @description
+		 * Land and calibrate the compass.
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_fault"),
+					    events::Log::Critical, "Stopping compass use");
 
-			_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
-		}
-
-		// Check for a magnetomer fault and notify the user
-		if (estimator_status_flags.cs_mag_fault) {
-			/* EVENT
-			 * @description
-			 * Land and calibrate the compass.
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-						    events::ID("check_estimator_mag_fault"),
-						    events::Log::Critical, "Stopping compass use");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
-			}
-		}
-
-		if (estimator_status_flags.cs_gps_yaw_fault) {
-			/* EVENT
-			 * @description
-			 * Land now
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-						    events::ID("check_estimator_gnss_fault"),
-						    events::Log::Critical, "GNSS heading not reliable");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
 		}
 	}
 
-	const hrt_abstime now = hrt_absolute_time();
+	if (estimator_status_flags.cs_gps_yaw_fault) {
+		/* EVENT
+		 * @description
+		 * Land now
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_gnss_fault"),
+					    events::Log::Critical, "GNSS heading not reliable");
 
-	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-	* for a short time interval after takeoff.
-	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-	* if this does not fix the issue we need to stop using a position controlled
-	* mode to prevent flyaway crashes.
-	*/
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
+		}
+	}
 
-	if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	if ((_param_com_arm_mag_str.get() >= 1) && estimator_status_flags.cs_mag_field_disturbed) {
+		NavModes required_groups_mag = required_groups;
 
-		if (!context.isArmed()) {
-			_nav_test_failed = false;
-			_nav_test_passed = false;
+		if (_param_com_arm_mag_str.get() != 1) {
+			required_groups_mag = NavModes::None; // optional
+		}
 
-		} else {
-			if (!_nav_test_passed) {
-				// Both test ratios need to pass/fail together to change the nav test status
-				const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
-							     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_interference"),
+					    events::Log::Warning, "Strong magnetic interference detected");
 
-				const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference detected");
+		}
+	}
 
-				if (innovation_pass) {
-					_time_last_innov_pass = now;
+	if (estimator_status_flags.pre_flt_fail_innov_heading) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_heading_not_stable"),
+					    events::Log::Error, "Heading estimate not stable");
 
-					// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-					const bool sufficient_time = context.status().takeoff_time != 0
-								     && now - context.status().takeoff_time > 30_s;
-					const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
+		}
 
-					// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-					if (now - _time_last_innov_fail > 10_s && (sufficient_time || sufficient_speed)) {
-						_nav_test_passed = true;
-						_nav_test_failed = false;
-					}
+	} else if (estimator_status_flags.pre_flt_fail_innov_vel_horiz) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hor_vel_not_stable"),
+					    events::Log::Error, "Horizontal velocity estimate not stable");
 
-				} else if (innovation_fail) {
-					_time_last_innov_fail = now;
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity estimate not stable");
+		}
 
-					if (now - _time_last_innov_pass > 2_s) {
-						// if the innovation test has failed continuously, declare the nav as failed
-						_nav_test_failed = true;
-						/* EVENT
-						 * @description
-						 * Land and recalibrate the sensors.
-						 */
-						reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-									    events::ID("check_estimator_nav_failure"),
-									    events::Log::Emergency, "Navigation failure");
+	} else if (estimator_status_flags.pre_flt_fail_innov_vel_vert) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_vert_vel_not_stable"),
+					    events::Log::Error, "Vertical velocity estimate not stable");
 
-						if (reporter.mavlink_log_pub()) {
-							mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
-						}
-					}
-				}
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity estimate not stable");
+		}
+
+	} else if (estimator_status_flags.pre_flt_fail_innov_height) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hgt_not_stable"),
+					    events::Log::Error, "Height estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
 		}
 	}
 }

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -59,11 +59,11 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
-	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
+	void checkEstimatorStatus(const Context &context, Report &reporter, const vehicle_local_position_s &lpos,
 				  NavModes required_groups);
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
-	void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
-				       const vehicle_local_position_s &lpos);
+	void checkEstimatorStatusFlags(const Context &context, Report &reporter,
+				       const estimator_status_flags_s &estimator_status_flags, NavModes required_groups);
 
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void gpsNoLongerValid(const Context &context, Report &reporter) const;

src/modules/ekf2/EKF2.cpp

@@ -1351,12 +1351,6 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 
 	status.time_slip = _last_time_slip_us * 1e-6f;
 
-	status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
-	status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
-	status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
-	status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
-	status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
-
 	status.accel_device_id = _device_id_accel;
 	status.baro_device_id = _device_id_baro;
 	status.gyro_device_id = _device_id_gyro;
@@ -1464,6 +1458,11 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_optflow_x                = _ekf.innov_check_fail_status_flags().reject_optflow_X;
 		status_flags.reject_optflow_y                = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
 
+		status_flags.pre_flt_fail_innov_heading   = _preflt_checker.hasHeadingFailed();
+		status_flags.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
+		status_flags.pre_flt_fail_innov_vel_vert  = _preflt_checker.hasVertVelFailed();
+		status_flags.pre_flt_fail_innov_height    = _preflt_checker.hasHeightFailed();
+
 		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 

src/modules/logger/logger.cpp

@@ -583,6 +583,49 @@ void Logger::run()
 
 	uORB::Subscription parameter_update_sub(ORB_ID(parameter_update));
 
+	if (!initialize_topics()) {
+		return;
+	}
+
+	//all topics added. Get required message buffer size
+	int max_msg_size = 0;
+
+	for (int sub = 0; sub < _num_subscriptions; ++sub) {
+		//use o_size, because that's what orb_copy will use
+		if (_subscriptions[sub].get_topic()->o_size > max_msg_size) {
+			max_msg_size = _subscriptions[sub].get_topic()->o_size;
+		}
+	}
+
+	if (_event_subscription.get_topic()->o_size > max_msg_size) {
+		max_msg_size = _event_subscription.get_topic()->o_size;
+	}
+
+	max_msg_size += sizeof(ulog_message_data_s);
+
+	if (sizeof(ulog_message_logging_s) > (size_t)max_msg_size) {
+		max_msg_size = sizeof(ulog_message_logging_s);
+	}
+
+	if (_polling_topic_meta && _polling_topic_meta->o_size > max_msg_size) {
+		max_msg_size = _polling_topic_meta->o_size;
+	}
+
+	if (max_msg_size > _msg_buffer_len) {
+		if (_msg_buffer) {
+			delete[](_msg_buffer);
+		}
+
+		_msg_buffer_len = max_msg_size;
+		_msg_buffer = new uint8_t[_msg_buffer_len];
+
+		if (!_msg_buffer) {
+			PX4_ERR("failed to alloc message buffer");
+			return;
+		}
+	}
+
+
 	if (!_writer.init()) {
 		PX4_ERR("writer init failed");
 		return;
@@ -597,7 +640,7 @@ void Logger::run()
 	const bool disable_boot_logging = get_disable_boot_logging();
 
 	if ((_log_mode == LogMode::boot_until_disarm || _log_mode == LogMode::boot_until_shutdown) && !disable_boot_logging) {
-		_start_immediately = true;
+		start_log_file(LogType::Full);
 	}
 
 	/* init the update timer */
@@ -643,56 +686,9 @@ void Logger::run()
 
 	while (!should_exit()) {
 		// Start/stop logging (depending on logging mode, by default when arming/disarming)
-		if (should_start_logging()) {
-
-			if (!initialize_topics()) {
-				return;
-			}
-
-			//all topics added. Get required message buffer size
-			int max_msg_size = 0;
-
-			for (int sub = 0; sub < _num_subscriptions; ++sub) {
-				//use o_size, because that's what orb_copy will use
-				if (_subscriptions[sub].get_topic()->o_size > max_msg_size) {
-					max_msg_size = _subscriptions[sub].get_topic()->o_size;
-				}
-			}
-
-			if (_event_subscription.get_topic()->o_size > max_msg_size) {
-				max_msg_size = _event_subscription.get_topic()->o_size;
-			}
-
-			max_msg_size += sizeof(ulog_message_data_s);
-
-			if (sizeof(ulog_message_logging_s) > (size_t)max_msg_size) {
-				max_msg_size = sizeof(ulog_message_logging_s);
-			}
-
-			if (_polling_topic_meta && _polling_topic_meta->o_size > max_msg_size) {
-				max_msg_size = _polling_topic_meta->o_size;
-			}
-
-			if (max_msg_size > _msg_buffer_len) {
-				if (_msg_buffer) {
-					delete[](_msg_buffer);
-				}
-
-				_msg_buffer_len = max_msg_size;
-				_msg_buffer = new uint8_t[_msg_buffer_len];
-
-				if (!_msg_buffer) {
-					PX4_ERR("failed to alloc message buffer");
-					return;
-				}
-			}
-
-			start_log_file(LogType::Full);
-
-			if ((MissionLogType)_param_sdlog_mission.get() != MissionLogType::Disabled) {
-				start_log_file(LogType::Mission);
-			}
+		const bool logging_started = start_stop_logging();
 
+		if (logging_started) {
 #ifdef DBGPRINT
 			timer_start = hrt_absolute_time();
 			total_bytes = 0;
@@ -710,7 +706,7 @@ void Logger::run()
 
 		const hrt_abstime loop_time = hrt_absolute_time();
 
-		if (_msg_buffer && _writer.is_started(LogType::Full)) { // mission log only runs when full log is also started
+		if (_writer.is_started(LogType::Full)) { // mission log only runs when full log is also started
 
 			if (!was_started) {
 				adjust_subscription_updates();
@@ -870,7 +866,7 @@ void Logger::run()
 			// - we avoid subscribing to many topics at once, when logging starts
 			// - we'll get the data immediately once we start logging (no need to wait for the next subscribe timeout)
 			if (next_subscribe_topic_index != -1) {
-				if (_subscriptions && !_subscriptions[next_subscribe_topic_index].valid()) {
+				if (!_subscriptions[next_subscribe_topic_index].valid()) {
 					_subscriptions[next_subscribe_topic_index].subscribe();
 				}
 
@@ -1095,18 +1091,12 @@ bool Logger::get_disable_boot_logging()
 	return false;
 }
 
-bool Logger::should_start_logging()
+bool Logger::start_stop_logging()
 {
 	bool updated = false;
 	bool desired_state = false;
 
-	if (_start_immediately) {
-		desired_state = true;
-		updated = true;
-
-		_start_immediately = false;
-
-	} else if (_log_mode == LogMode::rc_aux1) {
+	if (_log_mode == LogMode::rc_aux1) {
 		// aux1-based logging
 		manual_control_setpoint_s manual_control_setpoint;
 
@@ -1127,17 +1117,11 @@ bool Logger::should_start_logging()
 		}
 	}
 
-	const bool delayed_start = (_param_sdlog_delay_s.get() > 0);
-
-	if (delayed_start && (_start_requested_time != HRT_ABSTIME_INVALID)) {
-		desired_state = true;
-		updated = true;
-	}
-
 	desired_state = desired_state || _manually_logging_override;
 
 	// only start/stop if this is a state transition
 	if (updated && _prev_state != desired_state) {
+		_prev_state = desired_state;
 
 		if (desired_state) {
 			if (_should_stop_file_log) { // happens on quick stop/start toggling
@@ -1145,24 +1129,14 @@ bool Logger::should_start_logging()
 				stop_log_file(LogType::Full);
 			}
 
-			if (delayed_start) {
-				if (_start_requested_time == HRT_ABSTIME_INVALID) {
-					_start_requested_time = hrt_absolute_time();
+			start_log_file(LogType::Full);
 
-					PX4_INFO("Delayed start in %" PRIi32 " seconds", _param_sdlog_delay_s.get());
-					return false;
-
-				} else if (hrt_elapsed_time(&_start_requested_time) > _param_sdlog_delay_s.get() * 1_s) {
-					_start_requested_time = HRT_ABSTIME_INVALID; // reset
-					_prev_state = true;
-					return true;
-				}
-
-			} else {
-				_prev_state = true;
-				return true;
+			if ((MissionLogType)_param_sdlog_mission.get() != MissionLogType::Disabled) {
+				start_log_file(LogType::Mission);
 			}
 
+			return true;
+
 		} else {
 			// delayed stop: we measure the process loads and then stop
 			initialize_load_output(PrintLoadReason::Postflight);
@@ -1171,8 +1145,6 @@ bool Logger::should_start_logging()
 			if ((MissionLogType)_param_sdlog_mission.get() != MissionLogType::Disabled) {
 				stop_log_file(LogType::Mission);
 			}
-
-			_prev_state = false;
 		}
 	}
 

src/modules/logger/logger.h

@@ -292,7 +292,7 @@ private:
 	 * configured params.
 	 * @return true if log started
 	 */
-	bool should_start_logging();
+	bool start_stop_logging();
 
 	void handle_vehicle_command_update();
 	void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result);
@@ -330,9 +330,6 @@ private:
 
 	bool						_prev_state{false}; ///< previous state depending on logging mode (arming or aux1 state)
 	bool						_manually_logging_override{false};
-	bool						_start_immediately{false};
-
-	hrt_abstime                                     _start_requested_time{HRT_ABSTIME_INVALID};
 
 	Statistics					_statistics[(int)LogType::Count];
 	hrt_abstime					_last_sync_time{0}; ///< last time a sync msg was sent
@@ -383,8 +380,7 @@ private:
 		(ParamInt<px4::params::SDLOG_PROFILE>) _param_sdlog_profile,
 		(ParamInt<px4::params::SDLOG_MISSION>) _param_sdlog_mission,
 		(ParamBool<px4::params::SDLOG_BOOT_BAT>) _param_sdlog_boot_bat,
-		(ParamBool<px4::params::SDLOG_UUID>) _param_sdlog_uuid,
-		(ParamInt<px4::params::SDLOG_DELAY_S>) _param_sdlog_delay_s
+		(ParamBool<px4::params::SDLOG_UUID>) _param_sdlog_uuid
 #if defined(PX4_CRYPTO)
 		, (ParamInt<px4::params::SDLOG_ALGORITHM>) _param_sdlog_crypto_algorithm,
 		(ParamInt<px4::params::SDLOG_KEY>) _param_sdlog_crypto_key,

src/modules/logger/params.c

@@ -176,17 +176,6 @@ PARAM_DEFINE_INT32(SDLOG_DIRS_MAX, 0);
  */
 PARAM_DEFINE_INT32(SDLOG_UUID, 1);
 
-/**
- * Start delay
- *
- * Start delay in seconds, only enabled if > 0.
- *
- * @unit s
- * @min 0
- * @group SD Logging
- */
-PARAM_DEFINE_INT32(SDLOG_DELAY_S, 0);
-
 /**
  * Logfile Encryption algorithm
  *