Commander: start pulling arming related parts into separate folder

* PreFlightCheck: remove unused reportFailures flag * Commander: pull all pre flight checks together on the PreFlightCheck class * PreFlightCheck: separate checks into their own files
2026-10-22 10:23:34 +08:00 · 2019-11-05 11:25:59 -05:00
parent 08a27492b4
commit 1a79f75f94
32 changed files with 1838 additions and 1281 deletions
@@ -0,0 +1,287 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017  Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include "ArmAuthorization.h"
+
+#include <string.h>
+#include <unistd.h>
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/px4_config.h>
+
+#include <systemlib/mavlink_log.h>
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+static orb_advert_t *mavlink_log_pub;
+static int command_ack_sub = -1;
+
+static param_t param_arm_parameters;
+
+static hrt_abstime auth_timeout;
+
+static enum {
+	ARM_AUTH_IDLE = 0,
+	ARM_AUTH_WAITING_AUTH,
+	ARM_AUTH_WAITING_AUTH_WITH_ACK,
+	ARM_AUTH_MISSION_APPROVED
+} state = ARM_AUTH_IDLE;
+
+union {
+	struct {
+		uint8_t authorizer_system_id;
+		union {
+			uint16_t auth_method_arm_timeout_msec;
+			uint16_t auth_method_two_arm_timeout_msec;
+		} auth_method_param;
+		uint8_t authentication_method;
+	} __attribute__((packed)) struct_value;
+	int32_t param_value;
+} arm_parameters;
+
+static uint8_t *system_id;
+
+static uint8_t _auth_method_arm_req_check();
+static uint8_t _auth_method_two_arm_check();
+
+static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
+	_auth_method_arm_req_check,
+	_auth_method_two_arm_check,
+};
+
+static void arm_auth_request_msg_send()
+{
+	vehicle_command_s vcmd{};
+	vcmd.timestamp = hrt_absolute_time();
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
+	vcmd.target_system = arm_parameters.struct_value.authorizer_system_id;
+
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	vcmd_pub.publish(vcmd);
+}
+
+static uint8_t _auth_method_arm_req_check()
+{
+	switch (state) {
+	case ARM_AUTH_IDLE:
+		/* no authentication in process? handle bellow */
+		break;
+
+	case ARM_AUTH_MISSION_APPROVED:
+		return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+	default:
+		return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+	}
+
+	/* handling ARM_AUTH_IDLE */
+	arm_auth_request_msg_send();
+
+	hrt_abstime now = hrt_absolute_time();
+	auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000);
+	state = ARM_AUTH_WAITING_AUTH;
+
+	while (now < auth_timeout) {
+		arm_auth_update(now);
+
+		if (state != ARM_AUTH_WAITING_AUTH && state != ARM_AUTH_WAITING_AUTH_WITH_ACK) {
+			break;
+		}
+
+		/* 0.5ms */
+		px4_usleep(500);
+		now = hrt_absolute_time();
+	}
+
+	switch (state) {
+	case ARM_AUTH_WAITING_AUTH:
+	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
+		state = ARM_AUTH_IDLE;
+		mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
+		break;
+
+	default:
+		break;
+	}
+
+	return state == ARM_AUTH_MISSION_APPROVED ?
+	       vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+}
+
+static uint8_t _auth_method_two_arm_check()
+{
+	switch (state) {
+	case ARM_AUTH_IDLE:
+		/* no authentication in process? handle bellow */
+		break;
+
+	case ARM_AUTH_MISSION_APPROVED:
+		return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+	case ARM_AUTH_WAITING_AUTH:
+	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
+		return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+
+	default:
+		return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+	}
+
+	/* handling ARM_AUTH_IDLE */
+	arm_auth_request_msg_send();
+
+	hrt_abstime now = hrt_absolute_time();
+	auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000);
+	state = ARM_AUTH_WAITING_AUTH;
+
+	mavlink_log_critical(mavlink_log_pub, "Arm auth: Requesting authorization...");
+
+	return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+}
+
+uint8_t arm_auth_check()
+{
+	if (arm_parameters.struct_value.authentication_method < ARM_AUTH_METHOD_LAST) {
+		return arm_check_method[arm_parameters.struct_value.authentication_method]();
+	}
+
+	return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+}
+
+void arm_auth_update(hrt_abstime now, bool param_update)
+{
+	if (param_update) {
+		param_get(param_arm_parameters, &arm_parameters.param_value);
+	}
+
+	switch (state) {
+	case ARM_AUTH_WAITING_AUTH:
+	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
+		/* handle bellow */
+		break;
+
+	case ARM_AUTH_MISSION_APPROVED:
+		if (now > auth_timeout) {
+			state = ARM_AUTH_IDLE;
+		}
+
+		return;
+
+	case ARM_AUTH_IDLE:
+	default:
+		return;
+	}
+
+	/*
+	 * handling ARM_AUTH_WAITING_AUTH, ARM_AUTH_WAITING_AUTH_WITH_ACK
+	 */
+	vehicle_command_ack_s command_ack;
+	bool updated = false;
+
+	orb_check(command_ack_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
+	}
+
+	if (updated
+	    && command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
+	    && command_ack.target_system == *system_id) {
+		switch (command_ack.result) {
+		case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
+			state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
+			break;
+
+		case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
+			mavlink_log_critical(mavlink_log_pub,
+					     "Arm auth: Authorized for the next %u seconds",
+					     command_ack.result_param2);
+			state = ARM_AUTH_MISSION_APPROVED;
+			auth_timeout = now + (command_ack.result_param2 * 1000000);
+			return;
+
+		case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
+			mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
+			state = ARM_AUTH_IDLE;
+			return;
+
+		case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
+		default:
+			switch (command_ack.result_param1) {
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
+				/* Authorizer will send reason to ground station */
+				break;
+
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %i have a invalid value", command_ack.result_param2);
+				break;
+
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
+				break;
+
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use");
+				break;
+
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
+				break;
+
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
+			default:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
+			}
+
+			state = ARM_AUTH_IDLE;
+			return;
+		}
+	}
+
+	if (now > auth_timeout) {
+		mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
+		state = ARM_AUTH_IDLE;
+	}
+}
+
+void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
+{
+	system_id = sys_id;
+	param_arm_parameters = param_find("COM_ARM_AUTH");
+	command_ack_sub = orb_subscribe(ORB_ID(vehicle_command_ack));
+	mavlink_log_pub = mav_log_pub;
+}
+
+enum arm_auth_methods arm_auth_method_get()
+{
+	return (enum arm_auth_methods) arm_parameters.struct_value.authentication_method;
+}
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017  Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+
+enum arm_auth_methods {
+	ARM_AUTH_METHOD_ARM_REQ = 0,
+	ARM_AUTH_METHOD_TWO_ARM_REQ,
+	ARM_AUTH_METHOD_LAST
+};
+
+void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *system_id);
+void arm_auth_update(hrt_abstime now, bool param_update = true);
+uint8_t arm_auth_check();
+enum arm_auth_methods arm_auth_method_get();
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(ArmAuthorization
+	ArmAuthorization.cpp
+)
+target_include_directories(ArmAuthorization
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_subdirectory(PreFlightCheck)
+add_subdirectory(ArmAuthorization)
+add_subdirectory(HealthFlags)
@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(HealthFlags
+	HealthFlags.cpp
+)
+target_include_directories(HealthFlags PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file HealthFlags.cpp
+ *
+ * Contains helper functions to efficiently set the system health flags from commander and preflight check.
+ *
+ * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
+ */
+
+#include "HealthFlags.h"
+
+void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
+{
+	PX4_DEBUG("set_health_flags: Type %llu pres=%u enabl=%u ok=%u", subsystem_type, present, enabled, ok);
+
+	if (present) {
+		status.onboard_control_sensors_present |= (uint32_t)subsystem_type;
+
+	} else {
+		status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
+	}
+
+	if (enabled) {
+		status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
+
+	} else {
+		status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
+	}
+
+	if (ok) {
+		status.onboard_control_sensors_health |= (uint32_t)subsystem_type;
+
+	} else {
+		status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
+	}
+}
+
+void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status)
+{
+	set_health_flags(subsystem_type, present, status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy,
+			 status);
+}
+
+void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status)
+{
+	set_health_flags(subsystem_type, status.onboard_control_sensors_present & (uint32_t)subsystem_type,
+			 status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, status);
+}
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file HealthFlags.h
+ *
+ * Contains helper functions to efficiently set the system health flags from commander and preflight check.
+ *
+ * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
+ */
+
+#pragma once
+
+#include <px4_platform_common/log.h>
+#include <uORB/topics/vehicle_status.h>
+
+void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
+void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
+void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status);
@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(PreFlightCheck
+	PreFlightCheck.cpp
+	checks/preArmCheck.cpp
+	checks/magnetometerCheck.cpp
+	checks/magConsistencyCheck.cpp
+	checks/accelerometerCheck.cpp
+	checks/gyroCheck.cpp
+	checks/baroCheck.cpp
+	checks/imuConsistencyCheck.cpp
+	checks/airspeedCheck.cpp
+	checks/rcCalibrationCheck.cpp
+	checks/powerCheck.cpp
+	checks/ekf2Check.cpp
+	checks/failureDetectorCheck.cpp
+)
+target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags)
@@ -0,0 +1,373 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PreFlightCheck.cpp
+ */
+
+#include "PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+static constexpr unsigned max_mandatory_gyro_count = 1;
+static constexpr unsigned max_optional_gyro_count = 3;
+static constexpr unsigned max_mandatory_accel_count = 1;
+static constexpr unsigned max_optional_accel_count = 3;
+static constexpr unsigned max_mandatory_mag_count = 1;
+static constexpr unsigned max_optional_mag_count = 4;
+static constexpr unsigned max_mandatory_baro_count = 1;
+static constexpr unsigned max_optional_baro_count = 1;
+
+bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+				    vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
+				    const hrt_abstime &time_since_boot)
+{
+	if (time_since_boot < 2_s) {
+		// the airspeed driver filter doesn't deliver the actual value yet
+		reportFailures = false;
+	}
+
+	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
+	bool checkSensors = !hil_enabled;
+	const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
+	const bool checkDynamic = !hil_enabled;
+	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
+	const bool checkFailureDetector = true;
+
+	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.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
+		checkAirspeed = true;
+	}
+
+	reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
+			  && !status_flags.condition_calibration_enabled);
+
+	bool failed = false;
+
+	/* ---- MAG ---- */
+	if (checkSensors) {
+		bool prime_found = false;
+
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
+		int32_t sys_has_mag = 1;
+		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
+
+		bool mag_fail_reported = false;
+
+		/* check all sensors individually, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_mag_count; i++) {
+			const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
+			const bool report_fail = (reportFailures && !failed && !mag_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					mag_fail_reported = true;
+				}
+			}
+		}
+
+		if (sys_has_mag == 1) {
+			/* check if the primary device is present */
+			if (!prime_found) {
+				if (reportFailures && !failed) {
+					mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
+				}
+
+				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
+				failed = true;
+			}
+
+			/* mag consistency checks (need to be performed after the individual checks) */
+			if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- ACCEL ---- */
+	if (checkSensors) {
+		bool prime_found = false;
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
+		bool accel_fail_reported = false;
+
+		/* check all sensors individually, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_accel_count; i++) {
+			const bool required = (i < max_mandatory_accel_count);
+			const bool report_fail = (reportFailures && !failed && !accel_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
+
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					accel_fail_reported = true;
+				}
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			if (reportFailures && !failed) {
+				mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
+			}
+
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
+			failed = true;
+		}
+	}
+
+	/* ---- GYRO ---- */
+	if (checkSensors) {
+		bool prime_found = false;
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
+		bool gyro_fail_reported = false;
+
+		/* check all sensors individually, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
+			const bool required = (i < max_mandatory_gyro_count);
+			const bool report_fail = (reportFailures && !failed && !gyro_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					gyro_fail_reported = true;
+				}
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			if (reportFailures && !failed) {
+				mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
+			}
+
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
+			failed = true;
+		}
+	}
+
+	/* ---- BARO ---- */
+	if (checkSensors) {
+		//bool prime_found = false;
+
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
+		int32_t sys_has_baro = 1;
+		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
+
+		bool baro_fail_reported = false;
+
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_baro_count; i++) {
+			const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
+			const bool report_fail = (reportFailures && !failed && !baro_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					//prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					baro_fail_reported = true;
+				}
+			}
+		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present
+		// if (false) {
+		// 	if (reportFailures && !failed) {
+		// 		mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
+		// 	}
+
+		// 	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status);
+		// 	failed = true;
+		// }
+	}
+
+	/* ---- IMU CONSISTENCY ---- */
+	// To be performed after the individual sensor checks have completed
+	if (checkSensors) {
+		if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
+			failed = true;
+		}
+	}
+
+	/* ---- AIRSPEED ---- */
+	if (checkAirspeed) {
+		int32_t optional = 0;
+		param_get(param_find("FW_ARSP_MODE"), &optional);
+
+		if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
+			failed = true;
+		}
+	}
+
+	/* ---- RC CALIBRATION ---- */
+	if (checkRC) {
+		if (rcCalibrationCheck(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) {
+			if (reportFailures) {
+				mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
+			}
+
+			failed = true;
+
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status);
+			status_flags.rc_calibration_valid = false;
+
+		} else {
+			// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
+			status_flags.rc_calibration_valid = true;
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
+					 !status.rc_signal_lost, status);
+		}
+	}
+
+	/* ---- SYSTEM POWER ---- */
+	if (checkPower) {
+		if (!powerCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
+			failed = true;
+		}
+	}
+
+	/* ---- Navigation EKF ---- */
+	// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
+	int32_t estimator_type = -1;
+
+	if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) {
+		param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
+
+	} else {
+		// EKF2 is currently the only supported option for FW & VTOL
+		estimator_type = 2;
+	}
+
+	if (estimator_type == 2) {
+		// don't report ekf failures for the first 10 seconds to allow time for the filter to start
+		bool report_ekf_fail = (time_since_boot > 10_s);
+
+		if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
+			failed = true;
+		}
+	}
+
+	/* ---- Failure Detector ---- */
+	if (checkFailureDetector) {
+		if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
+			failed = true;
+		}
+	}
+
+	/* Report status */
+	return !failed;
+}
+
+bool PreFlightCheck::check_calibration(const char *param_template, const int32_t device_id)
+{
+	bool calibration_found = false;
+
+	char s[20];
+	int instance = 0;
+
+	/* old style transition: check param values */
+	while (!calibration_found) {
+		sprintf(s, param_template, instance);
+		const param_t parm = param_find_no_notification(s);
+
+		/* if the calibration param is not present, abort */
+		if (parm == PARAM_INVALID) {
+			break;
+		}
+
+		/* if param get succeeds */
+		int32_t calibration_devid = -1;
+
+		if (param_get(parm, &calibration_devid) == PX4_OK) {
+
+			/* if the devid matches, exit early */
+			if (device_id == calibration_devid) {
+				calibration_found = true;
+				break;
+			}
+		}
+
+		instance++;
+	}
+
+	return calibration_found;
+}
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PreFlightCheck.hpp
+ *
+ * Check if flight is safely possible
+ * if not prevent it and inform the user.
+ */
+
+#pragma once
+
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+#include <uORB/topics/vehicle_status.h>
+#include <drivers/drv_hrt.h>
+
+class PreFlightCheck
+{
+public:
+	PreFlightCheck() = default;
+	~PreFlightCheck() = default;
+
+	/**
+	* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
+	*
+	* The function won't fail the test if optional sensors are not found, however,
+	* it will fail the test if optional sensors are found but not in working condition.
+	*
+	* @param mavlink_log_pub
+	*   Mavlink output orb handle reference for feedback when a sensor fails
+	* @param checkMag
+	*   true if the magneteometer should be checked
+	* @param checkAcc
+	*   true if the accelerometers should be checked
+	* @param checkGyro
+	*   true if the gyroscopes should be checked
+	* @param checkBaro
+	*   true if the barometer should be checked
+	* @param checkAirspeed
+	*   true if the airspeed sensor should be checked
+	* @param checkRC
+	*   true if the Remote Controller should be checked
+	* @param checkGNSS
+	*   true if the GNSS receiver should be checked
+	* @param checkPower
+	*   true if the system power should be checked
+	**/
+	static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+				   vehicle_status_flags_s &status_flags,
+				   const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot);
+
+	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
+				const safety_s &safety, const uint8_t arm_requirements);
+
+	typedef enum {
+		ARM_REQ_NONE = 0,
+		ARM_REQ_MISSION_BIT = (1 << 0),
+		ARM_REQ_ARM_AUTH_BIT = (1 << 1),
+		ARM_REQ_GPS_BIT = (1 << 2),
+		ARM_REQ_ESCS_CHECK_BIT = (1 << 3)
+	} arm_requirements_t;
+
+private:
+	static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				    const bool optional, int32_t &device_id, const bool report_fail);
+	static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
+	static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				       const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail);
+	static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			      const bool optional, int32_t &device_id, const bool report_fail);
+	static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			      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);
+	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);
+	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
+			      const bool report_fail, const bool enforce_gps_required);
+	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+					 const bool prearm);
+	static bool check_calibration(const char *param_template, const int32_t device_id);
+};
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <math.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+					const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
+	bool calibration_valid = false;
+	bool accel_valid = true;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
+
+		accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
+
+		if (!accel_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance);
+			}
+		}
+
+		device_id = accel.get().device_id;
+
+		calibration_valid = check_calibration("CAL_ACC%u_ID", device_id);
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance);
+			}
+
+		} else {
+
+			if (dynamic) {
+				const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
+								    + accel.get().y * accel.get().y
+								    + accel.get().z * accel.get().z);
+
+				if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+					if (report_fail) {
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
+					}
+
+					/* this is frickin' fatal */
+					accel_valid = false;
+				}
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor #%u missing", instance);
+		}
+	}
+
+	const bool success = calibration_valid && accel_valid;
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status);
+	}
+
+	return success;
+}
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/subsystem_info.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)
+{
+	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();
+
+	if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) {
+		if (report_fail && !optional) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing");
+		}
+
+		present = false;
+		success = false;
+		goto out;
+	}
+
+	/*
+	 * 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.
+	 */
+	if (prearm && fabsf(airspeed.confidence) < 0.95f) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck");
+		}
+
+		present = true;
+		success = false;
+		goto out;
+	}
+
+	/**
+	 * Check if airspeed is higher than 4m/s (accepted max) 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 (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot");
+		}
+
+		present = true;
+		success = false;
+		goto out;
+	}
+
+out:
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
+
+	return success;
+}
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			       const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
+	bool baro_valid = false;
+
+	if (exists) {
+		uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
+
+		baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s);
+
+		if (!baro_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro #%u", instance);
+			}
+		}
+
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor #%u missing", instance);
+		}
+	}
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status);
+	}
+
+	return baro_valid;
+}
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <math.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/subsystem_info.h>
+
+bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
+			       const bool report_fail, const bool enforce_gps_required)
+{
+	bool success = true; // start with a pass and change to a fail if any test fails
+	bool present = true;
+	float test_limit = 1.0f; // pass limit re-used for each test
+
+	bool gps_success = true;
+	bool gps_present = true;
+
+	// Get estimator status data if available and exit with a fail recorded if not
+	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status)};
+	status_sub.update();
+	const estimator_status_s &status = status_sub.get();
+
+	if (status.timestamp == 0) {
+		present = false;
+		goto out;
+	}
+
+	// Check if preflight check performed by estimator has failed
+	if (status.pre_flt_fail_innov_heading ||
+	    status.pre_flt_fail_innov_vel_horiz ||
+	    status.pre_flt_fail_innov_vel_vert ||
+	    status.pre_flt_fail_innov_height) {
+		if (report_fail) {
+			if (status.pre_flt_fail_innov_heading) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_vel_horiz) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_vel_horiz) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_height) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
+			}
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check vertical position innovation test ratio
+	param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
+
+	if (status.hgt_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check velocity innovation test ratio
+	param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
+
+	if (status.vel_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check horizontal position innovation test ratio
+	param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
+
+	if (status.pos_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check magnetometer innovation test ratio
+	param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
+
+	if (status.mag_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check accelerometer delta velocity bias estimates
+	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
+
+	for (uint8_t index = 13; index < 16; index++) {
+		// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+		// adjust test threshold by 3-sigma
+		float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));
+
+		if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
+			}
+
+			success = false;
+			goto out;
+		}
+	}
+
+	// check gyro delta angle bias estimates
+	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
+
+	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
+	    || fabsf(status.states[12]) > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+	if (enforce_gps_required || report_fail) {
+		const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+		const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
+
+		gps_success = ekf_gps_fusion; // default to success if gps data is fused
+
+		if (ekf_gps_check_fail) {
+			if (report_fail) {
+				// Only report the first failure to avoid spamming
+				const char *message = nullptr;
+
+				if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
+					message = "Preflight%s: GPS fix too low";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
+					message = "Preflight%s: not enough GPS Satellites";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) {
+					message = "Preflight%s: GPS GDoP too low";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
+					message = "Preflight%s: GPS Horizontal Pos Error too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
+					message = "Preflight%s: GPS Vertical Pos Error too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
+					message = "Preflight%s: GPS Speed Accuracy too low";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
+					message = "Preflight%s: GPS Horizontal Pos Drift too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
+					message = "Preflight%s: GPS Vertical Pos Drift too high";
+
+				} else if (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";
+
+				} else if (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";
+
+				} else {
+					if (!ekf_gps_fusion) {
+						// Likely cause unknown
+						message = "Preflight%s: Estimator not using GPS";
+						gps_present = false;
+
+					} 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";
+					}
+				}
+
+				if (message) {
+					if (enforce_gps_required) {
+						mavlink_log_critical(mavlink_log_pub, message, " Fail");
+
+					} else {
+						mavlink_log_warning(mavlink_log_pub, message, "");
+					}
+				}
+			}
+
+			gps_success = false;
+
+			if (enforce_gps_required) {
+				success = false;
+				goto out;
+			}
+		}
+	}
+
+out:
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status);
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status);
+
+	return success;
+}
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <systemlib/mavlink_log.h>
+
+bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+		const bool report_fail, const bool prearm)
+{
+	// Ignore failure detector check after arming
+	if (!prearm) {
+		return true;
+	}
+
+	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
+		if (report_fail) {
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
+			}
+		}
+
+		return false;
+	}
+
+	return true;
+}
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			       const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
+	bool calibration_valid = false;
+	bool gyro_valid = false;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
+
+		gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
+
+		if (!gyro_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro #%u", instance);
+			}
+		}
+
+		device_id = gyro.get().device_id;
+
+		calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id);
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u uncalibrated", instance);
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor #%u missing", instance);
+		}
+	}
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status);
+	}
+
+	return calibration_valid && gyro_valid;
+}
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_preflight.h>
+#include <uORB/topics/subsystem_info.h>
+
+bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+		const bool report_status)
+{
+	float test_limit = 1.0f; // pass limit re-used for each test
+
+	// Get sensor_preflight data if available and exit with a fail recorded if not
+	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
+	sensors_sub.update();
+	const sensor_preflight_s &sensors = sensors_sub.get();
+
+	// Use the difference between IMU's to detect a bad calibration.
+	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
+	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
+
+	if (sensors.accel_inconsistency_m_s_s > test_limit) {
+		if (report_status) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal");
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
+		}
+
+		return false;
+
+	} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
+		if (report_status) {
+			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal");
+		}
+	}
+
+	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
+	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
+
+	if (sensors.gyro_inconsistency_rad_s > test_limit) {
+		if (report_status) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal");
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
+		}
+
+		return false;
+
+	} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
+		if (report_status) {
+			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal");
+		}
+	}
+
+	return true;
+}
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/sensor_preflight.h>
+
+// return false if the magnetomer measurements are inconsistent
+bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+		const bool report_status)
+{
+	bool pass = false; // flag for result of checks
+
+	// get the sensor preflight data
+	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
+	sensors_sub.update();
+	const sensor_preflight_s &sensors = sensors_sub.get();
+
+	if (sensors.timestamp == 0) {
+		// can happen if not advertised (yet)
+		pass = true;
+	}
+
+	// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
+	// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
+	int32_t angle_difference_limit_deg;
+	param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg);
+
+	pass = pass || angle_difference_limit_deg < 0; // disabled, pass check
+	pass = pass || sensors.mag_inconsistency_angle < math::radians<float>(angle_difference_limit_deg);
+
+	if (!pass && report_status) {
+		mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent",
+				     static_cast<int>(math::degrees<float>(sensors.mag_inconsistency_angle)));
+		mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate");
+		set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);
+		set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status);
+	}
+
+	return pass;
+}
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				     const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
+	bool calibration_valid = false;
+	bool mag_valid = false;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
+
+		mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s);
+
+		if (!mag_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance);
+			}
+		}
+
+		device_id = magnetometer.get().device_id;
+
+		calibration_valid = check_calibration("CAL_MAG%u_ID", device_id);
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance);
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor #%u missing", instance);
+		}
+	}
+
+	const bool success = calibration_valid && mag_valid;
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
+	}
+
+	return success;
+}
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/system_power.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+				const bool prearm)
+{
+	bool success = true;
+
+	if (!prearm) {
+		// Ignore power check after arming.
+		return true;
+
+	} else {
+		uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
+		system_power_sub.update();
+		const system_power_s &system_power = system_power_sub.get();
+
+		if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
+
+			/* copy avionics voltage */
+			float avionics_power_rail_voltage = system_power.voltage5v_v;
+
+			// avionics rail
+			// Check avionics rail voltages
+			if (avionics_power_rail_voltage < 4.5f) {
+				success = false;
+
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
+							     (double)avionics_power_rail_voltage);
+				}
+
+			} else if (avionics_power_rail_voltage < 4.9f) {
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
+				}
+
+			} else if (avionics_power_rail_voltage > 5.4f) {
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
+				}
+			}
+		}
+	}
+
+	return success;
+}
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <ArmAuthorization.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
+				 const safety_s &safety, const uint8_t arm_requirements)
+{
+	bool prearm_ok = true;
+
+	// USB not connected
+	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
+		mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe");
+
+		prearm_ok = false;
+	}
+
+	// battery and system power status
+	if (!status_flags.circuit_breaker_engaged_power_check) {
+
+		// Fail transition if power is not good
+		if (!status_flags.condition_power_input_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module");
+
+			prearm_ok = false;
+		}
+
+		// main battery level
+		if (!status_flags.condition_battery_healthy) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery");
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: mission
+	if (arm_requirements & ARM_REQ_MISSION_BIT) {
+
+		if (!status_flags.condition_auto_mission_available) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission");
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!status_flags.condition_global_position_valid) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position");
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: global position
+	if (arm_requirements & ARM_REQ_GPS_BIT) {
+
+		if (!status_flags.condition_global_position_valid) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required");
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// safety button
+	if (safety.safety_switch_available && !safety.safety_off) {
+		// Fail transition if we need safety switch press
+		if (prearm_ok) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first");
+		}
+
+		prearm_ok = false;
+	}
+
+	if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
+		if (prearm_ok) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready");
+		}
+
+		prearm_ok = false;
+
+	}
+
+	if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) {
+		if (prearm_ok) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline");
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: authorization
+	// check last, and only if everything else has passed
+	if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) {
+		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+			// feedback provided in arm_auth_check
+			prearm_ok = false;
+		}
+	}
+
+
+	return prearm_ok;
+}
@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <parameters/param.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/input_rc.h>
+
+/**
+ * Maximum deadzone value
+ */
+#define RC_INPUT_MAX_DEADZONE_US	500
+
+/**
+ * Minimum value
+ */
+#define RC_INPUT_LOWEST_MIN_US	500
+
+/**
+ * Maximum value
+ */
+#define RC_INPUT_HIGHEST_MAX_US	2500
+
+int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
+{
+	char nbuf[20];
+	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
+		_parameter_handles_rev, _parameter_handles_dz;
+
+	unsigned map_fail_count = 0;
+
+	const char *rc_map_mandatory[] = {	/*"RC_MAP_MODE_SW",*/
+		/* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
+		nullptr /* end marker */
+	};
+
+	unsigned j = 0;
+
+	/* if VTOL, check transition switch mapping */
+	if (isVTOL) {
+		param_t trans_parm = param_find("RC_MAP_TRANS_SW");
+
+		if (trans_parm == PARAM_INVALID) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC_MAP_TRANS_SW PARAMETER MISSING."); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+			map_fail_count++;
+
+		} else {
+			int32_t transition_switch;
+			param_get(trans_parm, &transition_switch);
+
+			if (transition_switch < 1) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Transition switch RC_MAP_TRANS_SW not set."); }
+
+				map_fail_count++;
+			}
+		}
+	}
+
+	/* first check channel mappings */
+	while (rc_map_mandatory[j] != nullptr) {
+
+		param_t map_parm = param_find(rc_map_mandatory[j]);
+
+		if (map_parm == PARAM_INVALID) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: PARAM %s MISSING.", rc_map_mandatory[j]); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+			map_fail_count++;
+			j++;
+			continue;
+		}
+
+		int32_t mapping;
+		param_get(map_parm, &mapping);
+
+		if (mapping > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: %s >= NUMBER OF CHANNELS.", rc_map_mandatory[j]); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+			map_fail_count++;
+		}
+
+		if (mapping == 0) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: mandatory %s is unmapped.", rc_map_mandatory[j]); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+			map_fail_count++;
+		}
+
+		j++;
+	}
+
+	unsigned total_fail_count = 0;
+	unsigned channels_failed = 0;
+
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		/* should the channel be enabled? */
+		uint8_t count = 0;
+
+		/* initialize values to values failing the check */
+		float param_min = 0.0f;
+		float param_max = 0.0f;
+		float param_trim = 0.0f;
+		float param_rev = 0.0f;
+		float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
+
+		/* min values */
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		_parameter_handles_min = param_find(nbuf);
+		param_get(_parameter_handles_min, &param_min);
+
+		/* trim values */
+		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		_parameter_handles_trim = param_find(nbuf);
+		param_get(_parameter_handles_trim, &param_trim);
+
+		/* max values */
+		sprintf(nbuf, "RC%d_MAX", i + 1);
+		_parameter_handles_max = param_find(nbuf);
+		param_get(_parameter_handles_max, &param_max);
+
+		/* channel reverse */
+		sprintf(nbuf, "RC%d_REV", i + 1);
+		_parameter_handles_rev = param_find(nbuf);
+		param_get(_parameter_handles_rev, &param_rev);
+
+		/* channel deadzone */
+		sprintf(nbuf, "RC%d_DZ", i + 1);
+		_parameter_handles_dz = param_find(nbuf);
+		param_get(_parameter_handles_dz, &param_dz);
+
+		/* assert min..center..max ordering */
+		if (param_min < RC_INPUT_LOWEST_MIN_US) {
+			count++;
+
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MIN < %u.", i + 1, RC_INPUT_LOWEST_MIN_US); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+		}
+
+		if (param_max > RC_INPUT_HIGHEST_MAX_US) {
+			count++;
+
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MAX > %u.", i + 1, RC_INPUT_HIGHEST_MAX_US); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+		}
+
+		if (param_trim < param_min) {
+			count++;
+
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM < MIN (%d/%d).", i + 1, (int)param_trim, (int)param_min); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+		}
+
+		if (param_trim > param_max) {
+			count++;
+
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM > MAX (%d/%d).", i + 1, (int)param_trim, (int)param_max); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+		}
+
+		/* assert deadzone is sane */
+		if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_DZ > %u.", i + 1, RC_INPUT_MAX_DEADZONE_US); }
+
+			/* give system time to flush error message in case there are more */
+			px4_usleep(100000);
+			count++;
+		}
+
+		total_fail_count += count;
+
+		if (count) {
+			channels_failed++;
+		}
+	}
+
+	if (channels_failed) {
+		px4_sleep(2);
+
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.",
+					     total_fail_count,
+					     (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
+		}
+
+		px4_usleep(100000);
+	}
+
+	return total_fail_count + map_fail_count;
+}