Merge pull request '修改px4 ， 新添加自主路径规划模块(offline_task_manager)' (#6) from luogang_branch into master

本提交新增了一个 PX4 内部模块，用于无人机的离线任务托管和自动航线执行。
主要功能：
- 从本地 JSON 文件读取离线规划任务点 (/tmp/offline_tasks.json)
- 基于最近邻算法进行路径规划，支持任务优先级和紧急任务插队
- 电池电量感知与低电量任务过滤（支持模拟和真实电池）
- 自动生成 PX4 mission 并写入 dataman 持久化存储
- 完整状态机：IDLE → 等待home点 → 等待GPS定位 → 加载任务文件
  → 验证任务 → 规划路径 → 写入mission → 启动任务 → 监控执行
- 自动发送解锁命令并切换至 AUTO_MISSION 飞行模式
模块组成：
- offline_task.hpp/cpp: 任务数据结构定义与路径规划器
- cJSON.h/c: 嵌入式 JSON 解析库（最小实现）
- offline_task_parser: JSON 文件读取与校验
- offline_task_manager: 主模块，集成 PX4 uORB、dataman 与参数系统
新增参数：
- OTM_ENABLE: 启用/禁用模块
- OTM_START: 触发任务执行
- OTM_LOW_BATT: 低电量阈值
- OTM_FILE_PATH: 任务文件路径
关键修复：
- 在切换 AUTO_MISSION 模式前先发送 ARM 解锁命令，确保无人机正常起飞
- 使用 haversine 公式计算地球表面两点距离
集成说明：
- 添加到 amovlab_sitl_default 和 px4_sitl_default 构建目标
- 订阅 uORB 主题：mission、vehicle_command、battery_status、
  vehicle_global_position、home_position
- 通过 vehicle_command 主题发送 MAVLink 模式切换命令
- 使用 dataman 存储离线任务 mission items
测试验证：
- SITL 仿真环境验证通过
- 支持最多 32 个离线任务点
- 成功实现任务加载、路径规划、mission 写入和模式切换

src/prometheus_px4/src/modules/airship_att_control/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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_module(
+	MODULE modules__airship_att_control
+	MAIN airship_att_control
+	STACK_MAX 3500
+	COMPILE_FLAGS
+	SRCS
+		airship_att_control_main.cpp
+	DEPENDS
+		px4_work_queue
+	)

src/prometheus_px4/src/modules/airship_att_control/airship_att_control.hpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 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 <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+
+using namespace time_literals;
+
+/**
+ * Airship attitude control app start / stop handling function
+ */
+extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
+
+class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	AirshipAttitudeControl();
+
+	virtual ~AirshipAttitudeControl();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * Check for parameter update and handle it.
+	 */
+	void		parameter_update_poll();
+
+	void		publish_actuator_controls();
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};		/**< parameter updates subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
+
+	struct manual_control_setpoint_s	_manual_control_sp {};	/**< manual control setpoint */
+	struct vehicle_status_s			_vehicle_status {};	/**< vehicle status */
+	struct actuator_controls_s		_actuators {};		/**< actuator controls */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+};

src/prometheus_px4/src/modules/airship_att_control/airship_att_control_main.cpp

@@ -0,0 +1,213 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 airship_att_control_main.cpp
+ * Airship attitude controller.
+ *
+ * @author Anton Erasmus	<anton@flycloudline.com>
+ */
+
+#include "airship_att_control.hpp"
+
+using namespace matrix;
+
+AirshipAttitudeControl::AirshipAttitudeControl() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_actuators_0_pub(ORB_ID(actuator_controls_0)),
+	_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
+{
+}
+
+AirshipAttitudeControl::~AirshipAttitudeControl()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+AirshipAttitudeControl::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_velocity callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+AirshipAttitudeControl::parameter_update_poll()
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+}
+
+void
+AirshipAttitudeControl::publish_actuator_controls()
+{
+	// zero actuators if not armed
+	if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+		for (uint8_t i = 0 ; i < 4 ; i++) {
+			_actuators.control[i] = 0.0f;
+		}
+
+	} else {
+		_actuators.control[0] = 0.0f;
+		_actuators.control[1] = _manual_control_sp.x;
+		_actuators.control[2] = _manual_control_sp.r;
+		_actuators.control[3] = _manual_control_sp.z;
+	}
+
+	// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
+	_actuators.timestamp = hrt_absolute_time();
+
+	_actuators_0_pub.publish(_actuators);
+}
+
+void
+AirshipAttitudeControl::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	/* run controller on gyro changes */
+	vehicle_angular_velocity_s angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+
+		const Vector3f rates{angular_velocity.xyz};
+
+		_actuators.timestamp_sample = angular_velocity.timestamp_sample;
+
+		/* run the rate controller immediately after a gyro update */
+		publish_actuator_controls();
+
+		/* check for updates in manual control topic */
+		if (_manual_control_sp_sub.updated()) {
+			_manual_control_sp_sub.update(&_manual_control_sp);
+		}
+
+		/* check for updates in vehicle status topic */
+		if (_vehicle_status_sub.updated()) {
+			_vehicle_status_sub.update(&_vehicle_status);
+		}
+
+		parameter_update_poll();
+	}
+
+	perf_end(_loop_perf);
+}
+
+int AirshipAttitudeControl::task_spawn(int argc, char *argv[])
+{
+	AirshipAttitudeControl *instance = new AirshipAttitudeControl();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int AirshipAttitudeControl::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	print_message(_actuators);
+
+	return 0;
+}
+
+int AirshipAttitudeControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AirshipAttitudeControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the airship attitude and rate controller. Ideally it would
+take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
+via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
+
+Currently it is feeding the `manual_control_setpoint` topic directly to the actuators.
+
+### Implementation
+To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("airship_att_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int airship_att_control_main(int argc, char *argv[])
+{
+	return AirshipAttitudeControl::main(argc, argv);
+}

src/prometheus_px4/src/modules/airspeed_selector/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2018 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_module(
+	MODULE modules__airspeed_selector
+	MAIN airspeed_selector
+	SRCS
+		airspeed_selector_main.cpp
+	DEPENDS
+		git_ecl
+		ecl_airdata
+		AirspeedValidator
+)

src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -0,0 +1,675 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 <drivers/drv_hrt.h>
+#include <ecl/airdata/WindEstimator.hpp>
+#include <matrix/math.hpp>
+#include <parameters/param.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/airspeed/airspeed.h>
+#include <AirspeedValidator.hpp>
+#include <systemlib/mavlink_log.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/estimator_selector_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_setpoint.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/airspeed_wind.h>
+
+using namespace time_literals;
+
+static constexpr uint32_t SCHEDULE_INTERVAL{100_ms};	/**< The schedule interval in usec (10 Hz) */
+
+using matrix::Dcmf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+
+class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+
+	AirspeedModule();
+
+	~AirspeedModule() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+private:
+
+	void Run() override;
+
+	static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
+	enum airspeed_index {
+		DISABLED_INDEX = -1,
+		GROUND_MINUS_WIND_INDEX,
+		FIRST_SENSOR_INDEX,
+		SECOND_SENSOR_INDEX,
+		THIRD_SENSOR_INDEX
+	};
+
+	uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)};			/**< airspeed validated topic*/
+	uORB::PublicationMulti<airspeed_wind_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
+	orb_advert_t 	_mavlink_log_pub {nullptr}; 						/**< mavlink log topic*/
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+	uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)};
+	uORB::SubscriptionMultiArray<airspeed_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};
+
+
+	estimator_status_s _estimator_status {};
+	vehicle_acceleration_s _accel {};
+	vehicle_air_data_s _vehicle_air_data {};
+	vehicle_attitude_s _vehicle_attitude {};
+	vehicle_land_detected_s _vehicle_land_detected {};
+	vehicle_local_position_s _vehicle_local_position {};
+	vehicle_status_s _vehicle_status {};
+	vtol_vehicle_status_s _vtol_vehicle_status {};
+	position_setpoint_s _position_setpoint {};
+
+	WindEstimator	_wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */
+	airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
+
+	int32_t _number_of_airspeed_sensors{0}; /**<  number of airspeed sensors in use (detected during initialization)*/
+	int32_t _prev_number_of_airspeed_sensors{0}; /**<  number of airspeed sensors in previous loop (to detect a new added sensor)*/
+	AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */
+
+	hrt_abstime _time_now_usec{0};
+	int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
+	int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
+	bool _initialized{false}; /**< module initialized*/
+	bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
+	bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
+	float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
+
+	bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
+
+	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
+
+	perf_counter_t _perf_elapsed{};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
+		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
+		(ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
+		(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
+		(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
+		(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
+		(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
+		(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
+		(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
+		(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
+		(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
+
+		(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
+		(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
+		(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
+		(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
+
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
+	)
+
+	void 		init(); 	/**< initialization of the airspeed validator instances */
+	void 		check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */
+	void		update_params(); /**< update parameters */
+	void 		poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
+	void 		update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
+	void		update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
+	void 		select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
+
+};
+
+AirspeedModule::AirspeedModule():
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	// initialise parameters
+	update_params();
+
+	_perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed");
+}
+
+AirspeedModule::~AirspeedModule()
+{
+	ScheduleClear();
+
+	perf_free(_perf_elapsed);
+}
+
+int
+AirspeedModule::task_spawn(int argc, char *argv[])
+{
+	AirspeedModule *dev = new AirspeedModule();
+
+	// check if the trampoline is called for the first time
+	if (!dev) {
+		PX4_ERR("alloc failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(dev);
+
+	dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
+	_task_id = task_id_is_work_queue;
+	return PX4_OK;
+
+}
+void
+AirspeedModule::init()
+{
+	check_for_connected_airspeed_sensors();
+
+	// Set the default sensor
+	if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
+		// constrain the index to the number of sensors connected
+		_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
+
+		if (_number_of_airspeed_sensors == 0) {
+			mavlink_log_info(&_mavlink_log_pub,
+					 "No airspeed sensor detected. Switch to non-airspeed mode.");
+
+		} else {
+			mavlink_log_info(&_mavlink_log_pub,
+					 "Primary airspeed index bigger than number connected sensors. Take last sensor.");
+		}
+
+	} else {
+		// set index to the one provided in the parameter ASPD_PRIMARY
+		_valid_airspeed_index =	_param_airspeed_primary_index.get();
+	}
+
+	_prev_airspeed_index = _valid_airspeed_index; // used to detect a sensor switching
+}
+
+void
+AirspeedModule::check_for_connected_airspeed_sensors()
+{
+	// check for new connected airspeed sensor
+	int detected_airspeed_sensors = 0;
+
+	if (_param_airspeed_primary_index.get() > 0) {
+
+		for (int i = 0; i < _airspeed_subs.size(); i++) {
+			if (!_airspeed_subs[i].advertised()) {
+				break;
+			}
+
+			detected_airspeed_sensors = i + 1;
+		}
+
+	} else {
+		// user has selected groundspeed-windspeed as primary source, or disabled airspeed
+		detected_airspeed_sensors = 0;
+	}
+
+	_number_of_airspeed_sensors = detected_airspeed_sensors;
+}
+
+
+void
+AirspeedModule::Run()
+{
+	_time_now_usec = hrt_absolute_time(); // hrt time of the current cycle
+
+	// do not run the airspeed selector until 2s after system boot,
+	// as data from airspeed sensor	and estimator may not be valid yet
+	if (_time_now_usec < 2_s) {
+		return;
+	}
+
+	perf_begin(_perf_elapsed);
+
+	if (!_initialized) {
+		init(); // initialize airspeed validator instances
+		_initialized = true;
+	}
+
+	parameter_update_s update;
+
+	if (_parameter_update_sub.update(&update)) {
+		update_params();
+	}
+
+	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+	// check for new connected airspeed sensors as long as we're disarmed
+	if (!armed) {
+		check_for_connected_airspeed_sensors();
+	}
+
+	poll_topics();
+	update_wind_estimator_sideslip();
+	update_ground_minus_wind_airspeed();
+
+	if (_number_of_airspeed_sensors > 0) {
+
+		// disable checks if not a fixed-wing or the vehicle is landing/landed, as then airspeed can fall below stall speed
+		// and wind estimate isn't accurate anymore. Even better would be to have a reliable "ground_contact" detection
+		// for fixed-wing landings.
+		const bool in_air_fixed_wing = !_vehicle_land_detected.landed &&
+					       _position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
+					       _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+
+		// Prepare data for airspeed_validator
+		struct airspeed_validator_update_data input_data = {};
+		input_data.timestamp = _time_now_usec;
+		input_data.lpos_vx = _vehicle_local_position.vx;
+		input_data.lpos_vy = _vehicle_local_position.vy;
+		input_data.lpos_vz = _vehicle_local_position.vz;
+		input_data.lpos_valid = _vehicle_local_position_valid;
+		input_data.lpos_evh = _vehicle_local_position.evh;
+		input_data.lpos_evv = _vehicle_local_position.evv;
+		input_data.att_q[0] = _vehicle_attitude.q[0];
+		input_data.att_q[1] = _vehicle_attitude.q[1];
+		input_data.att_q[2] = _vehicle_attitude.q[2];
+		input_data.att_q[3] = _vehicle_attitude.q[3];
+		input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa;
+		input_data.accel_z = _accel.xyz[2];
+		input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
+		input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
+
+		// iterate through all airspeed sensors, poll new data from them and update their validators
+		for (int i = 0; i < _number_of_airspeed_sensors; i++) {
+
+			// poll raw airspeed topic of the i-th sensor
+			airspeed_s airspeed_raw;
+
+			if (_airspeed_subs[i].update(&airspeed_raw)) {
+
+				input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
+				input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
+				input_data.airspeed_timestamp = airspeed_raw.timestamp;
+				input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
+
+				// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
+				if (_in_takeoff_situation &&
+				    (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
+				     _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
+					_in_takeoff_situation = false;
+				}
+
+				// reset takeoff_situation to true when not in air and not in fixed-wing mode
+				if (!in_air_fixed_wing) {
+					_in_takeoff_situation = true;
+				}
+
+				input_data.in_fixed_wing_flight = (armed && in_air_fixed_wing && !_in_takeoff_situation);
+
+				// push input data into airspeed validator
+				_airspeed_validator[i].update_airspeed_validator(input_data);
+
+				_time_last_airspeed_update[i] = _time_now_usec;
+
+			} else if (_time_now_usec - _time_last_airspeed_update[i] > 1_s) {
+				// declare airspeed invalid if more then 1s since last raw airspeed update
+				_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
+
+			}
+		}
+	}
+
+	select_airspeed_and_publish();
+
+	perf_end(_perf_elapsed);
+
+	if (should_exit()) {
+		exit_and_cleanup();
+	}
+}
+
+void AirspeedModule::update_params()
+{
+	updateParams();
+
+	_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
+	_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
+	_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
+	_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
+	_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
+	_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
+
+	for (int i = 0; i < _number_of_airspeed_sensors; i++) {
+		_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
+		_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
+		_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
+		_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
+		_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
+		_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
+		_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
+
+		// only apply manual entered airspeed scale to first airspeed measurement
+		// TODO: enable multiple airspeed sensors
+		_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
+
+		_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
+		_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
+		_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
+		_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
+		_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
+	}
+
+	// if the airspeed scale estimation is enabled and the airspeed is valid,
+	// then set the scale inside the wind estimator to -1 such that it starts to estimate it
+	if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
+		if (_valid_airspeed_index > 0) {
+			// set it to a negative value to start estimation inside wind estimator
+			_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
+
+		} else {
+			mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
+			_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
+			_param_west_scale_estimation_on.commit_no_notification();
+		}
+
+	} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
+		if (_valid_airspeed_index > 0) {
+
+			_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
+			_param_west_airspeed_scale.commit_no_notification();
+			_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
+
+			mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f",
+					 (double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
+
+		} else {
+			mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
+		}
+	}
+
+	_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
+
+}
+
+void AirspeedModule::poll_topics()
+{
+	// use primary estimator_status
+	if (_estimator_selector_status_sub.updated()) {
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
+				_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
+			}
+		}
+	}
+
+	_estimator_status_sub.update(&_estimator_status);
+	_vehicle_acceleration_sub.update(&_accel);
+	_vehicle_air_data_sub.update(&_vehicle_air_data);
+	_vehicle_attitude_sub.update(&_vehicle_attitude);
+	_vehicle_land_detected_sub.update(&_vehicle_land_detected);
+	_vehicle_status_sub.update(&_vehicle_status);
+	_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
+	_vehicle_local_position_sub.update(&_vehicle_local_position);
+	_position_setpoint_sub.update(&_position_setpoint);
+
+
+	_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
+					&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
+}
+
+void AirspeedModule::update_wind_estimator_sideslip()
+{
+	// update wind and airspeed estimator
+	_wind_estimator_sideslip.update(_time_now_usec);
+
+	if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
+		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
+		Quatf q(_vehicle_attitude.q);
+
+		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
+	}
+
+	_wind_estimate_sideslip.timestamp = _time_now_usec;
+	float wind[2];
+	_wind_estimator_sideslip.get_wind(wind);
+	_wind_estimate_sideslip.windspeed_north = wind[0];
+	_wind_estimate_sideslip.windspeed_east = wind[1];
+	float wind_cov[2];
+	_wind_estimator_sideslip.get_wind_var(wind_cov);
+	_wind_estimate_sideslip.variance_north = wind_cov[0];
+	_wind_estimate_sideslip.variance_east = wind_cov[1];
+	_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
+	_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
+	_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
+	_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
+	_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
+	_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
+}
+
+void AirspeedModule::update_ground_minus_wind_airspeed()
+{
+	// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
+	const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
+	const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
+	const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
+	_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
+	_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
+				 _vehicle_air_data.baro_temp_celcius);
+}
+
+
+void AirspeedModule::select_airspeed_and_publish()
+{
+	const bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
+			!_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
+	const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
+			_param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
+	const bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
+
+	if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
+
+		_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
+
+		// loop through all sensors and take the first valid one
+		for (int i = 0; i < _number_of_airspeed_sensors; i++) {
+			if (_airspeed_validator[i].get_airspeed_valid()) {
+				_valid_airspeed_index = i + 1;
+				break;
+			}
+		}
+	}
+
+	// check if airspeed based on ground-wind speed is valid and can be published
+	if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
+	    (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
+	     || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
+
+		// _vehicle_local_position_valid determines if ground-wind estimate is valid
+		if (_vehicle_local_position_valid &&
+		    (_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
+			_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;
+
+		} else {
+			_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
+		}
+	}
+
+	// print warning or info, depending of whether airspeed got declared invalid or healthy
+	if (_valid_airspeed_index != _prev_airspeed_index &&
+	    (_number_of_airspeed_sensors > 0 || !_vehicle_land_detected.landed) &&
+	    _valid_airspeed_index != _prev_airspeed_index) {
+		if (_prev_airspeed_index > 0) {
+			mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected (%i, %i)", _prev_airspeed_index,
+					     _valid_airspeed_index);
+
+		} else if (_prev_airspeed_index == 0 && _valid_airspeed_index == -1) {
+			mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation invalid");
+
+		} else if (_prev_airspeed_index == -1 && _valid_airspeed_index == 0) {
+			mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation valid");
+
+		} else {
+			mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)", _prev_airspeed_index,
+					 _valid_airspeed_index);
+		}
+	}
+
+	_prev_airspeed_index = _valid_airspeed_index;
+	_prev_number_of_airspeed_sensors = _number_of_airspeed_sensors;
+
+	airspeed_validated_s airspeed_validated = {};
+	airspeed_validated.timestamp = _time_now_usec;
+	airspeed_validated.true_ground_minus_wind_m_s = NAN;
+	airspeed_validated.calibrated_ground_minus_wind_m_s = NAN;
+	airspeed_validated.indicated_airspeed_m_s = NAN;
+	airspeed_validated.calibrated_airspeed_m_s = NAN;
+	airspeed_validated.true_airspeed_m_s = NAN;
+	airspeed_validated.airspeed_sensor_measurement_valid = false;
+	airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
+
+	switch (_valid_airspeed_index) {
+	case airspeed_index::DISABLED_INDEX:
+		break;
+
+	case airspeed_index::GROUND_MINUS_WIND_INDEX:
+		airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_CAS;
+		airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS;
+		airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
+		airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
+		airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
+
+		break;
+
+	default:
+		airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS();
+		airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_CAS();
+		airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS();
+		airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
+		airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
+		airspeed_validated.airspeed_sensor_measurement_valid = true;
+		break;
+	}
+
+	_airspeed_validated_pub.publish(airspeed_validated);
+
+	_wind_est_pub[0].publish(_wind_estimate_sideslip);
+
+	// publish the wind estimator states from all airspeed validators
+	for (int i = 0; i < _number_of_airspeed_sensors; i++) {
+		airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
+
+		if (i == 0) {
+			wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1;
+
+		} else if (i == 1) {
+			wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2;
+
+		} else {
+			wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3;
+		}
+
+		_wind_est_pub[i + 1].publish(wind_est);
+	}
+
+}
+
+int AirspeedModule::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		int ret = AirspeedModule::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int AirspeedModule::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module provides a single airspeed_validated topic, containing indicated (IAS),
+calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
+is invalid and if based sensor readings or on groundspeed minus windspeed.
+Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
+to a valid sensor in case of failure detection. For failure detection as well as for
+the estimation of a scale factor from IAS to CAS, it runs several wind estimators
+and also publishes those.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[])
+{
+	return AirspeedModule::main(argc, argv);
+}

src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_params.c

@@ -0,0 +1,186 @@
+
+/**
+ * Airspeed Selector: Wind estimator wind process noise
+ *
+ * Wind process noise of the internal wind estimator(s) of the airspeed selector.
+ *
+ * @min 0
+ * @max 1
+ * @unit m/s^2
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
+
+/**
+ * Airspeed Selector: Wind estimator true airspeed scale process noise
+ *
+ * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.
+ *
+ * @min 0
+ * @max 0.1
+ * @unit Hz
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
+
+/**
+ * Airspeed Selector: Wind estimator true airspeed measurement noise
+ *
+ * True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector.
+ *
+ * @min 0
+ * @max 4
+ * @unit m/s
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
+
+/**
+ * Airspeed Selector: Wind estimator sideslip measurement noise
+ *
+ * Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector.
+ *
+ * @min 0
+ * @max 1
+ * @unit rad
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
+
+/**
+ * Airspeed Selector: Gate size for true airspeed fusion
+ *
+ * Sets the number of standard deviations used by the innovation consistency test.
+ *
+ * @min 1
+ * @max 5
+ * @unit SD
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
+
+/**
+ * Airspeed Selector: Gate size for sideslip angle fusion
+ *
+ * Sets the number of standard deviations used by the innovation consistency test.
+ *
+ * @min 1
+ * @max 5
+ * @unit SD
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
+
+/**
+ * Automatic airspeed scale estimation on
+ *
+ * Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level
+ * altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
+ * Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
+ *
+ * @boolean
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
+
+/**
+ * Airspeed scale (scale from IAS to CAS)
+ *
+ * Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
+ *
+ * @min 0.5
+ * @max 1.5
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
+
+/**
+ * Index or primary airspeed measurement source
+ *
+ * @value -1 Disabled
+ * @value 0 Groundspeed minus windspeed
+ * @value 1 First airspeed sensor
+ * @value 2 Second airspeed sensor
+ * @value 3 Third airspeed sensor
+ *
+ * @reboot_required true
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
+
+
+/**
+ * Enable checks on airspeed sensors
+ *
+ * If set to true then the data comming from the airspeed sensors is checked for validity. Only applied if ASPD_PRIMARY > 0.
+ *
+ * @boolean
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1);
+
+/**
+ * Enable fallback to sensor-less airspeed estimation
+ *
+ * If set to true and airspeed checks are enabled, it will use a sensor-less airspeed estimation based on groundspeed
+ * minus windspeed if no other airspeed sensor available to fall back to.
+ *
+ * @value 0 Disable fallback to sensor-less estimation
+ * @value 1 Enable fallback to sensor-less estimation
+ * @boolean
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
+
+/**
+ * Airspeed failsafe consistency threshold
+ *
+ * This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive,
+ * smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the
+ * inconsistency between predicted and measured airspeed is large enough to cause the wind EKF to reject airspeed measurements.
+ * The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter.
+*
+ * @min 0.5
+ * @max 3.0
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
+
+/**
+ * Airspeed failsafe consistency delay
+ *
+ * This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe.
+ * For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will
+ * rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values
+ * make it more sensitive.
+ *
+ * @unit s
+ * @max 30.0
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);
+
+/**
+ * Airspeed failsafe stop delay
+ *
+ * Delay before stopping use of airspeed sensor if checks indicate sensor is bad.
+ *
+ * @unit s
+ * @group Airspeed Validator
+ * @min 1
+ * @max 10
+ */
+PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
+
+/**
+ * Airspeed failsafe start delay
+ *
+ * Delay before switching back to using airspeed sensor if checks indicate sensor is good.
+ * Set to a negative value to disable the re-enabling in flight.
+ *
+ * @unit s
+ * @group Airspeed Validator
+ * @min -1
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);

src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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 AngularVelocityControl.cpp
+ */
+
+#include <AngularVelocityControl.hpp>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_p = P;
+	_gain_i = I;
+	_gain_d = D;
+}
+
+void AngularVelocityControl::setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+		const matrix::Vector<bool, 3> &saturation_negative)
+{
+	_saturation_positive = saturation_positive;
+	_saturation_negative = saturation_negative;
+}
+
+void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp,
+				    const Vector3f &angular_acceleration, const float dt, const bool landed)
+{
+	// angular rates error
+	Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity;
+
+	// P + D control
+	_angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration);
+
+	// I + FF control
+	Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp);
+
+	// compute torque setpoint
+	_torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity);
+
+	// update integral only if we are not landed
+	if (!landed) {
+		updateIntegral(angular_velocity_error, dt);
+	}
+}
+
+void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt)
+{
+	for (int i = 0; i < 3; i++) {
+		// prevent further positive control saturation
+		if (_saturation_positive(i)) {
+			angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f);
+		}
+
+		// prevent further negative control saturation
+		if (_saturation_negative(i)) {
+			angular_velocity_error(i) = math::max(angular_velocity_error(i), 0.f);
+		}
+
+		// I term factor: reduce the I gain with increasing rate error.
+		// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
+		// change (noticeable in a bounce-back effect after a flip).
+		// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
+		// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
+		// and up to 200 deg error leads to <25% reduction of I.
+		float i_factor = angular_velocity_error(i) / math::radians(400.f);
+		i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
+
+		// Perform the integration using a first order method
+		float angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt;
+
+		// do not propagate the result if out of range or invalid
+		if (PX4_ISFINITE(angular_velocity_i)) {
+			_angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i));
+		}
+	}
+}
+
+void AngularVelocityControl::reset()
+{
+	_angular_velocity_int.zero();
+	_torque_sp.zero();
+	_angular_accel_sp.zero();
+}

src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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 AngularVelocityControl.hpp
+ *
+ * PID 3 axis angular velocity control.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
+
+class AngularVelocityControl
+{
+public:
+	AngularVelocityControl() = default;
+	~AngularVelocityControl() = default;
+
+	/**
+	 * Set the control gains
+	 * @param P 3D vector of proportional gains for body x,y,z axis
+	 * @param I 3D vector of integral gains
+	 * @param D 3D vector of derivative gains
+	 */
+	void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+
+	/**
+	 * Set the mximum absolute value of the integrator for all axes
+	 * @param integrator_limit limit value for all axes x, y, z
+	 */
+	void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
+
+	/**
+	 * Set direct angular velocity setpoint to torque feed forward gain
+	 * @see _gain_ff
+	 * @param FF 3D vector of feed forward gains for body x,y,z axis
+	 */
+	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
+
+	/**
+	 * Set inertia matrix
+	 * @see _inertia
+	 * @param inertia inertia matrix
+	 */
+	void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
+
+	/**
+	 * Set saturation status
+	 * @see _saturation_positive
+	 * @see _saturation_negative
+	 * @param saturation_positive positive saturation
+	 * @param saturation_negative negative saturation
+	 */
+	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+				 const matrix::Vector<bool, 3> &saturation_negative);
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param angular_velocity estimation of the current vehicle angular velocity
+	 * @param angular_velocity_sp desired vehicle angular velocity setpoint
+	 * @param angular_acceleration estimation of the current vehicle angular acceleration
+	 * @param dt elapsed time since last update
+	 * @param landed whether the vehicle is on the ground
+	 */
+	void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp,
+		    const matrix::Vector3f &angular_acceleration, const float dt, const bool landed);
+
+	/**
+	 * Get the desired angular acceleration
+	 * @see _angular_accel_sp
+	 */
+	const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
+
+	/**
+	 * Get the torque vector to apply to the vehicle
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
+
+	/**
+	 * Get the integral term
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getIntegral() {return _angular_velocity_int;};
+
+	/**
+	 * Set the integral term to 0 to prevent windup
+	 * @see _angular_velocity_int
+	 */
+	void resetIntegral() { _angular_velocity_int.zero(); }
+
+	/**
+	 * ReSet the controller state
+	 */
+	void reset();
+
+private:
+	void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt);
+
+	// Gains
+	matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z
+	matrix::Vector3f _gain_i; ///< integral gain
+	matrix::Vector3f _gain_d; ///< derivative gain
+	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
+	matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain
+	matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
+
+	// States
+	matrix::Vector3f _angular_velocity_int;
+	matrix::Vector<bool, 3> _saturation_positive;
+	matrix::Vector<bool, 3> _saturation_negative;
+
+	// Output
+	matrix::Vector3f _angular_accel_sp; 	//< Angular acceleration setpoint computed using P and D gains
+	matrix::Vector3f _torque_sp;		//< Torque setpoint to apply to the vehicle
+};

src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include <AngularVelocityControl.hpp>
+
+using namespace matrix;
+
+TEST(AngularVelocityControlTest, AllZeroCase)
+{
+	AngularVelocityControl control;
+	control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
+	Vector3f torque = control.getTorqueSetpoint();
+	EXPECT_EQ(torque, Vector3f());
+}

src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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(AngularVelocityControl
+	AngularVelocityControl.cpp
+)
+target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(AngularVelocityControl PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)

src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -0,0 +1,342 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 "AngularVelocityController.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+#include <ecl/geo/geo.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+AngularVelocityController::AngularVelocityController() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+	parameters_updated();
+}
+
+AngularVelocityController::~AngularVelocityController()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+AngularVelocityController::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_velocity callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+AngularVelocityController::parameters_updated()
+{
+	// Control parameters
+	// The controller gain K is used to convert the parallel (P + I/s + sD) form
+	// to the ideal (K * [1 + 1/sTi + sTd]) form
+	const Vector3f k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get());
+
+	_control.setGains(
+		k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())),
+		k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())),
+		k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get())));
+
+	_control.setIntegratorLimit(
+		Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get()));
+
+	_control.setFeedForwardGain(
+		Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get()));
+
+	// inertia matrix
+	const float inertia[3][3] = {
+		{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
+		{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
+		{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
+	};
+	_control.setInertiaMatrix(matrix::Matrix3f(inertia));
+
+	// Hover thrust
+	if (!_param_mpc_use_hte.get()) {
+		_hover_thrust = _param_mpc_thr_hover.get();
+	}
+}
+
+void
+AngularVelocityController::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		parameters_updated();
+	}
+
+	/* run controller on gyro changes */
+	vehicle_angular_velocity_s vehicle_angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
+		const hrt_abstime now = hrt_absolute_time();
+		_timestamp_sample = vehicle_angular_velocity.timestamp_sample;
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+		_last_run = now;
+
+		const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
+
+		/* check for updates in other topics */
+		_vehicle_status_sub.update(&_vehicle_status);
+		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
+
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+
+			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed;
+				_maybe_landed = vehicle_land_detected.maybe_landed;
+			}
+		}
+
+		// Check for updates of hover thrust
+		if (_param_mpc_use_hte.get()) {
+			hover_thrust_estimate_s hte;
+
+			if (_hover_thrust_estimate_sub.update(&hte)) {
+				_hover_thrust = hte.hover_thrust;
+			}
+		}
+
+		// check angular acceleration topic
+		vehicle_angular_acceleration_s vehicle_angular_acceleration;
+
+		if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
+			_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
+		}
+
+		// check rates setpoint topic
+		vehicle_rates_setpoint_s vehicle_rates_setpoint;
+
+		if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
+			_angular_velocity_sp(0) = vehicle_rates_setpoint.roll;
+			_angular_velocity_sp(1) = vehicle_rates_setpoint.pitch;
+			_angular_velocity_sp(2) = vehicle_rates_setpoint.yaw;
+			_thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body);
+
+			// Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G
+			_thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust);
+		}
+
+		// run the controller
+		if (_vehicle_control_mode.flag_control_rates_enabled) {
+			// reset integral if disarmed
+			if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				_control.resetIntegral();
+			}
+
+			// update saturation status from mixer feedback
+			control_allocator_status_s control_allocator_status;
+
+			if (_control_allocator_status_sub.update(&control_allocator_status)) {
+				Vector<bool, 3> saturation_positive;
+				Vector<bool, 3> saturation_negative;
+
+				if (!control_allocator_status.torque_setpoint_achieved) {
+					for (size_t i = 0; i < 3; i++) {
+						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
+							saturation_positive(i) = true;
+
+						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
+							saturation_negative(i) = true;
+						}
+					}
+				}
+
+				_control.setSaturationStatus(saturation_positive, saturation_negative);
+			}
+
+			// run rate controller
+			_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);
+
+			// publish rate controller status
+			rate_ctrl_status_s rate_ctrl_status{};
+			Vector3f integral = _control.getIntegral();
+			rate_ctrl_status.timestamp = hrt_absolute_time();
+			rate_ctrl_status.rollspeed_integ = integral(0);
+			rate_ctrl_status.pitchspeed_integ = integral(1);
+			rate_ctrl_status.yawspeed_integ = integral(2);
+			_rate_ctrl_status_pub.publish(rate_ctrl_status);
+
+			// publish controller output
+			publish_angular_acceleration_setpoint();
+			publish_torque_setpoint();
+			publish_thrust_setpoint();
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+void
+AngularVelocityController::publish_angular_acceleration_setpoint()
+{
+	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
+
+	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
+	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
+	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
+	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
+	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
+
+	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
+}
+
+void
+AngularVelocityController::publish_torque_setpoint()
+{
+	Vector3f torque_sp = _control.getTorqueSetpoint();
+
+	vehicle_torque_setpoint_s v_torque_sp = {};
+	v_torque_sp.timestamp = hrt_absolute_time();
+	v_torque_sp.timestamp_sample = _timestamp_sample;
+	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
+	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
+	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
+
+	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+}
+
+void
+AngularVelocityController::publish_thrust_setpoint()
+{
+	vehicle_thrust_setpoint_s v_thrust_sp = {};
+	v_thrust_sp.timestamp = hrt_absolute_time();
+	v_thrust_sp.timestamp_sample = _timestamp_sample;
+	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
+	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
+	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
+
+	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+}
+
+int AngularVelocityController::task_spawn(int argc, char *argv[])
+{
+	AngularVelocityController *instance = new AngularVelocityController();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int AngularVelocityController::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int AngularVelocityController::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AngularVelocityController::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the angular velocity controller.
+It takes angular velocity setpoints and measured angular
+velocity as inputs and outputs actuator setpoints.
+
+The controller has a PID loop for angular rate error.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Angular velocity controller app start / stop handling function
+ */
+extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]);
+
+int angular_velocity_controller_main(int argc, char *argv[])
+{
+	return AngularVelocityController::main(argc, argv);
+}

src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.hpp

@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <AngularVelocityControl.hpp>
+
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/hover_thrust_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+
+using namespace time_literals;
+
+class AngularVelocityController : public ModuleBase<AngularVelocityController>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	AngularVelocityController();
+
+	virtual ~AngularVelocityController();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void		parameters_updated();
+
+	void		vehicle_status_poll();
+
+	void		publish_angular_acceleration_setpoint();
+	void		publish_torque_setpoint();
+	void		publish_thrust_setpoint();
+	void		publish_actuator_controls();
+
+	AngularVelocityControl _control; 	///< class for control calculations
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};				/**< motor limits subscription */
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; 	/**< vehicle angular acceleration subscription */
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};			/**< vehicle control mode subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};			/**< vehicle land detected subscription */
+	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};			/**< vehicle rates setpoint subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};					/**< vehicle status subscription */
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::Publication<rate_ctrl_status_s>			   _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};						/**< controller status publication */
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};	/**< angular acceleration setpoint publication */
+	uORB::Publication<vehicle_thrust_setpoint_s>		   _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};				/**< thrust setpoint publication */
+	uORB::Publication<vehicle_torque_setpoint_s>		   _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};				/**< torque setpoint publication */
+
+	vehicle_control_mode_s		_vehicle_control_mode{};
+	vehicle_status_s		_vehicle_status{};
+
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	float _battery_status_scale{0.0f};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	matrix::Vector3f _angular_velocity_sp;		/**< angular velocity setpoint */
+	matrix::Vector3f _angular_acceleration;		/**< angular acceleration (estimated) */
+	matrix::Vector3f _thrust_sp;			/**< thrust setpoint */
+
+	float _hover_thrust{0.5f};			/**< Normalized hover thrust **/
+
+	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
+
+	hrt_abstime _task_start{hrt_absolute_time()};
+	hrt_abstime _last_run{0};
+	hrt_abstime _timestamp_sample{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::AVC_X_P>) _param_avc_x_p,
+		(ParamFloat<px4::params::AVC_X_I>) _param_avc_x_i,
+		(ParamFloat<px4::params::AVC_X_I_LIM>) _param_avc_x_i_lim,
+		(ParamFloat<px4::params::AVC_X_D>) _param_avc_x_d,
+		(ParamFloat<px4::params::AVC_X_FF>) _param_avc_x_ff,
+		(ParamFloat<px4::params::AVC_X_K>) _param_avc_x_k,
+
+		(ParamFloat<px4::params::AVC_Y_P>) _param_avc_y_p,
+		(ParamFloat<px4::params::AVC_Y_I>) _param_avc_y_i,
+		(ParamFloat<px4::params::AVC_Y_I_LIM>) _param_avc_y_i_lim,
+		(ParamFloat<px4::params::AVC_Y_D>) _param_avc_y_d,
+		(ParamFloat<px4::params::AVC_Y_FF>) _param_avc_y_ff,
+		(ParamFloat<px4::params::AVC_Y_K>) _param_avc_y_k,
+
+		(ParamFloat<px4::params::AVC_Z_P>) _param_avc_z_p,
+		(ParamFloat<px4::params::AVC_Z_I>) _param_avc_z_i,
+		(ParamFloat<px4::params::AVC_Z_I_LIM>) _param_avc_z_i_lim,
+		(ParamFloat<px4::params::AVC_Z_D>) _param_avc_z_d,
+		(ParamFloat<px4::params::AVC_Z_FF>) _param_avc_z_ff,
+		(ParamFloat<px4::params::AVC_Z_K>) _param_avc_z_k,
+
+		(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,
+		(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,
+		(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
+		(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
+		(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
+		(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
+		(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
+
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
+		(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte
+	)
+
+};

src/prometheus_px4/src/modules/angular_velocity_controller/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   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(AngularVelocityControl)
+
+px4_add_module(
+	MODULE modules__angular_velocity_control
+	MAIN angular_velocity_controller
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		AngularVelocityController.cpp
+		AngularVelocityController.hpp
+	DEPENDS
+		circuit_breaker
+		mathlib
+		AngularVelocityControl
+		px4_work_queue
+	)

src/prometheus_px4/src/modules/angular_velocity_controller/angular_velocity_controller_params.c

@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 angular_velocity_controller_params.c
+ * Parameters for angular velocity controller.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Body X axis angular velocity P gain
+ *
+ * Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_P, 18.f);
+
+/**
+ * Body X axis angular velocity I gain
+ *
+ * Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f);
+
+/**
+ * Body X axis angular velocity integrator limit
+ *
+ * Body X axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f);
+
+/**
+ * Body X axis angular velocity D gain
+ *
+ * Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f);
+
+/**
+ * Body X axis angular velocity feedforward gain
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f);
+
+/**
+ * Body X axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_X_K * (AVC_X_P * error
+ * 			+ AVC_X_I * error_integral
+ * 			+ AVC_X_D * error_derivative)
+ * Set AVC_X_P=1 to implement a PID in the ideal form.
+ * Set AVC_X_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f);
+
+/**
+ * Body Y axis angular velocity P gain
+ *
+ * Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f);
+
+/**
+ * Body Y axis angular velocity I gain
+ *
+ * Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f);
+
+/**
+ * Body Y axis angular velocity integrator limit
+ *
+ * Body Y axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f);
+
+/**
+ * Body Y axis angular velocity D gain
+ *
+ * Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f);
+
+/**
+ * Body Y axis angular velocity feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f);
+
+/**
+ * Body Y axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_Y_K * (AVC_Y_P * error
+ * 			     + AVC_Y_I * error_integral
+ * 			     + AVC_Y_D * error_derivative)
+ * Set AVC_Y_P=1 to implement a PID in the ideal form.
+ * Set AVC_Y_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 20.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f);
+
+/**
+ * Body Z axis angular velocity P gain
+ *
+ * Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 20.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f);
+
+/**
+ * Body Z axis angular velocity I gain
+ *
+ * Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @unit Nm/rad
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f);
+
+/**
+ * Body Z axis angular velocity integrator limit
+ *
+ * Body Z axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f);
+
+/**
+ * Body Z axis angular velocity D gain
+ *
+ * Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f);
+
+/**
+ * Body Z axis angular velocity feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f);
+
+/**
+ * Body Z axis angular velocity controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = AVC_Z_K * (AVC_Z_P * error
+ * 			     + AVC_Z_I * error_integral
+ * 			     + AVC_Z_D * error_derivative)
+ * Set AVC_Z_P=1 to implement a PID in the ideal form.
+ * Set AVC_Z_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Angular Velocity Control
+ */
+PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f);

src/prometheus_px4/src/modules/angular_velocity_controller/vehicle_model_params.c

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 vehicle_model_params.c
+ * Parameters for vehicle model.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Mass
+ *
+ * @unit kg
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
+
+/**
+ * Inertia matrix, XX component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f);
+
+/**
+ * Inertia matrix, YY component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f);
+
+/**
+ * Inertia matrix, ZZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f);
+
+/**
+ * Inertia matrix, XY component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
+
+/**
+ * Inertia matrix, XZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
+
+/**
+ * Inertia matrix, YZ component
+ *
+ * @unit kg m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);

src/prometheus_px4/src/modules/attitude_estimator_q/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+#############################################################################
+set(MODULE_CFLAGS)
+px4_add_module(
+	MODULE modules__attitude_estimator_q
+	MAIN attitude_estimator_q
+	COMPILE_FLAGS
+	STACK_MAX 1600
+	SRCS
+		attitude_estimator_q_main.cpp
+	DEPENDS
+		git_ecl
+		ecl_geo_lookup
+	)
+

src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -0,0 +1,615 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 attitude_estimator_q_main.cpp
+ *
+ * Attitude estimator (quaternion based)
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <float.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/ecl/geo/geo.h>
+#include <lib/ecl/geo_lookup/geo_mag_declination.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <matrix/math.hpp>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_magnetometer.h>
+#include <uORB/topics/vehicle_odometry.h>
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
+using namespace time_literals;
+
+class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem
+{
+public:
+
+	AttitudeEstimatorQ();
+	~AttitudeEstimatorQ() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+
+	void Run() override;
+
+	void update_parameters(bool force = false);
+
+	bool init_attq();
+
+	bool update(float dt);
+
+	// Update magnetic declination (in rads) immediately changing yaw rotation
+	void update_mag_declination(float new_declination);
+
+
+	const float _eo_max_std_dev = 100.0f;		/**< Maximum permissible standard deviation for estimated orientation */
+	const float _dt_min = 0.00001f;
+	const float _dt_max = 0.02f;
+
+	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
+
+	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription		_gps_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription		_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription		_vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
+	uORB::Subscription		_mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
+	uORB::Subscription		_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
+
+	uORB::Publication<vehicle_attitude_s>	_att_pub{ORB_ID(vehicle_attitude)};
+
+	float		_mag_decl{0.0f};
+	float		_bias_max{0.0f};
+
+	Vector3f	_gyro;
+	Vector3f	_accel;
+	Vector3f	_mag;
+
+	Vector3f	_vision_hdg;
+	Vector3f	_mocap_hdg;
+
+	Quatf		_q;
+	Vector3f	_rates;
+	Vector3f	_gyro_bias;
+
+	Vector3f	_vel_prev;
+	hrt_abstime	_vel_prev_t{0};
+
+	Vector3f	_pos_acc;
+
+	hrt_abstime	_last_time{0};
+
+	bool		_inited{false};
+	bool		_data_good{false};
+	bool		_ext_hdg_good{false};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,
+		(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,
+		(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
+		(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,
+		(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
+		(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
+		(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
+		(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
+		(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
+		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
+	)
+};
+
+AttitudeEstimatorQ::AttitudeEstimatorQ() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	_vel_prev.zero();
+	_pos_acc.zero();
+
+	_gyro.zero();
+	_accel.zero();
+	_mag.zero();
+
+	_vision_hdg.zero();
+	_mocap_hdg.zero();
+
+	_q.zero();
+	_rates.zero();
+	_gyro_bias.zero();
+
+	update_parameters(true);
+}
+
+bool
+AttitudeEstimatorQ::init()
+{
+	if (!_sensors_sub.registerCallback()) {
+		PX4_ERR("sensor combined callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+AttitudeEstimatorQ::Run()
+{
+	if (should_exit()) {
+		_sensors_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	sensor_combined_s sensors;
+
+	if (_sensors_sub.update(&sensors)) {
+
+		update_parameters();
+
+		// Feed validator with recent sensor data
+		if (sensors.timestamp > 0) {
+			_gyro(0) = sensors.gyro_rad[0];
+			_gyro(1) = sensors.gyro_rad[1];
+			_gyro(2) = sensors.gyro_rad[2];
+		}
+
+		if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
+			_accel(0) = sensors.accelerometer_m_s2[0];
+			_accel(1) = sensors.accelerometer_m_s2[1];
+			_accel(2) = sensors.accelerometer_m_s2[2];
+
+			if (_accel.length() < 0.01f) {
+				PX4_ERR("degenerate accel!");
+				return;
+			}
+		}
+
+		// Update magnetometer
+		if (_magnetometer_sub.updated()) {
+			vehicle_magnetometer_s magnetometer;
+
+			if (_magnetometer_sub.copy(&magnetometer)) {
+				_mag(0) = magnetometer.magnetometer_ga[0];
+				_mag(1) = magnetometer.magnetometer_ga[1];
+				_mag(2) = magnetometer.magnetometer_ga[2];
+
+				if (_mag.length() < 0.01f) {
+					PX4_ERR("degenerate mag!");
+					return;
+				}
+			}
+
+		}
+
+		_data_good = true;
+
+		// Update vision and motion capture heading
+		_ext_hdg_good = false;
+
+		if (_vision_odom_sub.updated()) {
+			vehicle_odometry_s vision;
+
+			if (_vision_odom_sub.copy(&vision)) {
+				// validation check for vision attitude data
+				bool vision_att_valid = PX4_ISFINITE(vision.q[0])
+							&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
+									vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
+									fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
+											vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
+
+				if (vision_att_valid) {
+					Dcmf Rvis = Quatf(vision.q);
+					Vector3f v(1.0f, 0.0f, 0.4f);
+
+					// Rvis is Rwr (robot respect to world) while v is respect to world.
+					// Hence Rvis must be transposed having (Rwr)' * Vw
+					// Rrw * Vw = vn. This way we have consistency
+					_vision_hdg = Rvis.transpose() * v;
+
+					// vision external heading usage (ATT_EXT_HDG_M 1)
+					if (_param_att_ext_hdg_m.get() == 1) {
+						// Check for timeouts on data
+						_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
+					}
+				}
+			}
+		}
+
+		if (_mocap_odom_sub.updated()) {
+			vehicle_odometry_s mocap;
+
+			if (_mocap_odom_sub.copy(&mocap)) {
+				// validation check for mocap attitude data
+				bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
+						       && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
+								       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
+								       fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
+										       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
+
+				if (mocap_att_valid) {
+					Dcmf Rmoc = Quatf(mocap.q);
+					Vector3f v(1.0f, 0.0f, 0.4f);
+
+					// Rmoc is Rwr (robot respect to world) while v is respect to world.
+					// Hence Rmoc must be transposed having (Rwr)' * Vw
+					// Rrw * Vw = vn. This way we have consistency
+					_mocap_hdg = Rmoc.transpose() * v;
+
+					// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
+					if (_param_att_ext_hdg_m.get() == 2) {
+						// Check for timeouts on data
+						_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
+					}
+				}
+			}
+		}
+
+		if (_gps_sub.updated()) {
+			vehicle_gps_position_s gps;
+
+			if (_gps_sub.copy(&gps)) {
+				if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
+					/* set magnetic declination automatically */
+					update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
+				}
+			}
+		}
+
+		if (_local_position_sub.updated()) {
+			vehicle_local_position_s lpos;
+
+			if (_local_position_sub.copy(&lpos)) {
+
+				if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
+				    && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
+
+					/* position data is actual */
+					const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
+
+					/* velocity updated */
+					if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
+						float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
+						/* calculate acceleration in body frame */
+						_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
+					}
+
+					_vel_prev_t = lpos.timestamp;
+					_vel_prev = vel;
+
+				} else {
+					/* position data is outdated, reset acceleration */
+					_pos_acc.zero();
+					_vel_prev.zero();
+					_vel_prev_t = 0;
+				}
+			}
+		}
+
+		/* time from previous iteration */
+		hrt_abstime now = hrt_absolute_time();
+		const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
+		_last_time = now;
+
+		if (update(dt)) {
+			vehicle_attitude_s att = {};
+			att.timestamp_sample = sensors.timestamp;
+			_q.copyTo(att.q);
+
+			/* the instance count is not used here */
+			att.timestamp = hrt_absolute_time();
+			_att_pub.publish(att);
+
+		}
+	}
+}
+
+void
+AttitudeEstimatorQ::update_parameters(bool force)
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+
+		// disable mag fusion if the system does not have a mag
+		if (_param_sys_has_mag.get() == 0) {
+			_param_att_w_mag.set(0.0f);
+		}
+
+		// if the weight is zero (=mag disabled), make sure the estimator initializes
+		if (_param_att_w_mag.get() < FLT_EPSILON) {
+			_mag(0) = 1.f;
+			_mag(1) = 0.f;
+			_mag(2) = 0.f;
+		}
+
+		update_mag_declination(math::radians(_param_att_mag_decl.get()));
+	}
+}
+
+bool
+AttitudeEstimatorQ::init_attq()
+{
+	// Rotation matrix can be easily constructed from acceleration and mag field vectors
+	// 'k' is Earth Z axis (Down) unit vector in body frame
+	Vector3f k = -_accel;
+	k.normalize();
+
+	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
+	Vector3f i = (_mag - k * (_mag * k));
+	i.normalize();
+
+	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
+	Vector3f j = k % i;
+
+	// Fill rotation matrix
+	Dcmf R;
+	R.row(0) = i;
+	R.row(1) = j;
+	R.row(2) = k;
+
+	// Convert to quaternion
+	_q = R;
+
+	// Compensate for magnetic declination
+	Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
+	_q = _q * decl_rotation;
+
+	_q.normalize();
+
+	if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
+	    PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
+	    _q.length() > 0.95f && _q.length() < 1.05f) {
+		_inited = true;
+
+	} else {
+		_inited = false;
+	}
+
+	return _inited;
+}
+
+bool
+AttitudeEstimatorQ::update(float dt)
+{
+	if (!_inited) {
+
+		if (!_data_good) {
+			return false;
+		}
+
+		return init_attq();
+	}
+
+	Quatf q_last = _q;
+
+	// Angular rate of correction
+	Vector3f corr;
+	float spinRate = _gyro.length();
+
+	if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
+		if (_param_att_ext_hdg_m.get() == 1) {
+			// Vision heading correction
+			// Project heading to global frame and extract XY component
+			Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
+			float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
+		}
+
+		if (_param_att_ext_hdg_m.get() == 2) {
+			// Mocap heading correction
+			// Project heading to global frame and extract XY component
+			Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
+			float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
+		}
+	}
+
+	if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
+		// Magnetometer correction
+		// Project mag field vector to global frame and extract XY component
+		Vector3f mag_earth = _q.conjugate(_mag);
+		float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+		float gainMult = 1.0f;
+		const float fifty_dps = 0.873f;
+
+		if (spinRate > fifty_dps) {
+			gainMult = math::min(spinRate / fifty_dps, 10.0f);
+		}
+
+		// Project magnetometer correction to body frame
+		corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
+	}
+
+	_q.normalize();
+
+
+	// Accelerometer correction
+	// Project 'k' unit vector of earth frame to body frame
+	// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
+	// Optimized version with dropped zeros
+	Vector3f k(
+		2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
+		2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
+		(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
+	);
+
+	// If we are not using acceleration compensation based on GPS velocity,
+	// fuse accel data only if its norm is close to 1 g (reduces drift).
+	const float accel_norm_sq = _accel.norm_squared();
+	const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
+	const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
+
+	if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
+					  (accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
+
+		corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
+	}
+
+	// Gyro bias estimation
+	if (spinRate < 0.175f) {
+		_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
+
+		for (int i = 0; i < 3; i++) {
+			_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
+		}
+
+	}
+
+	_rates = _gyro + _gyro_bias;
+
+	// Feed forward gyro
+	corr += _rates;
+
+	// Apply correction to state
+	_q += _q.derivative1(corr) * dt;
+
+	// Normalize quaternion
+	_q.normalize();
+
+	if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
+	      PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
+
+		// Reset quaternion to last good state
+		_q = q_last;
+		_rates.zero();
+		_gyro_bias.zero();
+		return false;
+	}
+
+	return true;
+}
+
+void
+AttitudeEstimatorQ::update_mag_declination(float new_declination)
+{
+	// Apply initial declination or trivial rotations without changing estimation
+	if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
+		_mag_decl = new_declination;
+
+	} else {
+		// Immediately rotate current estimation to avoid gyro bias growth
+		Quatf decl_rotation = Eulerf(0.0f, 0.0f, new_declination - _mag_decl);
+		_q = _q * decl_rotation;
+		_mag_decl = new_declination;
+	}
+}
+
+int
+AttitudeEstimatorQ::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int
+AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
+{
+	AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int
+AttitudeEstimatorQ::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Attitude estimator q.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("AttitudeEstimatorQ", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
+{
+	return AttitudeEstimatorQ::main(argc, argv);
+}

src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 attitude_estimator_q_params.c
+ *
+ * Parameters for attitude estimator (quaternion based)
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+/**
+ * Complimentary filter accelerometer weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
+
+/**
+ * Complimentary filter magnetometer weight
+ *
+ * Set to 0 to avoid using the magnetometer.
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
+
+/**
+ * Complimentary filter external heading weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
+
+/**
+ * Complimentary filter gyroscope bias weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
+
+/**
+ * Magnetic declination, in degrees
+ *
+ * This parameter is not used in normal operation,
+ * as the declination is looked up based on the
+ * GPS coordinates of the vehicle.
+ *
+ * @group Attitude Q estimator
+ * @unit deg
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
+
+/**
+ * Automatic GPS based declination compensation
+ *
+ * @group Attitude Q estimator
+ * @boolean
+ */
+PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
+
+/**
+ * External heading usage mode (from Motion capture/Vision)
+ *
+ * Set to 1 to use heading estimate from vision.
+ * Set to 2 to use heading from motion capture.
+ *
+ * @group Attitude Q estimator
+ * @value 0 None
+ * @value 1 Vision
+ * @value 2 Motion Capture
+ * @min 0
+ * @max 2
+ */
+PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
+
+/**
+ * Acceleration compensation based on GPS velocity.
+ *
+ * @group Attitude Q estimator
+ * @boolean
+ */
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 1);
+
+/**
+ * Gyro bias limit
+ *
+ * @group Attitude Q estimator
+ * @unit rad/s
+ * @min 0
+ * @max 2
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);

src/prometheus_px4/src/modules/battery_status/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2015-2020 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_module(
+	MODULE modules__battery_status
+	MAIN battery_status
+	SRCS
+		battery_status.cpp
+		analog_battery.cpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		battery
+		conversion
+		drivers__device
+		mathlib
+	)

src/prometheus_px4/src/modules/battery_status/analog_battery.cpp

@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2021 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 <stdio.h>
+#include <lib/battery/battery.h>
+#include "analog_battery.h"
+
+// Defaults to use if the parameters are not set
+#if BOARD_NUMBER_BRICKS > 0
+#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST)
+static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
+static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
+#else
+static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
+static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
+#endif
+#else
+static constexpr int DEFAULT_V_CHANNEL[1] = {0};
+static constexpr int DEFAULT_I_CHANNEL[1] = {0};
+#endif
+
+AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us) :
+	Battery(index, parent, sample_interval_us)
+{
+	char param_name[17];
+
+	_analog_param_handles.v_offs_cur = param_find("BAT_V_OFFS_CURR");
+
+	snprintf(param_name, sizeof(param_name), "BAT%d_V_DIV", index);
+	_analog_param_handles.v_div = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "BAT%d_A_PER_V", index);
+	_analog_param_handles.a_per_v = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "BAT%d_V_CHANNEL", index);
+	_analog_param_handles.v_channel = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index);
+	_analog_param_handles.i_channel = param_find(param_name);
+
+	_analog_param_handles.v_div_old = param_find("BAT_V_DIV");
+	_analog_param_handles.a_per_v_old = param_find("BAT_A_PER_V");
+	_analog_param_handles.adc_channel_old = param_find("BAT_ADC_CHANNEL");
+}
+
+void
+AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
+				      int source, int priority, float throttle_normalized)
+{
+	float voltage_v = voltage_raw * _analog_params.v_div;
+	float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
+
+	bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
+			 (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
+
+
+	Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected,
+				     source, priority, throttle_normalized);
+}
+
+bool AnalogBattery::is_valid()
+{
+#ifdef BOARD_BRICK_VALID_LIST
+	bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
+	return valid[_index - 1];
+#else
+	// TODO: Maybe return false instead?
+	return true;
+#endif
+}
+
+int AnalogBattery::get_voltage_channel()
+{
+	if (_analog_params.v_channel >= 0) {
+		return _analog_params.v_channel;
+
+	} else {
+		return DEFAULT_V_CHANNEL[_index - 1];
+	}
+}
+
+int AnalogBattery::get_current_channel()
+{
+	if (_analog_params.i_channel >= 0) {
+		return _analog_params.i_channel;
+
+	} else {
+		return DEFAULT_I_CHANNEL[_index - 1];
+	}
+}
+
+
+void
+AnalogBattery::updateParams()
+{
+	if (_index == 1) {
+		migrateParam<float>(_analog_param_handles.v_div_old, _analog_param_handles.v_div, &_analog_params.v_div_old,
+				    &_analog_params.v_div, _first_parameter_update);
+		migrateParam<float>(_analog_param_handles.a_per_v_old, _analog_param_handles.a_per_v, &_analog_params.a_per_v_old,
+				    &_analog_params.a_per_v, _first_parameter_update);
+		migrateParam<int32_t>(_analog_param_handles.adc_channel_old, _analog_param_handles.v_channel,
+				      &_analog_params.adc_channel_old, &_analog_params.v_channel, _first_parameter_update);
+
+	} else {
+		param_get(_analog_param_handles.v_div, &_analog_params.v_div);
+		param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
+		param_get(_analog_param_handles.v_channel, &_analog_params.v_channel);
+	}
+
+	param_get(_analog_param_handles.i_channel, &_analog_params.i_channel);
+	param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur);
+
+	if (_analog_params.v_div < 0.0f) {
+		/* apply scaling according to defaults if set to default */
+		_analog_params.v_div = BOARD_BATTERY1_V_DIV;
+		param_set_no_notification(_analog_param_handles.v_div, &_analog_params.v_div);
+
+		if (_index == 1) {
+			_analog_params.v_div_old = BOARD_BATTERY1_V_DIV;
+			param_set_no_notification(_analog_param_handles.v_div_old, &_analog_params.v_div_old);
+		}
+	}
+
+	if (_analog_params.a_per_v < 0.0f) {
+		/* apply scaling according to defaults if set to default */
+
+		_analog_params.a_per_v = BOARD_BATTERY1_A_PER_V;
+		param_set_no_notification(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
+
+		if (_index == 1) {
+			_analog_params.a_per_v_old = BOARD_BATTERY1_A_PER_V;
+			param_set_no_notification(_analog_param_handles.a_per_v_old, &_analog_params.a_per_v_old);
+		}
+	}
+
+	Battery::updateParams();
+}

src/prometheus_px4/src/modules/battery_status/analog_battery.h

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2021 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <battery/battery.h>
+#include <parameters/param.h>
+
+class AnalogBattery : public Battery
+{
+public:
+	AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us);
+
+	/**
+	 * Update current battery status message.
+	 *
+	 * @param voltage_raw Battery voltage read from ADC, volts
+	 * @param current_raw Voltage of current sense resistor, volts
+	 * @param timestamp Time at which the ADC was read (use hrt_absolute_time())
+	 * @param source The source as defined by param BAT%d_SOURCE
+	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
+	 * @param throttle_normalized Throttle of the vehicle, between 0 and 1
+	 */
+	void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
+				    int source, int priority, float throttle_normalized);
+
+	/**
+	 * Whether the ADC channel for the voltage of this battery is valid.
+	 * Corresponds to BOARD_BRICK_VALID_LIST
+	 */
+	bool is_valid();
+
+	/**
+	 * Which ADC channel is used for voltage reading of this battery
+	 */
+	int get_voltage_channel();
+
+	/**
+	 * Which ADC channel is used for current reading of this battery
+	 */
+	int get_current_channel();
+
+protected:
+
+	struct {
+		param_t v_offs_cur;
+		param_t v_div;
+		param_t a_per_v;
+		param_t v_channel;
+		param_t i_channel;
+
+		param_t v_div_old;
+		param_t a_per_v_old;
+		param_t adc_channel_old;
+	} _analog_param_handles;
+
+	struct {
+		float v_offs_cur;
+		float v_div;
+		float a_per_v;
+		int32_t v_channel;
+		int32_t i_channel;
+
+		float v_div_old;
+		float a_per_v_old;
+		int32_t adc_channel_old;
+	} _analog_params;
+
+	virtual void updateParams() override;
+};

src/prometheus_px4/src/modules/battery_status/analog_battery_params_common.c

@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Offset in volt as seen by the ADC input of the current sensor.
+ *
+ * This offset will be subtracted before calculating the battery
+ * current based on the voltage.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);

src/prometheus_px4/src/modules/battery_status/analog_battery_params_deprecated.c

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+/**
+ * This parameter is deprecated. Please use BAT1_ADC_CHANNEL.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);

src/prometheus_px4/src/modules/battery_status/battery_status.cpp

@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 sensors.cpp
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomas@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/log.h>
+#include <lib/mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_adc.h>
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/battery/battery.h>
+#include <lib/conversion/rotation.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/adc_report.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include "analog_battery.h"
+
+using namespace time_literals;
+
+/**
+ * The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h
+ */
+
+#ifndef BOARD_NUMBER_BRICKS
+#error "battery_status module requires power bricks"
+#endif
+
+#if BOARD_NUMBER_BRICKS == 0
+#error "battery_status module requires power bricks"
+#endif
+
+class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	BatteryStatus();
+	~BatteryStatus() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	uORB::Subscription	_actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};		/**< attitude controls sub */
+	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};				/**< notification of parameter updates */
+	uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
+
+	static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
+	static constexpr uint32_t SAMPLE_INTERVAL_US  = 1_s / SAMPLE_FREQUENCY_HZ;
+
+	AnalogBattery _battery1;
+
+#if BOARD_NUMBER_BRICKS > 1
+	AnalogBattery _battery2;
+#endif
+
+	AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] {
+		&_battery1,
+#if BOARD_NUMBER_BRICKS > 1
+		&_battery2,
+#endif
+	}; // End _analogBatteries
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	/**
+	 * Check for changes in parameters.
+	 */
+	void 		parameter_update_poll(bool forced = false);
+
+	/**
+	 * Poll the ADC and update readings to suit.
+	 *
+	 * @param raw			Combined sensor data structure into which
+	 *				data should be returned.
+	 */
+	void		adc_poll();
+};
+
+BatteryStatus::BatteryStatus() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_battery1(1, this, SAMPLE_INTERVAL_US),
+#if BOARD_NUMBER_BRICKS > 1
+	_battery2(2, this, SAMPLE_INTERVAL_US),
+#endif
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
+{
+	updateParams();
+}
+
+BatteryStatus::~BatteryStatus()
+{
+	ScheduleClear();
+}
+
+void
+BatteryStatus::parameter_update_poll(bool forced)
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated() || forced) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+}
+
+void
+BatteryStatus::adc_poll()
+{
+	/* For legacy support we publish the battery_status for the Battery that is
+	* associated with the Brick that is the selected source for VDD_5V_IN
+	* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
+	* Like in the FMUv4
+	*/
+
+	/* Per Brick readings with default unread channels at 0 */
+	float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
+	float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
+
+	int selected_source = -1;
+
+	adc_report_s adc_report;
+
+	if (_adc_report_sub.update(&adc_report)) {
+
+		/* Read add channels we got */
+		for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
+			for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
+
+				/* Once we have subscriptions, Do this once for the lowest (highest priority
+				 * supply on power controller) that is valid.
+				 */
+				if (selected_source < 0 && _analogBatteries[b]->is_valid()) {
+					/* Indicate the lowest brick (highest priority supply on power controller)
+					 * that is valid as the one that is the selected source for the
+					 * VDD_5V_IN
+					 */
+					selected_source = b;
+				}
+
+				/* look for specific channels and process the raw voltage to measurement data */
+
+				if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
+					/* Voltage in volts */
+					bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
+								      adc_report.v_ref /
+								      adc_report.resolution;
+
+				} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
+					bat_current_adc_readings[b] = adc_report.raw_data[i] *
+								      adc_report.v_ref /
+								      adc_report.resolution;
+				}
+
+			}
+		}
+
+		for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
+
+			actuator_controls_s ctrl{};
+			_actuator_ctrl_0_sub.copy(&ctrl);
+
+			_analogBatteries[b]->updateBatteryStatusADC(
+				hrt_absolute_time(),
+				bat_voltage_adc_readings[b],
+				bat_current_adc_readings[b],
+				battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+				b,
+				ctrl.control[actuator_controls_s::INDEX_THROTTLE]
+			);
+		}
+	}
+}
+
+void
+BatteryStatus::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	/* check parameters for updates */
+	parameter_update_poll();
+
+	/* check battery voltage */
+	adc_poll();
+
+	perf_end(_loop_perf);
+}
+
+int
+BatteryStatus::task_spawn(int argc, char *argv[])
+{
+	BatteryStatus *instance = new BatteryStatus();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+bool
+BatteryStatus::init()
+{
+	return _adc_report_sub.registerCallback();
+}
+
+int BatteryStatus::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int BatteryStatus::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+The provided functionality includes:
+- Read the output from the ADC driver (via ioctl interface) and publish `battery_status`.
+
+
+### Implementation
+It runs in its own thread and polls on the currently selected gyro topic.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("battery_status", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int battery_status_main(int argc, char *argv[])
+{
+	return BatteryStatus::main(argc, argv);
+}

src/prometheus_px4/src/modules/battery_status/module.yaml

@@ -0,0 +1,63 @@
+__max_num_config_instances: &max_num_config_instances 2
+
+module_name: battery_status
+
+parameters:
+    - group: Battery Calibration
+      definitions:
+        BAT${i}_V_DIV:
+            description:
+                short: Battery ${i} voltage divider (V divider)
+                long: |
+                    This is the divider from battery ${i} voltage to ADC voltage.
+                    If using e.g. Mauch power modules the value from the datasheet
+                    can be applied straight here. A value of -1 means to use
+                    the board default.
+
+            type: float
+            decimal: 8
+            reboot_required: true
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: [-1.0, -1.0]
+
+        BAT${i}_A_PER_V:
+            description:
+                short: Battery ${i} current per volt (A/V)
+                long: |
+                    The voltage seen by the ADC multiplied by this factor
+                    will determine the battery current. A value of -1 means to use
+                    the board default.
+
+            type: float
+            decimal: 8
+            reboot_required: true
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: [-1.0, -1.0]
+
+        BAT${i}_V_CHANNEL:
+            description:
+                short: Battery ${i} Voltage ADC Channel
+                long: |
+                    This parameter specifies the ADC channel used to monitor voltage of main power battery.
+                    A value of -1 means to use the board default.
+
+            type: int32
+            reboot_required: true
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: [-1, -1]
+
+        BAT${i}_I_CHANNEL:
+            description:
+                short: Battery ${i} Current ADC Channel
+                long: |
+                    This parameter specifies the ADC channel used to monitor current of main power battery.
+                    A value of -1 means to use the board default.
+
+            type: int32
+            reboot_required: true
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: [-1, -1]

src/prometheus_px4/src/modules/camera_feedback/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2017-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_module(
+	MODULE modules__camera_feedback
+	MAIN camera_feedback
+	SRCS
+		CameraFeedback.cpp
+		CameraFeedback.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.cpp

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-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 "CameraFeedback.hpp"
+
+CameraFeedback::CameraFeedback() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+}
+
+bool
+CameraFeedback::init()
+{
+	if (!_trigger_sub.registerCallback()) {
+		PX4_ERR("camera_trigger callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+CameraFeedback::Run()
+{
+	if (should_exit()) {
+		_trigger_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	camera_trigger_s trig{};
+
+	if (_trigger_sub.update(&trig)) {
+
+		// update geotagging subscriptions
+		vehicle_global_position_s gpos{};
+		_gpos_sub.copy(&gpos);
+
+		vehicle_attitude_s att{};
+		_att_sub.copy(&att);
+
+		if (trig.timestamp == 0 ||
+		    gpos.timestamp == 0 ||
+		    att.timestamp == 0) {
+
+			// reject until we have valid data
+			return;
+		}
+
+		camera_capture_s capture{};
+
+		// Fill timestamps
+		capture.timestamp = trig.timestamp;
+		capture.timestamp_utc = trig.timestamp_utc;
+
+		// Fill image sequence
+		capture.seq = trig.seq;
+
+		// Fill position data
+		capture.lat = gpos.lat;
+		capture.lon = gpos.lon;
+		capture.alt = gpos.alt;
+
+		if (gpos.terrain_alt_valid) {
+			capture.ground_distance = gpos.alt - gpos.terrain_alt;
+
+		} else {
+			capture.ground_distance = -1.0f;
+		}
+
+		// Fill attitude data
+		// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
+		capture.q[0] = att.q[0];
+		capture.q[1] = att.q[1];
+		capture.q[2] = att.q[2];
+		capture.q[3] = att.q[3];
+		capture.result = 1;
+
+		_capture_pub.publish(capture);
+	}
+}
+
+int
+CameraFeedback::task_spawn(int argc, char *argv[])
+{
+	CameraFeedback *instance = new CameraFeedback();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int
+CameraFeedback::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int
+CameraFeedback::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("camera_feedback", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[])
+{
+	return CameraFeedback::main(argc, argv);
+}

src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.hpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-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.
+ *
+ ****************************************************************************/
+
+/**
+ *
+ * Online and offline geotagging from camera feedback
+ *
+ * @author Mohammed Kabir <kabir@uasys.io>
+ */
+
+#pragma once
+
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/camera_capture.h>
+#include <uORB/topics/camera_trigger.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
+{
+public:
+	CameraFeedback();
+	~CameraFeedback() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+
+	void Run() override;
+
+	uORB::SubscriptionCallbackWorkItem _trigger_sub{this, ORB_ID(camera_trigger)};
+
+	uORB::Subscription	_gpos_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription	_att_sub{ORB_ID(vehicle_attitude)};
+
+	uORB::Publication<camera_capture_s>	_capture_pub{ORB_ID(camera_capture)};
+};

src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp

@@ -0,0 +1,298 @@
+/****************************************************************************
+ *
+ *   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 <lib/parameters/param.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+using namespace time_literals;
+
+static orb_advert_t *mavlink_log_pub;
+static int command_ack_sub = -1;
+
+static hrt_abstime auth_timeout;
+static hrt_abstime auth_req_time;
+
+static hrt_abstime _param_com_arm_auth_timout;
+static enum arm_auth_methods _param_com_arm_auth_method;
+static int32_t _param_com_arm_auth_id;
+
+static enum {
+	ARM_AUTH_IDLE = 0,
+	ARM_AUTH_WAITING_AUTH,
+	ARM_AUTH_WAITING_AUTH_WITH_ACK,
+	ARM_AUTH_MISSION_APPROVED
+} state = ARM_AUTH_IDLE;
+
+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,
+};
+
+void arm_auth_param_update()
+{
+	float timeout = 0;
+	param_get(param_find("COM_ARM_AUTH_TO"), &timeout);
+	_param_com_arm_auth_timout = timeout * 1_s;
+
+	int32_t auth_method = ARM_AUTH_METHOD_ARM_REQ;
+	param_get(param_find("COM_ARM_AUTH_MET"), &auth_method);
+
+	if (auth_method >= 0 && auth_method < ARM_AUTH_METHOD_LAST) {
+		_param_com_arm_auth_method = (arm_auth_methods)auth_method;
+
+	} else {
+		_param_com_arm_auth_method = ARM_AUTH_METHOD_ARM_REQ;
+	}
+
+	param_get(param_find("COM_ARM_AUTH_ID"), &_param_com_arm_auth_id);
+}
+
+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 = _param_com_arm_auth_id;
+
+	uORB::Publication<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_req_time = now;
+	auth_timeout = now + _param_com_arm_auth_timout;
+	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_req_time = now;
+	auth_timeout = now + _param_com_arm_auth_timout;
+	state = ARM_AUTH_WAITING_AUTH;
+
+	mavlink_log_info(mavlink_log_pub, "Arm auth: Requesting authorization...");
+
+	return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+}
+
+uint8_t arm_auth_check()
+{
+	if (_param_com_arm_auth_method < ARM_AUTH_METHOD_LAST) {
+		return arm_check_method[_param_com_arm_auth_method]();
+	}
+
+	return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+}
+
+void arm_auth_update(hrt_abstime now, bool param_update)
+{
+	if (param_update) {
+		arm_auth_param_update();
+	}
+
+	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
+	    && command_ack.timestamp > auth_req_time) {
+		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_info(mavlink_log_pub,
+					 "Arm auth: Authorized for the next %" PRId32 " seconds",
+					 command_ack.result_param2);
+			state = ARM_AUTH_MISSION_APPROVED;
+			auth_timeout = command_ack.timestamp + (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 %" PRId32 " 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)
+{
+	arm_auth_param_update();
+	system_id = sys_id;
+	command_ack_sub = orb_subscribe(ORB_ID(vehicle_command_ack));
+	mavlink_log_pub = mav_log_pub;
+}

src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   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();

src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt

@@ -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}
+)

src/prometheus_px4/src/modules/commander/Arming/CMakeLists.txt

@@ -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)

src/prometheus_px4/src/modules/commander/Arming/HealthFlags/CMakeLists.txt

@@ -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})

src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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", (long long unsigned int)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);
+}
+
+void _print_sub(const char *name, const vehicle_status_s &status, uint32_t bit)
+{
+	PX4_INFO("%s:\t\t%s\t%s", name,
+		 (status.onboard_control_sensors_enabled & bit) ? "EN" : " ",
+		 (status.onboard_control_sensors_present & bit) ?
+		 ((status.onboard_control_sensors_health & bit) ? "OK" : "ERR") :
+		 (status.onboard_control_sensors_enabled & bit) ? "OFF" : "");
+}
+
+void print_health_flags(const vehicle_status_s &status)
+{
+#ifndef CONSTRAINED_FLASH
+	PX4_INFO("DEVICE\t\tSTATUS");
+	PX4_INFO("----------------------------------");
+	_print_sub("GYRO", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
+	_print_sub("ACC", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC);
+	_print_sub("MAG", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG);
+	_print_sub("PRESS", status, subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
+	_print_sub("AIRSP", status, subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE);
+	_print_sub("GPS", status, subsystem_info_s::SUBSYSTEM_TYPE_GPS);
+	_print_sub("OPT", status, subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW);
+	_print_sub("VIO", status, subsystem_info_s::SUBSYSTEM_TYPE_CVPOSITION);
+	_print_sub("LASER", status, subsystem_info_s::SUBSYSTEM_TYPE_LASERPOSITION);
+	_print_sub("GTRUTH", status, subsystem_info_s::SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH);
+	_print_sub("RATES", status, subsystem_info_s::SUBSYSTEM_TYPE_ANGULARRATECONTROL);
+	_print_sub("ATT", status, subsystem_info_s::SUBSYSTEM_TYPE_ATTITUDESTABILIZATION);
+	_print_sub("YAW", status, subsystem_info_s::SUBSYSTEM_TYPE_YAWPOSITION);
+	_print_sub("ALTCTL", status, subsystem_info_s::SUBSYSTEM_TYPE_ALTITUDECONTROL);
+	_print_sub("POS", status, subsystem_info_s::SUBSYSTEM_TYPE_POSITIONCONTROL);
+	_print_sub("MOT", status, subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL);
+	_print_sub("RC  ", status, subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER);
+	_print_sub("GYRO2", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO2);
+	_print_sub("ACC2", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC2);
+	_print_sub("MAG2", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG2);
+	_print_sub("GEOFENCE", status, subsystem_info_s::SUBSYSTEM_TYPE_GEOFENCE);
+	_print_sub("AHRS", status, subsystem_info_s::SUBSYSTEM_TYPE_AHRS);
+	_print_sub("TERRAIN", status, subsystem_info_s::SUBSYSTEM_TYPE_TERRAIN);
+	_print_sub("REVMOT", status, subsystem_info_s::SUBSYSTEM_TYPE_REVERSEMOTOR);
+	_print_sub("LOGGIN", status, subsystem_info_s::SUBSYSTEM_TYPE_LOGGING);
+	_print_sub("BATT", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY);
+	_print_sub("PROX", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY);
+	_print_sub("SATCOM", status, subsystem_info_s::SUBSYSTEM_TYPE_SATCOM);
+	_print_sub("PREARM", status, subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK);
+	_print_sub("OBSAVD", status, subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE);
+#endif
+}

src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.h

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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>
+
+struct subsystem_info_s {
+	// keep in sync with mavlink MAV_SYS_STATUS_SENSOR
+	static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0;
+	static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1;
+	static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2;
+	static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3;
+	static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4;
+	static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5;
+	static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6;
+	static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7;
+	static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8;
+	static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9;
+	static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10;
+	static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11;
+	static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12;
+	static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13;
+	static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14;
+	static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15;
+	static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16;
+	static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17;
+	static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18;
+	static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19;
+	static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20;
+	static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21;
+	static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22;
+	static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23;
+	static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24;
+	static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25;
+	static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26;
+	static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27;
+	static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28;
+	static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29;
+};
+
+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);
+void print_health_flags(const vehicle_status_s &status);

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt

@@ -0,0 +1,54 @@
+############################################################################
+#
+#   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/airframeCheck.cpp
+	checks/ekf2Check.cpp
+	checks/failureDetectorCheck.cpp
+	checks/manualControlCheck.cpp
+	checks/cpuResourceCheck.cpp
+	checks/sdcardCheck.cpp
+)
+target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)

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

@@ -0,0 +1,267 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 <lib/parameters/param.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+
+using namespace time_literals;
+
+static constexpr unsigned max_mandatory_gyro_count = 1;
+static constexpr unsigned max_optional_gyro_count = 4;
+static constexpr unsigned max_mandatory_accel_count = 1;
+static constexpr unsigned max_optional_accel_count = 4;
+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 = 4;
+
+bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+				    vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm,
+				    const hrt_abstime &time_since_boot)
+{
+	report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
+			   && !status_flags.condition_calibration_enabled);
+
+	bool failed = false;
+
+	failed = failed || !airframeCheck(mavlink_log_pub, status);
+	failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures);
+
+	/* ---- MAG ---- */
+	{
+		int32_t sys_has_mag = 1;
+		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
+
+		if (sys_has_mag == 1) {
+
+			/* 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);
+				bool report_fail = report_failures;
+
+				int32_t device_id = -1;
+
+				if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+					if (required) {
+						failed = true;
+					}
+
+					report_fail = false; // only report the first failure
+				}
+			}
+
+			// TODO: highest priority mag
+
+			/* mag consistency checks (need to be performed after the individual checks) */
+			if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- ACCEL ---- */
+	{
+		/* 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);
+			bool report_fail = report_failures;
+
+			int32_t device_id = -1;
+
+			if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+				if (required) {
+					failed = true;
+				}
+
+				report_fail = false; // only report the first failure
+			}
+		}
+
+		// TODO: highest priority (from params)
+	}
+
+	/* ---- GYRO ---- */
+	{
+		/* 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);
+			bool report_fail = report_failures;
+
+			int32_t device_id = -1;
+
+			if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+				if (required) {
+					failed = true;
+				}
+
+				report_fail = false; // only report the first failure
+			}
+		}
+
+		// TODO: highest priority (from params)
+	}
+
+	/* ---- BARO ---- */
+	{
+		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);
+			bool report_fail = (required && report_failures && !baro_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+				if (required) {
+					baro_fail_reported = true;
+				}
+
+				report_fail = false; // only report the first failure
+			}
+		}
+	}
+
+	/* ---- IMU CONSISTENCY ---- */
+	// To be performed after the individual sensor checks have completed
+	{
+		if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) {
+			failed = true;
+		}
+	}
+
+	/* ---- AIRSPEED ---- */
+	/* 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)) {
+
+		int32_t airspeed_mode = 0;
+		param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
+		const bool optional = (airspeed_mode == 1);
+
+		int32_t max_airspeed_check_en = 0;
+		param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);
+
+		float airspeed_trim = 10.0f;
+		param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim);
+
+		const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
+
+		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en,
+				   arming_max_airspeed_allowed)
+		    && !(bool)optional) {
+			failed = true;
+		}
+	}
+
+	/* ---- RC CALIBRATION ---- */
+	if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) {
+		if (rcCalibrationCheck(mavlink_log_pub, report_failures, status.is_vtol) != OK) {
+			if (report_failures) {
+				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 (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
+		if (!powerCheck(mavlink_log_pub, status, report_failures, 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) {
+
+		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) &&
+					 ekf2CheckSensorBias(mavlink_log_pub, report_failures);
+
+		// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
+		// as not present.
+		// After that or if report_failures is true, we'll set the flags as is.
+
+		if (!ekf_healthy && time_since_boot < 10_s && !report_failures) {
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
+
+		} else {
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
+		}
+
+		failed |= !ekf_healthy;
+	}
+
+	/* ---- Failure Detector ---- */
+	if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) {
+		failed = true;
+	}
+
+	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
+	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
+
+	/* Report status */
+	return !failed;
+}

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

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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, bool reportFailures, const bool prearm,
+				   const hrt_abstime &time_since_boot);
+
+	struct arm_requirements_t {
+		bool arm_authorization = false;
+		bool esc_check = false;
+		bool global_position = false;
+		bool mission = false;
+		bool geofence = false;
+	};
+
+	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
+				const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
+				bool report_fail = true);
+
+private:
+	static bool magnetometerCheck(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, 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, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);
+	static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL);
+	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+			       const bool prearm);
+	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
+			      const bool report_fail);
+
+	static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
+
+	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+					 const bool prearm);
+
+	static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
+	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
+	static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
+	static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
+};

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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 <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_accel.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, 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 valid = true;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
+
+		valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
+
+		if (!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;
+
+		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			calibration_valid = true;
+
+		} else {
+			calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
+		}
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
+			}
+
+		} else {
+			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 fatal
+				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 && valid;
+
+	return success;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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>
+
+using namespace time_literals;
+
+bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status)
+{
+	bool success = true;
+
+#ifdef CONFIG_ARCH_BOARD_PX4_FMU_V2
+
+	// We no longer support VTOL on fmu-v2, so we need to warn existing users.
+	if (status.is_vtol) {
+		mavlink_log_critical(mavlink_log_pub,
+				     "VTOL is not supported with fmu-v2, see docs.px4.io/master/en/config/firmware.html#bootloader");
+		success = false;
+	}
+
+#endif
+
+	return success;
+}

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

@@ -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_validated.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
+				   const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed)
+{
+	bool present = true;
+	bool success = true;
+
+	uORB::SubscriptionData<airspeed_validated_s> airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	airspeed_validated_sub.update();
+	const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get();
+
+	/*
+	 * Check if Airspeed Selector is up and running.
+	 */
+	if (hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
+		if (report_fail && !optional) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down.");
+		}
+
+		present = false;
+		success = false;
+		goto out;
+	}
+
+	/*
+	 * Check if airspeed is declared valid or not by airspeed selector.
+	 */
+	if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid.");
+		}
+
+		present = true;
+		success = false;
+		goto out;
+	}
+
+	/*
+	 * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
+	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
+	 * might have been removed.
+	 */
+	if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed
+	    && prearm) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot");
+		}
+
+		present = true;
+		success = false;
+		goto out;
+	}
+
+out:
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
+
+	return success;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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>
+
+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 valid = false;
+
+	if (exists) {
+		uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
+
+		valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0);
+
+		if (!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, valid, status);
+	}
+
+	return valid;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <lib/parameters/param.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/cpuload.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
+{
+	bool success = true;
+
+	uORB::SubscriptionData<cpuload_s> cpuload_sub{ORB_ID(cpuload)};
+	cpuload_sub.update();
+
+	float cpuload_percent_max;
+	param_get(param_find("COM_CPU_MAX"), &cpuload_percent_max);
+
+	if (cpuload_percent_max > 0.f) {
+
+		if (hrt_elapsed_time(&cpuload_sub.get().timestamp) > 2_s) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Fail: No CPU load information");
+			}
+		}
+
+		const float cpuload_percent = cpuload_sub.get().load * 100.f;
+
+		if (cpuload_percent > cpuload_percent_max) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
+			}
+		}
+	}
+
+	return success;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -0,0 +1,287 @@
+/****************************************************************************
+ *
+ *   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 <lib/parameters/param.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_selector_status.h>
+#include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/estimator_status.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
+			       const bool report_fail)
+{
+	bool success = true; // start with a pass and change to a fail if any test fails
+
+	int32_t mag_strength_check_enabled = 1;
+	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
+
+	float hgt_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
+
+	float vel_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit);
+
+	float pos_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit);
+
+	float mag_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
+
+	int32_t arm_without_gps = 0;
+	param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
+
+	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_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
+	const estimator_status_s &status = status_sub.get();
+
+	if (status.timestamp == 0) {
+		success = 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_vert) {
+				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;
+	}
+
+	if ((mag_strength_check_enabled == 1) && status.pre_flt_fail_mag_field_disturbed) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check vertical position innovation test ratio
+	if (status.hgt_test_ratio > hgt_test_ratio_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check velocity innovation test ratio
+	if (status.vel_test_ratio > vel_test_ratio_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
+	if (status.pos_test_ratio > pos_test_ratio_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check magnetometer innovation test ratio
+	if (status.mag_test_ratio > mag_test_ratio_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+	{
+		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_MAX_PDOP)) {
+					message = "Preflight%s: GPS PDOP too high";
+
+				} 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 (!arm_without_gps) {
+						mavlink_log_critical(mavlink_log_pub, message, " Fail");
+
+					} else {
+						mavlink_log_warning(mavlink_log_pub, message, "");
+					}
+				}
+			}
+
+			gps_success = false;
+
+			if (!arm_without_gps) {
+				success = false;
+				goto out;
+			}
+		}
+	}
+
+out:
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status);
+	return success;
+}
+
+bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail)
+{
+	// Get estimator states data if available and exit with a fail recorded if not
+	uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::SubscriptionData<estimator_sensor_bias_s> estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance};
+	const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get();
+
+	if (hrt_elapsed_time(&bias.timestamp) < 30_s) {
+
+		// check accelerometer bias estimates
+		if (bias.accel_bias_valid) {
+			const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit;
+
+			for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
+				// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+				// adjust test threshold by 3-sigma
+				const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f));
+
+				if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
+					if (report_fail) {
+						PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
+							(double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty);
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
+					}
+
+					return false;
+				}
+			}
+		}
+
+		// check gyro bias estimates
+		if (bias.gyro_bias_valid) {
+			const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
+
+			for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
+				// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+				// adjust test threshold by 3-sigma
+				const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f));
+
+				if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
+					if (report_fail) {
+						PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
+							(double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty);
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
+					}
+
+					return false;
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
+			}
+		}
+
+		return false;
+	}
+
+	return true;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_gyro.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 valid = false;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
+
+		valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
+
+		if (!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;
+
+		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			calibration_valid = true;
+
+		} else {
+			calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
+		}
+
+		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);
+		}
+	}
+
+	return calibration_valid && valid;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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 <lib/parameters/param.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensors_status_imu.h>
+
+bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+		const bool report_status)
+{
+	float accel_test_limit = 1.f;
+	param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
+
+	float gyro_test_limit = 1.f;
+	param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
+
+	// Get sensor_preflight data if available and exit with a fail recorded if not
+	uORB::SubscriptionData<sensors_status_imu_s> sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
+	const sensors_status_imu_s &imu = sensors_status_imu_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.
+	for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
+		if (imu.accel_device_ids[i] != 0) {
+			if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status);
+
+			} else {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
+			}
+
+			const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
+
+			if (accel_inconsistency_m_s_s > accel_test_limit) {
+				if (report_status) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i);
+
+					if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
+
+					} else {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
+					}
+				}
+
+				return false;
+
+			} else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) {
+				if (report_status) {
+					mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i);
+				}
+			}
+		}
+	}
+
+	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
+	for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
+		if (imu.gyro_device_ids[i] != 0) {
+			if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status);
+
+			} else {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
+			}
+
+			const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
+
+			if (gyro_inconsistency_rad_s > gyro_test_limit) {
+				if (report_status) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i);
+
+					if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
+
+					} else {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
+					}
+				}
+
+				return false;
+
+			} else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) {
+				if (report_status) {
+					mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i);
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 <lib/parameters/param.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_preflight_mag.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_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
+	sensors_sub.update();
+	const sensor_preflight_mag_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 = 90;
+	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;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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 <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/sensor_mag.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::magnetometerCheck(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 valid = false;
+	bool is_mag_fault = false;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
+
+		valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
+
+		if (!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;
+
+		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			calibration_valid = true;
+
+		} else {
+			calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
+		}
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
+			}
+		}
+
+		for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
+
+			if (estimator_status_sub.get().mag_device_id == static_cast<uint32_t>(device_id)) {
+				if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) {
+					is_mag_fault = true;
+					break;
+				}
+			}
+		}
+
+		if (is_mag_fault && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
+		}
+	}
+
+	const bool success = calibration_valid && valid && !is_mag_fault;
+
+	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;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_switches.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
+{
+	bool success = true;
+
+	uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	const manual_control_switches_s &manual_control_switches = manual_control_switches_sub.get();
+
+	if (manual_control_switches.timestamp != 0) {
+
+		// check action switches
+		if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Failure: RTL switch engaged");
+			}
+		}
+
+		if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Failure: Kill switch engaged");
+			}
+		}
+
+		if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Failure: Landing gear switch set in UP position");
+			}
+		}
+
+	}
+
+	return success;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 <lib/parameters/param.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/system_power.h>
+
+using namespace time_literals;
+
+unsigned int countSetBits(unsigned int n)
+{
+	unsigned int count = 0;
+
+	while (n) {
+		count += n & 1;
+		n >>= 1;
+	}
+
+	return count;
+}
+
+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;
+	}
+
+	if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+		// Ignore power check in HITL.
+		return true;
+	}
+
+	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 (system_power.timestamp != 0) {
+		int32_t required_power_module_count = 0;
+		param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
+
+		// Check avionics rail voltages (if USB isn't connected)
+		if (!system_power.usb_connected) {
+			float avionics_power_rail_voltage = system_power.voltage5v_v;
+
+			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.8f) {
+				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);
+				}
+			}
+
+
+			const int power_module_count = countSetBits(system_power.brick_valid);
+
+			if (power_module_count < required_power_module_count) {
+				success = false;
+
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "Power redundancy not met: %d instead of %" PRId32,
+							     power_module_count, required_power_module_count);
+				}
+			}
+		}
+
+	} else {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "system power unavailable");
+		}
+
+		success = false;
+	}
+
+	return success;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@@ -0,0 +1,190 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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>
+#include <HealthFlags.h>
+
+bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
+				 const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
+{
+	bool prearm_ok = true;
+
+	// USB not connected
+	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
+		if (report_fail) { 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) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); }
+
+			prearm_ok = false;
+		}
+
+		// main battery level
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
+				 status_flags.condition_battery_healthy, status);
+
+		// Only arm if healthy
+		if (!status_flags.condition_battery_healthy) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); }
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: mission
+	if (arm_requirements.mission) {
+
+		if (!status_flags.condition_auto_mission_available) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); }
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!status_flags.condition_global_position_valid) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); }
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) {
+
+		if (!status_flags.condition_global_position_valid) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); }
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!status_flags.condition_home_position_valid) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Home position invalid"); }
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// safety button
+	if (safety.safety_switch_available && !safety.safety_off) {
+		// Fail transition if we need safety switch press
+		if (prearm_ok) {
+			if (report_fail) { 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) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); }
+		}
+
+		prearm_ok = false;
+
+	}
+
+	if (arm_requirements.esc_check && status_flags.condition_escs_error) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); }
+
+			prearm_ok = false;
+		}
+	}
+
+	if (arm_requirements.esc_check && status_flags.condition_escs_failure) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }
+
+			prearm_ok = false;
+		}
+	}
+
+	if (status.is_vtol) {
+
+		if (status.in_transition_mode) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is in transition state"); }
+
+				prearm_ok = false;
+			}
+		}
+
+		if (!status_flags.circuit_breaker_vtol_fw_arming_check
+		    && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is not in multicopter mode"); }
+
+				prearm_ok = false;
+			}
+		}
+	}
+
+	if (arm_requirements.geofence && status.geofence_violated) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle outside geofence");
+		}
+
+		prearm_ok = false;
+	}
+
+	// Arm Requirements: authorization
+	// check last, and only if everything else has passed
+	if (arm_requirements.arm_authorization && 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;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp

@@ -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;
+}

src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 <lib/parameters/param.h>
+#include <systemlib/mavlink_log.h>
+
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
+
+bool PreFlightCheck::sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once,
+				 const bool report_fail)
+{
+	bool success = true;
+
+	int32_t param_com_arm_sdcard{0};
+	param_get(param_find("COM_ARM_SDCARD"), &param_com_arm_sdcard);
+
+	if (param_com_arm_sdcard > 0) {
+		struct statfs statfs_buf;
+
+		if (!sd_card_detected_once && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
+			// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
+			sd_card_detected_once = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
+		}
+
+		if (!sd_card_detected_once) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Warning! Missing FMU SD Card.");
+			}
+
+			if (param_com_arm_sdcard == 2) {
+				// disallow arming without sd card
+				success = false;
+			}
+		}
+	}
+
+	return success;
+}

src/prometheus_px4/src/modules/commander/CMakeLists.txt

@@ -0,0 +1,73 @@
+############################################################################
+#
+#   Copyright (c) 2015 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(failure_detector)
+add_subdirectory(Arming)
+
+px4_add_module(
+	MODULE modules__commander
+	MAIN commander
+	COMPILE_FLAGS
+	SRCS
+		accelerometer_calibration.cpp
+		airspeed_calibration.cpp
+		calibration_routines.cpp
+		Commander.cpp
+		commander_helper.cpp
+		esc_calibration.cpp
+		factory_calibration_storage.cpp
+		gyro_calibration.cpp
+		level_calibration.cpp
+		lm_fit.cpp
+		mag_calibration.cpp
+		ManualControl.cpp
+		rc_calibration.cpp
+		state_machine_helper.cpp
+		worker_thread.cpp
+	DEPENDS
+		circuit_breaker
+		failure_detector
+		git_ecl
+		ecl_geo
+		hysteresis
+		PreFlightCheck
+		ArmAuthorization
+		HealthFlags
+		sensor_calibration
+	)
+
+if(PX4_TESTING)
+	add_subdirectory(commander_tests)
+endif()
+
+px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)

src/prometheus_px4/src/modules/commander/Commander.hpp

@@ -0,0 +1,444 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017, 2021 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/*   Helper classes  */
+#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
+#include "failure_detector/FailureDetector.hpp"
+#include "ManualControl.hpp"
+#include "state_machine_helper.h"
+#include "worker_thread.hpp"
+
+#include <lib/controllib/blocks.hpp>
+#include <lib/hysteresis/hysteresis.h>
+#include <lib/mathlib/mathlib.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+
+// publications
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/landing_gear.h>
+#include <uORB/topics/test_motor.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+// subscriptions
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/cpuload.h>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/estimator_selector_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/geofence_result.h>
+#include <uORB/topics/iridiumsbd_status.h>
+#include <uORB/topics/manual_control_switches.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/power_button_state.h>
+#include <uORB/topics/rtl_flight_time.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+
+using math::constrain;
+using systemlib::Hysteresis;
+
+using namespace time_literals;
+
+class Commander : public ModuleBase<Commander>, public ModuleParams
+{
+public:
+	Commander();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static Commander *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void enable_hil();
+
+	void get_circuit_breaker_params();
+
+private:
+	void answer_command(const vehicle_command_s &cmd, uint8_t result);
+
+	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
+	transition_result_t disarm(arm_disarm_reason_t calling_reason);
+	transition_result_t try_mode_change(main_state_t desired_mode);
+
+	void battery_status_check();
+
+	bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
+				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
+				   const bool was_valid);
+
+	void control_status_leds(bool changed, const uint8_t battery_warning);
+
+	/**
+	 * Checks the status of all available data links and handles switching between different system telemetry states.
+	 */
+	void data_link_check();
+
+	void avoidance_check();
+
+	void esc_status_check();
+
+	void estimator_check();
+
+	bool handle_command(const vehicle_command_s &cmd);
+
+	unsigned handle_command_motor_test(const vehicle_command_s &cmd);
+
+	void mission_init();
+
+	void offboard_control_update();
+
+	void print_reject_mode(uint8_t main_state);
+
+	void reset_posvel_validity();
+
+	bool set_home_position();
+	bool set_home_position_alt_only();
+	bool set_in_air_home_position();
+	bool isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const;
+	void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const;
+	void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const;
+	void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const;
+	void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const;
+	void setHomePosValid();
+	void updateHomePositionYaw(float yaw);
+
+	void update_control_mode();
+
+	void UpdateEstimateValidity();
+
+	// Set the main system state based on RC and override device inputs
+	transition_result_t set_main_state(bool &changed);
+
+	// Enable override (manual reversion mode) on the system
+	transition_result_t set_main_state_override_on(bool &changed);
+
+	// Set the system main state based on the current RC inputs
+	transition_result_t set_main_state_rc();
+
+	bool shutdown_if_allowed();
+
+	bool stabilization_required();
+
+	void send_parachute_command();
+
+	DEFINE_PARAMETERS(
+
+		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
+		(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
+
+		(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
+		(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
+
+		(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
+		(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
+		(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
+
+		(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
+		(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,
+		(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
+
+		(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
+		(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, 	/*Not realy used for now*/
+		(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
+		(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,	/* failsafe response to loss of navigation accuracy */
+
+		(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
+		(ParamInt<px4::params::COM_POS_FS_PROB>) _param_com_pos_fs_prob,
+		(ParamInt<px4::params::COM_POS_FS_GAIN>) _param_com_pos_fs_gain,
+
+		(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
+		(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
+		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
+
+		(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
+
+		(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
+
+		(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
+
+		// Offboard
+		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
+		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
+		(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
+
+		(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
+		(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
+
+		(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
+		(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
+
+		// Engine failure
+		(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,
+		(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
+		(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
+
+		(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
+		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
+		(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
+		(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
+		(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
+
+		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
+		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
+
+		(ParamInt<px4::params::COM_RC_IN_MODE>) _param_rc_in_off,
+
+		(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
+		(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
+		(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
+		(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
+		(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
+		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
+
+		// Circuit breakers
+		(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
+		(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
+		(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
+		(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
+		(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
+		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
+		(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
+
+		// Geofrence
+		(ParamInt<px4::params::GF_ACTION>) _param_geofence_action,
+
+		// Mavlink
+		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
+		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
+
+		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist,
+
+		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
+		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr
+	)
+
+	enum class PrearmedMode {
+		DISABLED = 0,
+		SAFETY_BUTTON = 1,
+		ALWAYS = 2
+	};
+
+	/* Decouple update interval and hysteresis counters, all depends on intervals */
+	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
+
+	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
+	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
+
+	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */
+	const int64_t POSVEL_PROBATION_MAX = 100_s;	/**< maximum probation duration (usec) */
+
+	PreFlightCheck::arm_requirements_t	_arm_requirements{};
+
+	hrt_abstime	_valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */
+
+	hrt_abstime	_last_gpos_fail_time_us{0};	/**< Last time that the global position validity recovery check failed (usec) */
+	hrt_abstime	_last_lpos_fail_time_us{0};	/**< Last time that the local position validity recovery check failed (usec) */
+	hrt_abstime	_last_lvel_fail_time_us{0};	/**< Last time that the local velocity validity recovery check failed (usec) */
+
+	// Probation times for position and velocity validity checks to pass if failed
+	hrt_abstime	_gpos_probation_time_us = POSVEL_PROBATION_MIN;
+	hrt_abstime	_lpos_probation_time_us = POSVEL_PROBATION_MIN;
+	hrt_abstime	_lvel_probation_time_us = POSVEL_PROBATION_MIN;
+
+	/* class variables used to check for navigation failure after takeoff */
+	hrt_abstime	_time_last_innov_pass{0};	/**< last time velocity and position innovations passed */
+	hrt_abstime	_time_last_innov_fail{0};	/**< last time velocity and position innovations failed */
+	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
+	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */
+
+	bool		_geofence_loiter_on{false};
+	bool		_geofence_rtl_on{false};
+	bool		_geofence_land_on{false};
+	bool		_geofence_warning_action_on{false};
+	bool		_geofence_violated_prev{false};
+
+	FailureDetector	_failure_detector;
+	bool		_flight_termination_triggered{false};
+	bool		_lockdown_triggered{false};
+
+
+	hrt_abstime	_datalink_last_heartbeat_gcs{0};
+	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};
+	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
+	bool		_onboard_controller_lost{false};
+	bool		_avoidance_system_lost{false};
+
+	hrt_abstime	_high_latency_datalink_heartbeat{0};
+	hrt_abstime	_high_latency_datalink_lost{0};
+
+	int		_last_esc_online_flags{-1};
+	int		_last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {};
+	hrt_abstime	_last_esc_status_updated{0};
+
+	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
+	float		_battery_current{0.0f};
+
+	Hysteresis	_auto_disarm_landed{false};
+	Hysteresis	_auto_disarm_killed{false};
+	Hysteresis	_offboard_available{false};
+
+	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
+
+	bool		_last_condition_local_altitude_valid{false};
+	bool		_last_condition_local_position_valid{false};
+	bool		_last_condition_global_position_valid{false};
+
+	bool		_last_overload{false};
+
+	unsigned int	_leds_counter{0};
+
+	manual_control_switches_s _manual_control_switches{};
+	manual_control_switches_s _last_manual_control_switches{};
+	ManualControl _manual_control{this};
+	hrt_abstime	_rc_signal_lost_timestamp{0};		///< Time at which the RC reception was lost
+	int32_t		_flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
+
+	hrt_abstime	_boot_timestamp{0};
+	hrt_abstime	_last_disarmed_timestamp{0};
+	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
+	hrt_abstime	_overload_start{0};		///< time when CPU overload started
+
+	uint32_t	_counter{0};
+	uint8_t		_heading_reset_counter{0};
+
+	bool		_status_changed{true};
+	bool		_arm_tune_played{false};
+	bool		_was_armed{false};
+	bool		_failsafe_old{false};	///< check which state machines for changes, clear "changed" flag
+	bool		_have_taken_off_since_arming{false};
+	bool		_should_set_home_on_takeoff{true};
+	bool		_system_power_usb_connected{false};
+
+	cpuload_s		_cpuload{};
+	geofence_result_s	_geofence_result{};
+	vehicle_land_detected_s	_land_detector{};
+	safety_s		_safety{};
+	vtol_vehicle_status_s	_vtol_status{};
+
+	// commander publications
+	actuator_armed_s        _armed{};
+	commander_state_s       _internal_state{};
+	vehicle_control_mode_s  _vehicle_control_mode{};
+	vehicle_status_s        _status{};
+	vehicle_status_flags_s  _status_flags{};
+
+	WorkerThread _worker_thread;
+
+	// Subscriptions
+	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
+	uORB::Subscription					_cmd_sub {ORB_ID(vehicle_command)};
+	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
+	uORB::Subscription                                      _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
+	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
+	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription					_safety_sub{ORB_ID(safety)};
+	uORB::Subscription					_manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
+	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription					_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+
+	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
+	uORB::SubscriptionMultiArray<distance_sensor_s>         _distance_sensor_subs{ORB_ID::distance_sensor};
+	uORB::SubscriptionMultiArray<telemetry_status_s>        _telemetry_status_subs{ORB_ID::telemetry_status};
+
+#if defined(BOARD_HAS_POWER_CONTROL)
+	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
+#endif // BOARD_HAS_POWER_CONTROL
+
+	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
+	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
+	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<rtl_flight_time_s>		_rtl_flight_time_sub{ORB_ID(rtl_flight_time)};
+
+	// Publications
+	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};
+	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
+	uORB::Publication<test_motor_s>				_test_motor_pub{ORB_ID(test_motor)};
+	uORB::Publication<vehicle_control_mode_s>		_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
+	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};
+	uORB::Publication<mission_s>				_mission_pub{ORB_ID(mission)};
+	uORB::Publication<landing_gear_s>			_landing_gear_pub{ORB_ID(landing_gear)};
+
+	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
+
+	uORB::Publication<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
+};

src/prometheus_px4/src/modules/commander/ManualControl.cpp

@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "ManualControl.hpp"
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+enum OverrideBits {
+	OVERRIDE_AUTO_MODE_BIT = (1 << 0),
+	OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
+	OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
+};
+
+bool ManualControl::update()
+{
+	bool updated = false;
+
+	if (_manual_control_setpoint_sub.updated()) {
+		_last_manual_control_setpoint = _manual_control_setpoint;
+		_manual_control_setpoint_sub.copy(&_manual_control_setpoint);
+
+		updated = true;
+	}
+
+	_rc_available = _rc_allowed
+			&& _manual_control_setpoint.timestamp != 0
+			&& (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s));
+
+	return updated && _rc_available;
+}
+
+bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode,
+				  const vehicle_status_s &vehicle_status)
+{
+	const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
+					&& vehicle_control_mode.flag_control_auto_enabled;
+
+	const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
+					    && vehicle_control_mode.flag_control_offboard_enabled;
+
+	const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL
+				       || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
+
+
+	if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {
+		const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
+
+		const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)
+				       || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
+				       || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
+		// Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1]
+		const bool throttle_moved =
+			(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
+		const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
+
+		if (rpy_moved || (use_throttle && throttle_moved)) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode,
+				const vehicle_status_s &vehicle_status,
+				manual_control_switches_s &manual_control_switches, const bool landed)
+{
+	bool ret = false;
+
+	const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
+	const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
+	const bool use_switch = !use_stick && !use_button;
+
+	const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	const bool stick_in_lower_left = use_stick
+					 && isThrottleLow()
+					 && _manual_control_setpoint.r < -.9f;
+	const bool arm_button_pressed = (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)
+					&& use_button;
+	const bool arm_switch_to_disarm_transition = use_switch
+			&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON)
+			&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF);
+	const bool mc_manual_thrust_mode = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+					   && vehicle_control_mode.flag_control_manual_enabled
+					   && !vehicle_control_mode.flag_control_climb_rate_enabled;
+
+	if (armed
+	    && (landed || mc_manual_thrust_mode)
+	    && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
+
+		const bool last_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
+		_stick_disarm_hysteresis.set_state_and_update(true, hrt_absolute_time());
+		const bool disarm_trigger = !last_disarm_hysteresis && _stick_disarm_hysteresis.get_state()
+					    && !_stick_arm_hysteresis.get_state();
+
+		if (disarm_trigger || arm_switch_to_disarm_transition) {
+			ret = true;
+		}
+
+	} else if (!arm_button_pressed) {
+
+		_stick_disarm_hysteresis.set_state_and_update(false, hrt_absolute_time());
+	}
+
+	return ret;
+}
+
+bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
+			     const manual_control_switches_s &manual_control_switches, const bool landed)
+{
+	bool ret = false;
+
+	const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
+	const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
+	const bool use_switch = !use_stick && !use_button;
+
+	const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	const bool stick_in_lower_right = use_stick
+					  && isThrottleLow()
+					  && _manual_control_setpoint.r > .9f;
+	const bool arm_button_pressed = use_button
+					&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
+	const bool arm_switch_to_arm_transition = use_switch
+			&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF)
+			&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
+
+	if (!armed
+	    && (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
+
+		const bool last_arm_hysteresis = _stick_arm_hysteresis.get_state();
+		_stick_arm_hysteresis.set_state_and_update(true, hrt_absolute_time());
+		const bool arm_trigger = !last_arm_hysteresis && _stick_arm_hysteresis.get_state()
+					 && !_stick_disarm_hysteresis.get_state();
+
+		if (arm_trigger || arm_switch_to_arm_transition) {
+			ret = true;
+		}
+
+	} else if (!arm_button_pressed) {
+
+		_stick_arm_hysteresis.set_state_and_update(false, hrt_absolute_time());
+	}
+
+	_last_manual_control_switches_arm_switch = manual_control_switches.arm_switch; // After disarm and arm check
+
+	return ret;
+}
+
+void ManualControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
+	_stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
+}

src/prometheus_px4/src/modules/commander/ManualControl.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 ManualControl.hpp
+ *
+ * @brief Logic for handling RC or Joystick input
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/hysteresis/hysteresis.h>
+#include <px4_platform_common/module_params.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+
+class ManualControl : ModuleParams
+{
+public:
+	ManualControl(ModuleParams *parent) : ModuleParams(parent) {};
+	~ManualControl() override = default;
+
+	void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; }
+
+	/**
+	 * Check for manual control input changes and process them
+	 * @return true if there was new data
+	 */
+	bool update();
+	bool isRCAvailable() const { return _rc_available; }
+	bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; }
+	bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status);
+	bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
+			 manual_control_switches_s &manual_control_switches, const bool landed);
+	bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
+		      const manual_control_switches_s &manual_control_switches, const bool landed);
+	bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; }
+	bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; }
+	hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; }
+
+private:
+	void updateParams() override;
+
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	manual_control_setpoint_s _manual_control_setpoint{};
+	manual_control_setpoint_s _last_manual_control_setpoint{};
+
+	// Availability
+	bool _rc_allowed{false};
+	bool _rc_available{false};
+
+	// Arming/disarming
+	systemlib::Hysteresis _stick_disarm_hysteresis{false};
+	systemlib::Hysteresis _stick_arm_hysteresis{false};
+	uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
+		(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
+		(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
+		(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
+		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
+		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov
+	)
+};

src/prometheus_px4/src/modules/commander/accelerometer_calibration.cpp

@@ -0,0 +1,605 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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 accelerometer_calibration.cpp
+ *
+ * Implementation of accelerometer calibration.
+ *
+ * Transform acceleration vector to true orientation, scale and offset
+ *
+ * ===== Model =====
+ * accel_corr = accel_T * (accel_raw - accel_offs)
+ *
+ * accel_corr[3] - fully corrected acceleration vector in body frame
+ * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
+ * accel_raw[3]  - raw acceleration vector
+ * accel_offs[3] - acceleration offset vector
+ *
+ * ===== Calibration =====
+ *
+ * Reference vectors
+ * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
+ *                        | -g  0  0 |     // nose down
+ *                        |  0  g  0 |     // left side down
+ *                        |  0 -g  0 |     // right side down
+ *                        |  0  0  g |     // on back
+ *                        [  0  0 -g ]     // level
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
+ * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
+ *
+ * Find accel_offs
+ *
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
+ *
+ * Find accel_T
+ *
+ * 9 unknown constants
+ * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
+ *
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
+ * A * x = b
+ *
+ * x = [ accel_T[0][i] ]
+ *     | accel_T[1][i] |
+ *     [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per side is enough
+ *     | accel_corr_ref[2][i] |
+ *     [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0]  a[0][1]  a[0][2] ]
+ *     | a[2][0]  a[2][1]  a[2][2] |
+ *     [ a[4][0]  a[4][1]  a[4][2] ]
+ *
+ * x = A^-1 * b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.80665
+ *
+ * ===== Rotation =====
+ *
+ * Calibrating using model:
+ * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
+ *
+ * Actual correction:
+ * accel_corr = rot * accel_T * (accel_raw - accel_offs)
+ *
+ * Known: accel_T_r, accel_offs_r, rot
+ * Unknown: accel_T, accel_offs
+ *
+ * Solution:
+ * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
+ * => accel_T = rot^-1 * accel_T_r * rot
+ * => accel_offs = rot^-1 * accel_offs_r
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "accelerometer_calibration.h"
+#include "calibration_messages.h"
+#include "calibration_routines.h"
+#include "commander_helper.h"
+#include "factory_calibration_storage.h"
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+#include <matrix/math.hpp>
+#include <lib/conversion/rotation.h>
+#include <lib/parameters/param.h>
+#include <lib/systemlib/err.h>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionBlocking.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/vehicle_attitude.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+static constexpr char sensor_name[] {"accel"};
+static constexpr unsigned MAX_ACCEL_SENS = 4;
+
+/// Data passed to calibration worker routine
+struct accel_worker_data_s {
+	orb_advert_t	*mavlink_log_pub{nullptr};
+	unsigned	done_count{0};
+	float		accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
+};
+
+// Read specified number of accelerometer samples, calculate average and dispersion.
+static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
+		unsigned orient, unsigned samples_num)
+{
+	Vector3f accel_sum[MAX_ACCEL_SENS] {};
+	unsigned counts[MAX_ACCEL_SENS] {};
+
+	unsigned errcount = 0;
+
+	// sensor thermal corrections
+	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+	sensor_correction_s sensor_correction{};
+	sensor_correction_sub.copy(&sensor_correction);
+
+	uORB::SubscriptionBlocking<sensor_accel_s> accel_sub[MAX_ACCEL_SENS] {
+		{ORB_ID(sensor_accel), 0, 0},
+		{ORB_ID(sensor_accel), 0, 1},
+		{ORB_ID(sensor_accel), 0, 2},
+		{ORB_ID(sensor_accel), 0, 3},
+	};
+
+	/* use the first sensor to pace the readout, but do per-sensor counts */
+	while (counts[0] < samples_num) {
+		if (accel_sub[0].updatedBlocking(100000)) {
+			for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
+				sensor_accel_s arp;
+
+				while (accel_sub[accel_index].update(&arp)) {
+					// fetch optional thermal offset corrections in sensor/board frame
+					Vector3f offset{0, 0, 0};
+					sensor_correction_sub.update(&sensor_correction);
+
+					if (sensor_correction.timestamp > 0 && arp.device_id != 0) {
+						for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
+							if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
+								switch (correction_index) {
+								case 0:
+									offset = Vector3f{sensor_correction.accel_offset_0};
+									break;
+								case 1:
+									offset = Vector3f{sensor_correction.accel_offset_1};
+									break;
+								case 2:
+									offset = Vector3f{sensor_correction.accel_offset_2};
+									break;
+								case 3:
+									offset = Vector3f{sensor_correction.accel_offset_3};
+									break;
+								}
+							}
+						}
+					}
+
+					accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
+					counts[accel_index]++;
+				}
+			}
+
+		} else {
+			errcount++;
+			continue;
+		}
+
+		if (errcount > samples_num / 10) {
+			return calibrate_return_error;
+		}
+	}
+
+	// rotate sensor measurements from sensor to body frame using board rotation matrix
+	const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
+
+	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
+		accel_sum[s] = board_rotation * accel_sum[s];
+	}
+
+	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
+		const Vector3f avg{accel_sum[s] / counts[s]};
+		avg.copyTo(accel_avg[s][orient]);
+	}
+
+	return calibrate_return_ok;
+}
+
+static calibrate_return accel_calibration_worker(detect_orientation_return orientation, void *data)
+{
+	static constexpr unsigned samples_num = 750;
+	accel_worker_data_s *worker_data = (accel_worker_data_s *)(data);
+
+	calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
+			     detect_orientation_str(orientation));
+
+	read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
+
+	// check accel
+	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
+		switch (orientation) {
+		case ORIENTATION_TAIL_DOWN:    // [ g, 0, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_TAIL_DOWN][0] < 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_NOSE_DOWN:    // [ -g, 0, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_NOSE_DOWN][0] > 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_LEFT:         // [ 0, g, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_LEFT][1] < 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_RIGHT:        // [ 0, -g, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHT][1] > 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_UPSIDE_DOWN:  // [ 0, 0, g ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_UPSIDE_DOWN][2] < 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_RIGHTSIDE_UP: // [ 0, 0, -g ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHTSIDE_UP][2] > 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%.3f %.3f %.3f]",
+			     detect_orientation_str(orientation),
+			     (double)worker_data->accel_ref[0][orientation][0],
+			     (double)worker_data->accel_ref[0][orientation][1],
+			     (double)worker_data->accel_ref[0][orientation][2]);
+
+	worker_data->done_count++;
+	calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
+
+	return calibrate_return_ok;
+}
+
+int do_accel_calibration(orb_advert_t *mavlink_log_pub)
+{
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
+
+	calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
+	unsigned active_sensors = 0;
+
+	for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) {
+		uORB::SubscriptionData<sensor_accel_s> accel_sub{ORB_ID(sensor_accel), cur_accel};
+
+		if (accel_sub.advertised() && (accel_sub.get().device_id != 0) && (accel_sub.get().timestamp > 0)) {
+			calibrations[cur_accel].set_device_id(accel_sub.get().device_id);
+			active_sensors++;
+
+		} else {
+			calibrations[cur_accel].Reset();
+		}
+
+		// reset calibration index to match uORB numbering
+		calibrations[cur_accel].set_calibration_index(cur_accel);
+	}
+
+	if (active_sensors == 0) {
+		calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
+		return PX4_ERROR;
+	}
+
+	FactoryCalibrationStorage factory_storage;
+
+	if (factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		return PX4_ERROR;
+	}
+
+	/* measure and calculate offsets & scales */
+	accel_worker_data_s worker_data{};
+	worker_data.mavlink_log_pub = mavlink_log_pub;
+	bool data_collected[detect_orientation_side_count] {};
+
+	if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data,
+				       false) == calibrate_return_ok) {
+
+		const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
+		const Dcmf board_rotation_t = board_rotation.transpose();
+
+		bool param_save = false;
+		bool failed = true;
+
+		for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) {
+			if (i < active_sensors) {
+				// calculate offsets
+				Vector3f offset{};
+
+				// X offset: average X from TAIL_DOWN + NOSE_DOWN
+				const Vector3f accel_tail_down{worker_data.accel_ref[i][ORIENTATION_TAIL_DOWN]};
+				const Vector3f accel_nose_down{worker_data.accel_ref[i][ORIENTATION_NOSE_DOWN]};
+				offset(0) = (accel_tail_down(0) + accel_nose_down(0)) * 0.5f;
+
+				// Y offset: average Y from LEFT + RIGHT
+				const Vector3f accel_left{worker_data.accel_ref[i][ORIENTATION_LEFT]};
+				const Vector3f accel_right{worker_data.accel_ref[i][ORIENTATION_RIGHT]};
+				offset(1) = (accel_left(1) + accel_right(1)) * 0.5f;
+
+				// Z offset: average Z from UPSIDE_DOWN + RIGHTSIDE_UP
+				const Vector3f accel_upside_down{worker_data.accel_ref[i][ORIENTATION_UPSIDE_DOWN]};
+				const Vector3f accel_rightside_up{worker_data.accel_ref[i][ORIENTATION_RIGHTSIDE_UP]};
+				offset(2) = (accel_upside_down(2) + accel_rightside_up(2)) * 0.5f;
+
+				// transform matrix
+				Matrix3f mat_A;
+				mat_A.row(0) = accel_tail_down - offset;
+				mat_A.row(1) = accel_left - offset;
+				mat_A.row(2) = accel_upside_down - offset;
+
+				// calculate inverse matrix for A: simplify matrices mult because b has only one non-zero element == g at index i
+				const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G;
+
+				// update calibration
+				const Vector3f accel_offs_rotated{board_rotation_t *offset};
+				calibrations[i].set_offset(accel_offs_rotated);
+
+				const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
+				calibrations[i].set_scale(accel_T_rotated.diag());
+
+#if defined(DEBUD_BUILD)
+				PX4_INFO("accel %d: offset", i);
+				offset.print();
+				PX4_INFO("accel %d: bT * offset", i);
+				accel_offs_rotated.print();
+
+				PX4_INFO("accel %d: mat_A", i);
+				mat_A.print();
+				PX4_INFO("accel %d: accel_T", i);
+				accel_T.print();
+				PX4_INFO("accel %d: bT * accel_T * b", i);
+				accel_T_rotated.print();
+#endif // DEBUD_BUILD
+				calibrations[i].PrintStatus();
+
+
+				if (calibrations[i].ParametersSave()) {
+					param_save = true;
+					failed = false;
+
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, "calibration save failed");
+					break;
+				}
+			}
+		}
+
+		if (!failed && factory_storage.store() != PX4_OK) {
+			failed = true;
+		}
+
+		if (param_save) {
+			param_notify_changes();
+		}
+
+		if (!failed) {
+			calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
+			px4_usleep(600000); // give this message enough time to propagate
+			return PX4_OK;
+		}
+	}
+
+	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
+	px4_usleep(600000); // give this message enough time to propagate
+	return PX4_ERROR;
+}
+
+int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
+{
+#if !defined(CONSTRAINED_FLASH)
+	PX4_INFO("Accelerometer quick calibration");
+
+	bool param_save = false;
+	bool failed = true;
+
+	FactoryCalibrationStorage factory_storage;
+
+	if (factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		return PX4_ERROR;
+	}
+
+	// sensor thermal corrections (optional)
+	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+	sensor_correction_s sensor_correction{};
+	sensor_correction_sub.copy(&sensor_correction);
+
+	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
+
+	/* use the first sensor to pace the readout, but do per-sensor counts */
+	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
+		sensor_accel_s arp{};
+		Vector3f accel_sum{};
+		unsigned count = 0;
+
+		while (accel_subs[accel_index].update(&arp)) {
+			// fetch optional thermal offset corrections in sensor/board frame
+			if ((arp.timestamp > 0) && (arp.device_id != 0)) {
+				Vector3f offset{0, 0, 0};
+
+				if (sensor_correction.timestamp > 0) {
+					for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
+						if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
+							switch (correction_index) {
+							case 0:
+								offset = Vector3f{sensor_correction.accel_offset_0};
+								break;
+							case 1:
+								offset = Vector3f{sensor_correction.accel_offset_1};
+								break;
+							case 2:
+								offset = Vector3f{sensor_correction.accel_offset_2};
+								break;
+							case 3:
+								offset = Vector3f{sensor_correction.accel_offset_3};
+								break;
+							}
+						}
+					}
+				}
+
+				const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - offset};
+
+				if (count > 0) {
+					const Vector3f diff{accel - (accel_sum / count)};
+
+					if (diff.norm() < 1.f) {
+						accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
+						count++;
+					}
+
+				} else {
+					accel_sum = accel;
+					count = 1;
+				}
+			}
+		}
+
+		if ((count > 0) && (arp.device_id != 0)) {
+
+			bool calibrated = false;
+			const Vector3f accel_avg = accel_sum / count;
+
+			Vector3f offset{0.f, 0.f, 0.f};
+
+			uORB::SubscriptionData<vehicle_attitude_s> attitude_sub{ORB_ID(vehicle_attitude)};
+			attitude_sub.update();
+
+			if (attitude_sub.advertised() && attitude_sub.get().timestamp != 0) {
+				// use vehicle_attitude if available
+				const vehicle_attitude_s &att = attitude_sub.get();
+				const matrix::Quatf q{att.q};
+				const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
+
+				// sanity check angle between acceleration vectors
+				const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();
+
+				if (angle <= math::radians(10.f)) {
+					offset = accel_avg - accel_ref;
+					calibrated = true;
+				}
+			}
+
+			if (!calibrated) {
+				// otherwise simply normalize to gravity and remove offset
+				Vector3f accel{accel_avg};
+				accel.normalize();
+				accel = accel * CONSTANTS_ONE_G;
+
+				offset = accel_avg - accel;
+				calibrated = true;
+			}
+
+			calibration::Accelerometer calibration{arp.device_id};
+
+			// reset cal index to uORB
+			calibration.set_calibration_index(accel_index);
+
+			if (!calibrated || (offset.norm() > CONSTANTS_ONE_G)
+			    || !PX4_ISFINITE(offset(0))
+			    || !PX4_ISFINITE(offset(1))
+			    || !PX4_ISFINITE(offset(2))) {
+
+				PX4_ERR("accel %d quick calibrate failed", accel_index);
+
+			} else {
+				calibration.set_offset(offset);
+
+				if (calibration.ParametersSave()) {
+					calibration.PrintStatus();
+					param_save = true;
+					failed = false;
+
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "calibration save failed");
+					break;
+				}
+			}
+		}
+	}
+
+	if (!failed && factory_storage.store() != PX4_OK) {
+		failed = true;
+	}
+
+	if (param_save) {
+		param_notify_changes();
+	}
+
+	if (!failed) {
+		return PX4_OK;
+	}
+
+#endif // !CONSTRAINED_FLASH
+
+	return PX4_ERROR;
+}

src/prometheus_px4/src/modules/commander/accelerometer_calibration.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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 accelerometer_calibration.h
+ *
+ * Definition of accelerometer calibration.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef ACCELEROMETER_CALIBRATION_H_
+#define ACCELEROMETER_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_accel_calibration(orb_advert_t *mavlink_log_pub);
+int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub);
+
+#endif /* ACCELEROMETER_CALIBRATION_H_ */

src/prometheus_px4/src/modules/commander/airspeed_calibration.cpp

@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2016 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 airspeed_calibration.cpp
+ * Airspeed sensor calibration routine
+ */
+
+#include "airspeed_calibration.h"
+#include "calibration_messages.h"
+#include "calibration_routines.h"
+#include "commander_helper.h"
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_airspeed.h>
+#include <uORB/topics/differential_pressure.h>
+#include <systemlib/mavlink_log.h>
+#include <parameters/param.h>
+#include <systemlib/err.h>
+
+static const char *sensor_name = "airspeed";
+
+static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
+{
+	px4_sleep(5);
+	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
+}
+
+int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
+{
+	const hrt_abstime calibration_started = hrt_absolute_time();
+
+	int result = PX4_OK;
+	unsigned calibration_counter = 0;
+	const unsigned maxcount = 2400;
+
+	/* give directions */
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
+
+	const unsigned calibration_count = (maxcount * 2) / 3;
+
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+
+	float diff_pres_offset = 0.0f;
+
+	/* Reset sensor parameters */
+	struct airspeed_scale airscale = {
+		diff_pres_offset,
+		1.0f,
+	};
+
+	bool paramreset_successful = false;
+	int  fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
+
+	if (fd >= 0) {
+		if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+			paramreset_successful = true;
+
+		} else {
+			calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
+		}
+
+		px4_close(fd);
+	}
+
+	if (!paramreset_successful) {
+
+		/* only warn if analog scaling is zero */
+		float analog_scaling = 0.0f;
+		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
+
+		if (fabsf(analog_scaling) < 0.1f) {
+			calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
+			goto error_return;
+		}
+
+		/* set scaling offset parameter */
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
+			goto error_return;
+		}
+	}
+
+	calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
+	px4_usleep(500 * 1000);
+
+	while (calibration_counter < calibration_count) {
+
+		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
+			goto error_return;
+		}
+
+		/* wait blocking for new data */
+		px4_pollfd_struct_t fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = px4_poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
+			calibration_counter++;
+
+			/* any differential pressure failure a reason to abort */
+			if (diff_pres.error_count != 0) {
+				calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
+							 diff_pres.error_count);
+				calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again");
+				feedback_calibration_failed(mavlink_log_pub);
+				goto error_return;
+			}
+
+			if (calibration_counter % (calibration_count / 20) == 0) {
+				calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
+			}
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			feedback_calibration_failed(mavlink_log_pub);
+			goto error_return;
+		}
+	}
+
+	diff_pres_offset = diff_pres_offset / calibration_count;
+
+	if (PX4_ISFINITE(diff_pres_offset)) {
+
+		int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
+		airscale.offset_pa = diff_pres_offset;
+
+		if (fd_scale >= 0) {
+			if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+				calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
+			}
+
+			px4_close(fd_scale);
+		}
+
+		// Prevent a completely zero param
+		// since this is used to detect a missing calibration
+		// This value is numerically down in the noise and has
+		// no effect on the sensor performance.
+		if (fabsf(diff_pres_offset) < 0.00000001f) {
+			diff_pres_offset = 0.00000001f;
+		}
+
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
+			goto error_return;
+		}
+
+	} else {
+		feedback_calibration_failed(mavlink_log_pub);
+		goto error_return;
+	}
+
+	calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
+
+	/* wait 500 ms to ensure parameter propagated through the system */
+	px4_usleep(500 * 1000);
+
+	calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
+
+	calibration_counter = 0;
+
+	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
+	while (calibration_counter < maxcount) {
+
+		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
+			goto error_return;
+		}
+
+		/* wait blocking for new data */
+		px4_pollfd_struct_t fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = px4_poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+			if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
+				if (diff_pres.differential_pressure_filtered_pa > 0) {
+					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
+							     (int)diff_pres.differential_pressure_filtered_pa);
+					break;
+
+				} else {
+					/* do not allow negative values */
+					calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
+								 (int)diff_pres.differential_pressure_filtered_pa);
+					calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
+
+					/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
+					diff_pres_offset = 0.0f;
+
+					if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+						calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
+						goto error_return;
+					}
+
+					/* save */
+					calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
+					param_save_default();
+
+					feedback_calibration_failed(mavlink_log_pub);
+					goto error_return;
+				}
+			}
+
+			if (calibration_counter % 500 == 0) {
+				calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
+						     (int)diff_pres.differential_pressure_filtered_pa);
+				tune_neutral(true);
+			}
+
+			calibration_counter++;
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			feedback_calibration_failed(mavlink_log_pub);
+			goto error_return;
+		}
+	}
+
+	if (calibration_counter == maxcount) {
+		feedback_calibration_failed(mavlink_log_pub);
+		goto error_return;
+	}
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
+	tune_neutral(true);
+
+	/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
+	 * the followup preflight checks might fail. */
+	px4_usleep(2e6);
+
+normal_return:
+	px4_close(diff_pres_sub);
+
+	// This give a chance for the log messages to go out of the queue before someone else stomps on then
+	px4_sleep(1);
+
+	return result;
+
+error_return:
+	result = PX4_ERROR;
+	goto normal_return;
+}

src/prometheus_px4/src/modules/commander/airspeed_calibration.h

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 gyro_calibration.h
+ * Airspeed sensor calibration routine
+ */
+
+#ifndef AIRSPEED_CALIBRATION_H_
+#define AIRSPEED_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_airspeed_calibration(orb_advert_t *mavlink_log_pub);
+
+#endif /* AIRSPEED_CALIBRATION_H_ */

src/prometheus_px4/src/modules/commander/calibration_messages.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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 calibration_messages.h
+ *
+ * Common calibration messages.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef CALIBRATION_MESSAGES_H_
+#define CALIBRATION_MESSAGES_H_
+
+// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state
+// machine to provide visual feedback for calibration. As such, the text for them or semantics of when
+// they are displayed cannot be modified without causing QGC to break. If modifications are made, make
+// sure to bump the calibration version number which will cause QGC to perform log based calibration
+// instead of visual calibration until such a time as QGC is update to the new version.
+
+// The number in the cal started message is used to indicate the version stamp for the current calibration code.
+#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 2 %s"
+#define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
+#define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
+// Warnings are deprecated because they were only used when it failed anyway.
+//#define CAL_QGC_WARNING_MSG			"[cal] calibration warning: %s"
+#define CAL_QGC_CANCELLED_MSG			"[cal] calibration cancelled"
+#define CAL_QGC_PROGRESS_MSG			"[cal] progress <%u>"
+#define CAL_QGC_ORIENTATION_DETECTED_MSG	"[cal] %s orientation detected"
+#define CAL_QGC_SIDE_DONE_MSG			"[cal] %s side done, rotate to a different side"
+
+#define CAL_ERROR_SENSOR_MSG		"[cal] calibration failed: reading sensor"
+#define CAL_ERROR_RESET_CAL_MSG		"[cal] calibration failed: to reset, sensor %u"
+#define CAL_ERROR_READ_CAL_MSG		"[cal] calibration failed: to read calibration"
+#define CAL_ERROR_APPLY_CAL_MSG		"[cal] calibration failed: to apply calibration"
+#define CAL_ERROR_SET_PARAMS_MSG	"[cal] calibration failed: to set parameters"
+
+#endif /* CALIBRATION_MESSAGES_H_ */

src/prometheus_px4/src/modules/commander/calibration_routines.cpp

@@ -0,0 +1,367 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2021  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 calibration_routines.cpp
+ * Calibration routines implementations.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <lib/ecl/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/SubscriptionBlocking.hpp>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+#include "calibration_routines.h"
+#include "calibration_messages.h"
+#include "commander_helper.h"
+
+using namespace time_literals;
+
+enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position)
+{
+	static constexpr unsigned ndim = 3;
+
+	float accel_ema[ndim] {};                       // exponential moving average of accel
+	float accel_disp[3] {};                         // max-hold dispersion of accel
+	static constexpr float ema_len = 0.5f;          // EMA time constant in seconds
+	static constexpr float normal_still_thr = 0.25; // normal still threshold
+	float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
+	static constexpr float accel_err_thr = 5.0f;    // set accel error threshold to 5m/s^2
+	const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
+
+	/* set timeout to 90s */
+	static constexpr hrt_abstime timeout = 90_s;
+
+	const hrt_abstime t_start = hrt_absolute_time();
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+
+	unsigned poll_errcount = 0;
+
+	// Setup subscriptions to onboard accel sensor
+	uORB::SubscriptionBlocking<vehicle_acceleration_s> vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+
+	while (true) {
+		vehicle_acceleration_s accel;
+
+		if (vehicle_acceleration_sub.updateBlocking(accel, 100000)) {
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+
+			for (unsigned i = 0; i < ndim; i++) {
+
+				float di = accel.xyz[i];
+
+				float d = di - accel_ema[i];
+				accel_ema[i] += d * w;
+				d = d * d;
+				accel_disp[i] = accel_disp[i] * (1.0f - w);
+
+				if (d > still_thr2 * 8.0f) {
+					d = still_thr2 * 8.0f;
+				}
+
+				if (d > accel_disp[i]) {
+					accel_disp[i] = d;
+				}
+			}
+
+			/* still detector with hysteresis */
+			if (accel_disp[0] < still_thr2 &&
+			    accel_disp[1] < still_thr2 &&
+			    accel_disp[2] < still_thr2) {
+
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					calibration_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
+					t_still = t;
+					t_timeout = t + timeout;
+
+				} else {
+					/* still since t_still */
+					if (t > t_still + still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+
+			} else if (accel_disp[0] > still_thr2 * 4.0f ||
+				   accel_disp[1] > still_thr2 * 4.0f ||
+				   accel_disp[2] > still_thr2 * 4.0f) {
+				/* not still, reset still start time */
+				if (t_still != 0) {
+					calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
+					px4_usleep(200000);
+					t_still = 0;
+				}
+			}
+
+		} else {
+			poll_errcount++;
+		}
+
+		if (t > t_timeout) {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
+			return ORIENTATION_ERROR;
+		}
+	}
+
+	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return ORIENTATION_TAIL_DOWN;        // [ g, 0, 0 ]
+	}
+
+	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return ORIENTATION_NOSE_DOWN;        // [ -g, 0, 0 ]
+	}
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return ORIENTATION_LEFT;        // [ 0, g, 0 ]
+	}
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return ORIENTATION_RIGHT;        // [ 0, -g, 0 ]
+	}
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
+		return ORIENTATION_UPSIDE_DOWN;        // [ 0, 0, g ]
+	}
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
+		return ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
+	}
+
+	calibration_log_critical(mavlink_log_pub, "ERROR: invalid orientation");
+
+	return ORIENTATION_ERROR;	// Can't detect orientation
+}
+
+const char *detect_orientation_str(enum detect_orientation_return orientation)
+{
+	static const char *rgOrientationStrs[] = {
+		"back",		// tail down
+		"front",	// nose down
+		"left",
+		"right",
+		"up",		// upside-down
+		"down",		// right-side up
+		"error"
+	};
+
+	return rgOrientationStrs[orientation];
+}
+
+calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
+		bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker,
+		void *worker_data, bool lenient_still_position)
+{
+	const hrt_abstime calibration_started = hrt_absolute_time();
+	calibrate_return result = calibrate_return_ok;
+
+	unsigned orientation_failures = 0;
+
+	// Rotate through all requested orientation
+	while (true) {
+		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
+			result = calibrate_return_cancelled;
+			break;
+		}
+
+		if (orientation_failures > 4) {
+			result = calibrate_return_error;
+			calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
+			break;
+		}
+
+		unsigned int side_complete_count = 0;
+
+		// Update the number of completed sides
+		for (unsigned i = 0; i < detect_orientation_side_count; i++) {
+			if (side_data_collected[i]) {
+				side_complete_count++;
+			}
+		}
+
+		if (side_complete_count == detect_orientation_side_count) {
+			// We have completed all sides, move on
+			break;
+		}
+
+		/* inform user which orientations are still needed */
+		char pendingStr[80];
+		pendingStr[0] = 0;
+
+		for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
+			if (!side_data_collected[cur_orientation]) {
+				strncat(pendingStr, " ", sizeof(pendingStr) - 1);
+				strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
+			}
+		}
+
+		calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
+		px4_usleep(20000);
+		calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
+		px4_usleep(20000);
+		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
+
+		if (orient == ORIENTATION_ERROR) {
+			orientation_failures++;
+			calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
+			px4_usleep(20000);
+			continue;
+		}
+
+		/* inform user about already handled side */
+		if (side_data_collected[orient]) {
+			orientation_failures++;
+			set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE);
+			calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
+			px4_usleep(20000);
+			continue;
+		}
+
+		calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
+		px4_usleep(20000);
+		calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
+		px4_usleep(20000);
+		orientation_failures = 0;
+
+		// Call worker routine
+		result = calibration_worker(orient, worker_data);
+
+		if (result != calibrate_return_ok) {
+			break;
+		}
+
+		calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
+		px4_usleep(20000);
+		calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
+		px4_usleep(20000);
+
+		// Note that this side is complete
+		side_data_collected[orient] = true;
+
+		// output neutral tune
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL);
+
+		// temporary priority boost for the white blinking led to come trough
+		rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1);
+		px4_usleep(200000);
+	}
+
+	return result;
+}
+
+bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started)
+{
+	bool ret = false;
+
+	uORB::Subscription vehicle_command_sub{ORB_ID(vehicle_command)};
+	vehicle_command_s cmd;
+
+	while (vehicle_command_sub.update(&cmd)) {
+		if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
+			// only handle commands sent after calibration started from external sources
+			if ((cmd.timestamp > calibration_started) && cmd.from_external) {
+
+				vehicle_command_ack_s command_ack{};
+
+				if ((int)cmd.param1 == 0 &&
+				    (int)cmd.param2 == 0 &&
+				    (int)cmd.param3 == 0 &&
+				    (int)cmd.param4 == 0 &&
+				    (int)cmd.param5 == 0 &&
+				    (int)cmd.param6 == 0) {
+
+					command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
+					tune_positive(true);
+					ret = true;
+
+				} else {
+					command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+					mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %" PRIu32, cmd.command);
+					tune_negative(true);
+					ret = false;
+				}
+
+				command_ack.command = cmd.command;
+				command_ack.target_system = cmd.source_system;
+				command_ack.target_component = cmd.source_component;
+				command_ack.timestamp = hrt_absolute_time();
+
+				uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+				command_ack_pub.publish(command_ack);
+
+				return ret;
+			}
+		}
+	}
+
+	return ret;
+}

src/prometheus_px4/src/modules/commander/calibration_routines.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 calibration_routines.h
+///	@author Don Gagne <don@thegagnes.com>
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <uORB/Publication.hpp>
+
+// The order of these cannot change since the calibration calculations depend on them in this order
+enum detect_orientation_return {
+	ORIENTATION_TAIL_DOWN,
+	ORIENTATION_NOSE_DOWN,
+	ORIENTATION_LEFT,
+	ORIENTATION_RIGHT,
+	ORIENTATION_UPSIDE_DOWN,
+	ORIENTATION_RIGHTSIDE_UP,
+	ORIENTATION_ERROR
+};
+static constexpr unsigned detect_orientation_side_count = 6;
+
+/// Wait for vehicle to become still and detect it's orientation
+///	@return Returns detect_orientation_return according to orientation when vehicle
+///		and ready for measurements
+enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,	///< uORB handle to write output to
+		bool lenient_still_detection);	///< true: Use more lenient still position detection
+
+/// Returns the human readable string representation of the orientation
+///	@param orientation Orientation to return string for, "error" if buffer is too small
+const char *detect_orientation_str(enum detect_orientation_return orientation);
+
+enum calibrate_return {
+	calibrate_return_ok,
+	calibrate_return_error,
+	calibrate_return_cancelled
+};
+
+typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return
+		orientation,	///< Orientation which was detected
+		void *worker_data);	///< Opaque worker data
+
+/// Perform calibration sequence which require a rest orientation detection prior to calibration.
+///	@return OK: Calibration succeeded, ERROR: Calibration failed
+calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,	///< uORB handle to write output to
+		bool side_data_collected[detect_orientation_side_count],	///< Sides for which data still needs calibration
+		calibration_from_orientation_worker_t calibration_worker,	///< Worker routine which performs the actual calibration
+		void *worker_data,						///< Opaque data passed to worker routine
+		bool lenient_still_detection);					///< true: Use more lenient still position detection
+
+/// Used to periodically check for a cancel command
+bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started);
+
+
+// TODO FIXME: below are workarounds for QGC. The issue is that sometimes
+// a mavlink log message is overwritten by the following one. A workaround
+// is to wait for some time after publishing each message and hope that it
+// gets sent out in the meantime.
+
+#define calibration_log_info(_pub, _text, ...)			\
+	do { \
+		mavlink_log_info(_pub, _text, ##__VA_ARGS__); \
+		px4_usleep(10000); \
+	} while(0);
+
+#define calibration_log_critical(_pub, _text, ...)			\
+	do { \
+		mavlink_log_critical(_pub, _text, ##__VA_ARGS__); \
+		px4_usleep(10000); \
+	} while(0);
+
+#define calibration_log_emergency(_pub, _text, ...)			\
+	do { \
+		mavlink_log_emergency(_pub, _text, ##__VA_ARGS__); \
+		px4_usleep(10000); \
+	} while(0);

src/prometheus_px4/src/modules/commander/commander_helper.cpp

@@ -0,0 +1,371 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 commander_helper.cpp
+ * Commander helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ */
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <math.h>
+#include <string.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/led_control.h>
+#include <uORB/topics/tune_control.h>
+#include <systemlib/err.h>
+#include <parameters/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include "commander_helper.h"
+
+#define VEHICLE_TYPE_FIXED_WING 1
+#define VEHICLE_TYPE_QUADROTOR 2
+#define VEHICLE_TYPE_COAXIAL 3
+#define VEHICLE_TYPE_HELICOPTER 4
+#define VEHICLE_TYPE_GROUND_ROVER 10
+#define VEHICLE_TYPE_HEXAROTOR 13
+#define VEHICLE_TYPE_OCTOROTOR 14
+#define VEHICLE_TYPE_TRICOPTER 15
+#define VEHICLE_TYPE_VTOL_DUOROTOR 19
+#define VEHICLE_TYPE_VTOL_QUADROTOR 20
+#define VEHICLE_TYPE_VTOL_TILTROTOR 21
+#define VEHICLE_TYPE_VTOL_RESERVED2 22
+#define VEHICLE_TYPE_VTOL_RESERVED3 23
+#define VEHICLE_TYPE_VTOL_RESERVED4 24
+#define VEHICLE_TYPE_VTOL_RESERVED5 25
+
+#define BLINK_MSG_TIME	700000	// 3 fast blinks (in us)
+
+bool is_multirotor(const vehicle_status_s &current_status)
+{
+	return ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status.system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const vehicle_status_s &current_status)
+{
+	return is_multirotor(current_status) || (current_status.system_type == VEHICLE_TYPE_HELICOPTER)
+	       || (current_status.system_type == VEHICLE_TYPE_COAXIAL);
+}
+
+bool is_vtol(const vehicle_status_s &current_status)
+{
+	return (current_status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED2 ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED3 ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED4 ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED5);
+}
+
+bool is_vtol_tailsitter(const vehicle_status_s &current_status)
+{
+	return (current_status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
+}
+
+bool is_fixed_wing(const vehicle_status_s &current_status)
+{
+	return current_status.system_type == VEHICLE_TYPE_FIXED_WING;
+}
+
+bool is_ground_rover(const vehicle_status_s &current_status)
+{
+	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
+}
+
+static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message
+static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence
+static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; // currently playing tune, can be interrupted after tune_end
+static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {};
+
+static int fd_leds{-1};
+
+static led_control_s led_control {};
+static orb_advert_t led_control_pub = nullptr;
+static tune_control_s tune_control {};
+static orb_advert_t tune_control_pub = nullptr;
+
+int buzzer_init()
+{
+	tune_durations[tune_control_s::TUNE_ID_NOTIFY_POSITIVE] = 800000;
+	tune_durations[tune_control_s::TUNE_ID_NOTIFY_NEGATIVE] = 900000;
+	tune_durations[tune_control_s::TUNE_ID_NOTIFY_NEUTRAL] = 500000;
+	tune_durations[tune_control_s::TUNE_ID_ARMING_WARNING] = 3000000;
+	tune_durations[tune_control_s::TUNE_ID_HOME_SET] = 800000;
+	tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_FAST] = 800000;
+	tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000;
+	tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000;
+
+	tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH);
+
+	return PX4_OK;
+}
+
+void buzzer_deinit()
+{
+	orb_unadvertise(tune_control_pub);
+}
+
+void set_tune_override(int tune)
+{
+	tune_control.tune_id = tune;
+	tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
+	tune_control.tune_override = true;
+	tune_control.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
+}
+
+void set_tune(int tune)
+{
+	unsigned int new_tune_duration = tune_durations[tune];
+
+	/* don't interrupt currently playing non-repeating tune by repeating */
+	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
+		/* allow interrupting current non-repeating tune by the same tune */
+		if (tune != tune_current || new_tune_duration != 0) {
+			tune_control.tune_id = tune;
+			tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
+			tune_control.tune_override = false;
+			tune_control.timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
+		}
+
+		tune_current = tune;
+
+		if (new_tune_duration != 0) {
+			tune_end = hrt_absolute_time() + new_tune_duration;
+
+		} else {
+			tune_end = 0;
+		}
+	}
+}
+
+void tune_home_set(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_HOME_SET);
+	}
+}
+
+void tune_mission_ok(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
+	}
+}
+
+void tune_mission_warn(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_YELLOW, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL);
+	}
+}
+
+void tune_mission_fail(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE);
+	}
+}
+
+/**
+ * Blink green LED and play positive tune (if use_buzzer == true).
+ */
+void tune_positive(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
+	}
+}
+
+/**
+ * Blink white LED and play neutral tune (if use_buzzer == true).
+ */
+void tune_neutral(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL);
+	}
+}
+
+/**
+ * Blink red LED and play negative tune (if use_buzzer == true).
+ */
+void tune_negative(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE);
+	}
+}
+
+void tune_failsafe(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color_and_mode(led_control_s::COLOR_PURPLE, led_control_s::MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
+	}
+}
+
+int blink_msg_state()
+{
+	if (blink_msg_end == 0) {
+		return 0;
+
+	} else if (hrt_absolute_time() > blink_msg_end) {
+		blink_msg_end = 0;
+		return 2;
+
+	} else {
+		return 1;
+	}
+}
+
+int led_init()
+{
+	blink_msg_end = 0;
+
+	led_control.led_mask = 0xff;
+	led_control.mode = led_control_s::MODE_OFF;
+	led_control.priority = 0;
+	led_control.timestamp = hrt_absolute_time();
+	led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
+
+	/* first open normal LEDs */
+	fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR);
+
+	if (fd_leds < 0) {
+		// there might not be an LED available, so don't make this an error
+		PX4_INFO("LED: open %s failed (%i)", LED0_DEVICE_PATH, errno);
+		return -errno;
+	}
+
+	/* the green LED is only available on FMUv5 */
+	px4_ioctl(fd_leds, LED_ON, LED_GREEN);
+
+	/* the blue LED is only available on AeroCore but not FMUv2 */
+	px4_ioctl(fd_leds, LED_ON, LED_BLUE);
+
+	/* switch blue off */
+	led_off(LED_BLUE);
+
+	/* we consider the amber led mandatory */
+	if (px4_ioctl(fd_leds, LED_ON, LED_AMBER)) {
+		PX4_WARN("Amber LED: ioctl fail");
+		return PX4_ERROR;
+	}
+
+	/* switch amber off */
+	led_off(LED_AMBER);
+
+	return 0;
+}
+
+void led_deinit()
+{
+	orb_unadvertise(led_control_pub);
+	px4_close(fd_leds);
+}
+
+int led_toggle(int led)
+{
+	return px4_ioctl(fd_leds, LED_TOGGLE, led);
+}
+
+int led_on(int led)
+{
+	return px4_ioctl(fd_leds, LED_ON, led);
+}
+
+int led_off(int led)
+{
+	return px4_ioctl(fd_leds, LED_OFF, led);
+}
+
+void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)
+{
+	led_control.mode = mode;
+	led_control.color = color;
+	led_control.num_blinks = blinks;
+	led_control.priority = prio;
+	led_control.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(led_control), led_control_pub, &led_control);
+}
+
+void rgbled_set_color_and_mode(uint8_t color, uint8_t mode)
+{
+	rgbled_set_color_and_mode(color, mode, 0, 0);
+}

src/prometheus_px4/src/modules/commander/commander_helper.h

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 commander_helper.h
+ * Commander helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef COMMANDER_HELPER_H_
+#define COMMANDER_HELPER_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_board_led.h>
+
+
+bool is_multirotor(const vehicle_status_s &current_status);
+bool is_rotary_wing(const vehicle_status_s &current_status);
+bool is_vtol(const vehicle_status_s &current_status);
+bool is_vtol_tailsitter(const vehicle_status_s &current_status);
+bool is_fixed_wing(const vehicle_status_s &current_status);
+bool is_ground_rover(const vehicle_status_s &current_status);
+
+int buzzer_init(void);
+void buzzer_deinit(void);
+
+void set_tune_override(int tune);
+void set_tune(int tune);
+void tune_home_set(bool use_buzzer);
+void tune_mission_ok(bool use_buzzer);
+void tune_mission_warn(bool use_buzzer);
+void tune_mission_fail(bool use_buzzer);
+void tune_positive(bool use_buzzer);
+void tune_neutral(bool use_buzzer);
+void tune_negative(bool use_buzzer);
+void tune_failsafe(bool use_buzzer);
+
+int blink_msg_state();
+
+/* methods to control the onboard LED(s) */
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+
+/**
+ * set the LED color & mode
+ * @param color @see led_control_s::COLOR_*
+ * @param mode @see led_control_s::MODE_*
+ */
+void rgbled_set_color_and_mode(uint8_t color, uint8_t mode);
+
+void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio);
+
+#endif /* COMMANDER_HELPER_H_ */

src/prometheus_px4/src/modules/commander/commander_tests/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2015 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_module(
+	MODULE modules__commander__commander_tests
+	MAIN commander_tests
+	SRCS
+		commander_tests.cpp
+		state_machine_helper_test.cpp
+		../state_machine_helper.cpp
+	DEPENDS
+	)

src/prometheus_px4/src/modules/commander/commander_tests/commander_tests.cpp

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 commander_tests.cpp
+ * Commander unit tests. Run the tests as follows:
+ *   nsh> commander_tests
+ *
+ */
+
+#include <systemlib/err.h>
+
+#include "state_machine_helper_test.h"
+
+extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
+
+
+int commander_tests_main(int argc, char *argv[])
+{
+	return stateMachineHelperTest() ? 0 : -1;
+}

src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -0,0 +1,546 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 state_machine_helper_test.cpp
+ * System state machine unit test.
+ *
+ */
+
+#include "state_machine_helper_test.h"
+
+#include "../state_machine_helper.h"
+#include <unit_test.h>
+#include "../Arming/PreFlightCheck/PreFlightCheck.hpp"
+
+class StateMachineHelperTest : public UnitTest
+{
+public:
+	StateMachineHelperTest() = default;
+	~StateMachineHelperTest() override = default;
+
+	bool run_tests() override;
+
+private:
+	bool armingStateTransitionTest();
+	bool mainStateTransitionTest();
+};
+
+bool StateMachineHelperTest::armingStateTransitionTest()
+{
+	// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
+	// to simulate machine state prior to testing an arming state transition. This structure is also
+	// use to represent the expected machine state after the transition has been requested.
+	typedef struct {
+		arming_state_t  arming_state;   // vehicle_status_s.arming_state
+		bool            armed;          // actuator_armed_s.armed
+		bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
+	} ArmingTransitionVolatileState_t;
+
+	// This structure represents a test case for arming_state_transition. It contains the machine
+	// state prior to transition, the requested state to transition to and finally the expected
+	// machine state after transition.
+	typedef struct {
+		const char                     *assertMsg;                              // Text to show when test case fails
+		ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transition
+		hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
+		bool
+		condition_system_sensors_initialized;   // Current vehicle_status_s.condition_system_sensors_initialized
+		bool                            safety_switch_available;                // Current safety_s.safety_switch_available
+		bool                            safety_off;                             // Current safety_s.safety_off
+		arming_state_t                  requested_state;                        // Requested arming state to transition to
+		ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
+		transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
+	} ArmingTransitionTest_t;
+
+	// We use these defines so that our test cases are more readable
+#define ATT_ARMED true
+#define ATT_DISARMED false
+#define ATT_READY_TO_ARM true
+#define ATT_NOT_READY_TO_ARM false
+#define ATT_SENSORS_INITIALIZED true
+#define ATT_SENSORS_NOT_INITIALIZED false
+#define ATT_SAFETY_AVAILABLE true
+#define ATT_SAFETY_NOT_AVAILABLE true
+#define ATT_SAFETY_OFF true
+#define ATT_SAFETY_ON false
+
+	// These are test cases for arming_state_transition
+	static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
+		// TRANSITION_NOT_CHANGED tests
+
+		{
+			"no transition: identical states",
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_INIT,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED
+		},
+
+		// TRANSITION_CHANGED tests
+
+		// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
+
+		{
+			"transition: init to standby",
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: init to standby error",
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: init to reboot",
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_SHUTDOWN,
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: standby to init",
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_INIT,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: standby to standby error",
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: standby to reboot",
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_SHUTDOWN,
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: armed to standby",
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: standby error to reboot",
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_SHUTDOWN,
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: in air restore to armed",
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: in air restore to reboot",
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_SHUTDOWN,
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		// hil on tests, standby error to standby not normally allowed
+
+		{
+			"transition: standby error to standby, hil on",
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		// Safety switch arming tests
+
+		{
+			"transition: standby to armed, no safety switch",
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: standby to armed, safety switch off",
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+		},
+
+		// TRANSITION_DENIED tests
+
+		// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
+
+		{
+			"no transition: init to armed",
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: armed to init",
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_INIT,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: armed to reboot",
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_SHUTDOWN,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: standby error to armed",
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: standby error to standby",
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: reboot to armed",
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: in air restore to standby",
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_STANDBY,
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+
+		// Sensor tests
+
+		//{ "transition to standby error: init to standby - sensors not initialized",
+		//    { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+		//    vehicle_status_s::ARMING_STATE_STANDBY,
+		//    { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
+		// Safety switch arming tests
+
+		{
+			"no transition: init to armed, safety switch on",
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			vehicle_status_s::ARMING_STATE_ARMED,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
+		},
+	};
+
+	struct vehicle_status_s status = {};
+	struct vehicle_status_flags_s status_flags = {};
+	struct safety_s         safety = {};
+	struct actuator_armed_s armed = {};
+
+	size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
+
+	for (size_t i = 0; i < cArmingTransitionTests; i++) {
+		const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
+
+		PreFlightCheck::arm_requirements_t arm_req{};
+
+		// Setup initial machine state
+		status.arming_state = test->current_state.arming_state;
+		status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
+		status.hil_state = test->hil_state;
+		// The power status of the test unit is not relevant for the unit test
+		status_flags.circuit_breaker_engaged_power_check = true;
+		safety.safety_switch_available = test->safety_switch_available;
+		safety.safety_off = test->safety_off;
+		armed.armed = test->current_state.armed;
+		armed.ready_to_arm = test->current_state.ready_to_arm;
+
+		// Attempt transition
+		transition_result_t result = arming_state_transition(status, safety, test->requested_state, armed,
+					     true /* enable pre-arm checks */,
+					     nullptr /* no mavlink_log_pub */,
+					     status_flags,
+					     arm_req,
+					     2e6, /* 2 seconds after boot, everything should be checked */
+					     arm_disarm_reason_t::UNIT_TEST);
+
+		// Validate result of transition
+		ut_compare(test->assertMsg, test->expected_transition_result, result);
+		ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
+		ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
+		ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
+	}
+
+	return true;
+}
+
+bool StateMachineHelperTest::mainStateTransitionTest()
+{
+	// This structure represent a single test case for testing Main State transitions.
+	typedef struct {
+		const char *assertMsg;				// Text to show when test case fails
+		uint8_t	 condition_bits;			// Bits for various condition_* values
+		uint8_t	from_state;				// State prior to transition request
+		main_state_t to_state;				// State to transition to
+		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
+	} MainTransitionTest_t;
+
+	// Bits for condition_bits
+#define MTT_ALL_NOT_VALID		0
+#define MTT_ROTARY_WING			1 << 0
+#define MTT_LOC_ALT_VALID		1 << 1
+#define MTT_LOC_POS_VALID		1 << 2
+#define MTT_HOME_POS_VALID		1 << 3
+#define MTT_GLOBAL_POS_VALID		1 << 4
+
+	static const MainTransitionTest_t rgMainTransitionTests[] = {
+
+		// TRANSITION_NOT_CHANGED tests
+
+		{
+			"no transition: identical states",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED
+		},
+
+		// TRANSITION_CHANGED tests
+
+		{
+			"transition: MANUAL to ACRO - rotary",
+			MTT_ROTARY_WING,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to ACRO - not rotary",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: ACRO to MANUAL",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to AUTO_LOITER - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: AUTO_LOITER to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to AUTO_RTL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: AUTO_RTL to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to ALTCTL - not rotary",
+			MTT_LOC_ALT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
+			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
+			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: ALTCTL to MANUAL - local altitude valid",
+			MTT_LOC_ALT_VALID,
+			commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to POSCTL - local position not valid, global position valid",
+			MTT_GLOBAL_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: MANUAL to POSCTL - local position valid, global position not valid",
+			MTT_LOC_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
+		},
+
+		{
+			"transition: POSCTL to MANUAL - local position valid, global position valid",
+			MTT_LOC_POS_VALID,
+			commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
+		},
+
+		// TRANSITION_DENIED tests
+
+		{
+			"no transition: MANUAL to AUTO_MISSION - global position not valid",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: MANUAL to AUTO_LOITER - global position not valid",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
+			MTT_HOME_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
+			MTT_GLOBAL_POS_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
+		},
+
+		{
+			"transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
+			MTT_ROTARY_WING,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
+		},
+
+		{
+			"no transition: MANUAL to POSCTL - local position not valid, global position not valid",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED
+		},
+	};
+
+	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
+
+	for (size_t i = 0; i < cMainTransitionTests; i++) {
+		const MainTransitionTest_t *test = &rgMainTransitionTests[i];
+
+		// Setup initial machine state
+		struct vehicle_status_s current_vehicle_status = {};
+		struct commander_state_s current_commander_state = {};
+		struct vehicle_status_flags_s current_status_flags = {};
+
+		current_commander_state.main_state = test->from_state;
+		current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ?
+						      vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+		current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		current_status_flags.condition_auto_mission_available = true;
+
+		// Attempt transition
+		transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,
+					     current_commander_state);
+
+		// Validate result of transition
+		ut_compare(test->assertMsg, test->expected_transition_result, result);
+
+		if (test->expected_transition_result == result) {
+			if (test->expected_transition_result == TRANSITION_CHANGED) {
+				ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
+
+			} else {
+				ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
+			}
+		}
+	}
+
+	return true;
+}
+
+bool StateMachineHelperTest::run_tests()
+{
+	ut_run_test(armingStateTransitionTest);
+	ut_run_test(mainStateTransitionTest);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.h

@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 state_machine_helper_test.h
+ */
+
+#pragma once
+
+bool stateMachineHelperTest(void);
+

src/prometheus_px4/src/modules/commander/esc_calibration.cpp

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2017 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 esc_calibration.cpp
+ *
+ * Definition of esc calibration
+ *
+ * @author Roman Bapst <roman@px4.io>
+ */
+
+#include "esc_calibration.h"
+#include "calibration_messages.h"
+#include "calibration_routines.h"
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/battery_status.h>
+
+using namespace time_literals;
+
+bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
+{
+	uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
+	const battery_status_s &battery = batt_sub.get();
+	batt_sub.update();
+
+	if (battery.timestamp == 0) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable");
+		return false;
+	}
+
+	// Make sure battery is disconnected
+	// battery is not connected if the connected flag is not set and we have a recent battery measurement
+	if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) {
+		return true;
+	}
+
+	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
+	return false;
+}
+
+int do_esc_calibration(orb_advert_t *mavlink_log_pub)
+{
+	int	return_code = PX4_OK;
+	hrt_abstime timeout_start = 0;
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
+
+	uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
+	const battery_status_s &battery = batt_sub.get();
+	batt_sub.update();
+	bool batt_connected = battery.connected;
+
+	int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
+		return_code = PX4_ERROR;
+		goto Out;
+	}
+
+	/* tell IO/FMU that its ok to disable its safety with the switch */
+	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
+		return_code = PX4_ERROR;
+		goto Out;
+	}
+
+	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
+	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
+		return_code = PX4_ERROR;
+		goto Out;
+	}
+
+	/* tell IO to switch off safety without using the safety switch */
+	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
+		return_code = PX4_ERROR;
+		goto Out;
+	}
+
+	calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
+
+	timeout_start = hrt_absolute_time();
+
+	while (true) {
+		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
+		// sit high.
+		static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
+		static constexpr hrt_abstime pwm_high_timeout{3_s};
+		hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
+
+		if (hrt_elapsed_time(&timeout_start) > timeout_wait) {
+			if (!batt_connected) {
+				calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
+				return_code = PX4_ERROR;
+				goto Out;
+			}
+
+			// PWM was high long enough
+			break;
+		}
+
+		if (!batt_connected) {
+			if (batt_sub.update()) {
+				if (battery.connected) {
+					// Battery is connected, signal to user and start waiting again
+					batt_connected = true;
+					timeout_start = hrt_absolute_time();
+					calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
+				}
+			}
+		}
+
+		px4_usleep(50000);
+	}
+
+Out:
+
+	if (fd != -1) {
+		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
+			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
+		}
+
+		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
+			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
+		}
+
+		if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
+			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
+		}
+
+		px4_close(fd);
+	}
+
+	if (return_code == PX4_OK) {
+		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
+	}
+
+	return return_code;
+}

src/prometheus_px4/src/modules/commander/esc_calibration.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 esc_calibration.h
+ *
+ * Definition of esc calibration
+ *
+ * @author Roman Bapst <roman@px4.io>
+ */
+
+#ifndef ESC_CALIBRATION_H_
+#define ESC_CALIBRATION_H_
+
+#include <uORB/topics/actuator_armed.h>
+
+int do_esc_calibration(orb_advert_t *mavlink_log_pub);
+
+bool check_battery_disconnected(orb_advert_t *mavlink_log_pub);
+
+#endif

src/prometheus_px4/src/modules/commander/factory_calibration_storage.cpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <errno.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <lib/parameters/param.h>
+#include <px4_platform_common/log.h>
+
+#include "factory_calibration_storage.h"
+
+
+static const char *CALIBRATION_STORAGE = "/fs/mtd_caldata";
+
+static bool filter_calibration_params(param_t handle)
+{
+	const char *name = param_name(handle);
+	// filter all non-calibration params
+	return strncmp(name, "CAL_", 4) == 0 || strncmp(name, "TC_", 3) == 0;
+}
+
+FactoryCalibrationStorage::FactoryCalibrationStorage()
+{
+	int32_t param = 0;
+	param_get(param_find("SYS_FAC_CAL_MODE"), &param);
+	_enabled = param == 1;
+}
+
+int FactoryCalibrationStorage::open()
+{
+	if (_fd >= 0) {
+		cleanup();
+	}
+
+	if (!_enabled) {
+		return 0;
+	}
+
+	_fd = ::open(CALIBRATION_STORAGE, O_RDWR);
+
+	if (_fd == -1) {
+		return -errno;
+	}
+
+	PX4_INFO("Storing parameters to factory storage %s", CALIBRATION_STORAGE);
+	param_control_autosave(false);
+	return 0;
+}
+
+int FactoryCalibrationStorage::store()
+{
+	if (!_enabled) {
+		return 0;
+	}
+
+	int ret = param_export(_fd, false, filter_calibration_params);
+
+	if (ret != 0) {
+		PX4_ERR("param export failed (%i)", ret);
+	}
+
+	return ret;
+}
+
+void FactoryCalibrationStorage::cleanup()
+{
+	if (_enabled) {
+		param_control_autosave(true);
+	}
+
+	if (_fd >= 0) {
+		close(_fd);
+		_fd = -1;
+	}
+}
+

src/prometheus_px4/src/modules/commander/factory_calibration_storage.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @class FactoryCalibrationStorage
+ * Stores calibration parameters to a separate storage, if enabled by parameter
+ */
+class FactoryCalibrationStorage
+{
+public:
+	FactoryCalibrationStorage();
+	~FactoryCalibrationStorage() { cleanup(); }
+
+	/**
+	 * open the storage & disable param autosaving
+	 * @return 0 on success, <0 error otherwise
+	 */
+	int open();
+
+	/**
+	 * store the calibration parameters
+	 * Note: this method requires a lot of stack
+	 * @return 0 on success, <0 error otherwise
+	 */
+	int store();
+
+private:
+	void cleanup();
+
+	bool _enabled{false};
+	int _fd{-1};
+};

src/prometheus_px4/src/modules/commander/failure_detector/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2018 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(failure_detector
+	FailureDetector.cpp
+)

src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.cpp

@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 FailureDetector.cpp
+*
+* @author Mathieu Bresciani	<brescianimathieu@gmail.com>
+*
+*/
+
+#include "FailureDetector.hpp"
+
+using namespace time_literals;
+
+FailureDetector::FailureDetector(ModuleParams *parent) :
+	ModuleParams(parent)
+{
+}
+
+bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
+{
+	uint8_t previous_status = _status;
+
+	if (vehicle_control_mode.flag_control_attitude_enabled) {
+		updateAttitudeStatus();
+
+		if (_param_fd_ext_ats_en.get()) {
+			updateExternalAtsStatus();
+		}
+
+	} else {
+		_status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT);
+	}
+
+	if (_param_escs_en.get()) {
+		updateEscsStatus(vehicle_status);
+	}
+
+	return _status != previous_status;
+}
+
+void FailureDetector::updateAttitudeStatus()
+{
+	vehicle_attitude_s attitude;
+
+	if (_vehicule_attitude_sub.update(&attitude)) {
+
+		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
+		const float roll(euler.phi());
+		const float pitch(euler.theta());
+
+		const float max_roll_deg = _param_fd_fail_r.get();
+		const float max_pitch_deg = _param_fd_fail_p.get();
+		const float max_roll(fabsf(math::radians(max_roll_deg)));
+		const float max_pitch(fabsf(math::radians(max_pitch_deg)));
+
+		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
+
+		hrt_abstime time_now = hrt_absolute_time();
+
+		// Update hysteresis
+		_roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get()));
+		_pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_p_ttri.get()));
+		_roll_failure_hysteresis.set_state_and_update(roll_status, time_now);
+		_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
+
+		// Update bitmask
+		_status &= ~(FAILURE_ROLL | FAILURE_PITCH);
+
+		if (_roll_failure_hysteresis.get_state()) {
+			_status |= FAILURE_ROLL;
+		}
+
+		if (_pitch_failure_hysteresis.get_state()) {
+			_status |= FAILURE_PITCH;
+		}
+	}
+}
+
+void FailureDetector::updateExternalAtsStatus()
+{
+	pwm_input_s pwm_input;
+
+	if (_pwm_input_sub.update(&pwm_input)) {
+
+		uint32_t pulse_width = pwm_input.pulse_width;
+		bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);
+
+		hrt_abstime time_now = hrt_absolute_time();
+
+		// Update hysteresis
+		_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
+		_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now);
+
+		_status &= ~FAILURE_EXT;
+
+		if (_ext_ats_failure_hysteresis.get_state()) {
+			_status |= FAILURE_EXT;
+		}
+	}
+}
+
+void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
+{
+	hrt_abstime time_now = hrt_absolute_time();
+
+	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		esc_status_s esc_status;
+
+		if (_esc_status_sub.update(&esc_status)) {
+			int all_escs_armed = (1 << esc_status.esc_count) - 1;
+
+			_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+			_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
+
+			if (_esc_failure_hysteresis.get_state()) {
+				_status |= FAILURE_ARM_ESCS;
+			}
+		}
+
+	} else {
+		// reset ESC bitfield
+		_esc_failure_hysteresis.set_state_and_update(false, time_now);
+		_status &= ~FAILURE_ARM_ESCS;
+	}
+}

src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.hpp

@@ -0,0 +1,104 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 FailureDetector.hpp
+* Base class for failure detection logic based on vehicle states
+* for failsafe triggering.
+*
+* @author Mathieu Bresciani 	<brescianimathieu@gmail.com>
+*
+*/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/module_params.h>
+#include <hysteresis/hysteresis.h>
+
+// subscriptions
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/pwm_input.h>
+
+typedef enum {
+	FAILURE_NONE = vehicle_status_s::FAILURE_NONE,
+	FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL,
+	FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
+	FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
+	FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
+	FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC
+} failure_detector_bitmak;
+
+using uORB::SubscriptionData;
+
+class FailureDetector : public ModuleParams
+{
+public:
+	FailureDetector(ModuleParams *parent);
+
+	bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
+	uint8_t getStatus() const { return _status; }
+
+private:
+	void updateAttitudeStatus();
+	void updateExternalAtsStatus();
+	void updateEscsStatus(const vehicle_status_s &vehicle_status);
+
+	uint8_t _status{FAILURE_NONE};
+
+	systemlib::Hysteresis _roll_failure_hysteresis{false};
+	systemlib::Hysteresis _pitch_failure_hysteresis{false};
+	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
+	systemlib::Hysteresis _esc_failure_hysteresis{false};
+
+	uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
+	uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
+		(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
+		(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,
+		(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
+		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
+		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
+		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en
+	)
+};

src/prometheus_px4/src/modules/commander/failure_detector/failure_detector_params.c

@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 failure_detector_params.c
+ *
+ * Parameters used by the Failure Detector.
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <parameters/param.h>
+
+/**
+ * FailureDetector Max Roll
+ *
+ * Maximum roll angle before FailureDetector triggers the attitude_failure flag.
+ * The flag triggers flight termination (if @CBRK_FLIGHTTERM = 0),
+ * which sets outputs to their failsafe values.
+ * On takeoff the flag triggers lockdown (irrespective of @CBRK_FLIGHTTERM),
+ * which disarms motors but does not set outputs to failsafe values.
+ *
+ * Setting this parameter to 0 disables the check
+ *
+ * @min 60
+ * @max 180
+ * @unit deg
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_FAIL_R, 60);
+
+/**
+ * FailureDetector Max Pitch
+ *
+ * Maximum pitch angle before FailureDetector triggers the attitude_failure flag.
+ * The flag triggers flight termination (if @CBRK_FLIGHTTERM = 0),
+ * which sets outputs to their failsafe values.
+ * On takeoff the flag triggers lockdown (irrespective of @CBRK_FLIGHTTERM),
+ * which disarms motors but does not set outputs to failsafe values.
+ *
+ * Setting this parameter to 0 disables the check
+ *
+ * @min 60
+ * @max 180
+ * @unit deg
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_FAIL_P, 60);
+
+/**
+ * Roll failure trigger time
+ *
+ * Seconds (decimal) that roll has to exceed FD_FAIL_R before being considered as a failure.
+ *
+ * @unit s
+ * @min 0.02
+ * @max 5
+ * @decimal 2
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_FLOAT(FD_FAIL_R_TTRI, 0.3);
+
+/**
+ * Pitch failure trigger time
+ *
+ * Seconds (decimal) that pitch has to exceed FD_FAIL_P before being considered as a failure.
+ *
+ * @unit s
+ * @min 0.02
+ * @max 5
+ * @decimal 2
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_FLOAT(FD_FAIL_P_TTRI, 0.3);
+
+/**
+ * Enable PWM input on for engaging failsafe from an external automatic trigger system (ATS).
+ *
+ * Enabled on either AUX5 or MAIN5 depending on board.
+ * External ATS is required by ASTM F3322-18.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
+
+/**
+ * The PWM threshold from external automatic trigger system for engaging failsafe.
+ *
+ * External ATS is required by ASTM F3322-18.
+ *
+ * @unit us
+ * @decimal 2
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
+
+/**
+ * Enable checks on ESCs that report their arming state.
+ *
+ * If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
+ * Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_ESCS_EN, 1);

src/prometheus_px4/src/modules/commander/gyro_calibration.cpp

@@ -0,0 +1,298 @@
+/****************************************************************************
+*
+*   Copyright (c) 2013-2020 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 gyro_calibration.cpp
+ *
+ * Gyroscope calibration routine
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include "factory_calibration_storage.h"
+#include "gyro_calibration.h"
+#include "calibration_messages.h"
+#include "calibration_routines.h"
+#include "commander_helper.h"
+
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/time.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionBlocking.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
+static constexpr char sensor_name[] {"gyro"};
+static constexpr unsigned MAX_GYROS = 4;
+
+using matrix::Vector3f;
+
+/// Data passed to calibration worker routine
+struct gyro_worker_data_t {
+	orb_advert_t *mavlink_log_pub{nullptr};
+
+	calibration::Gyroscope calibrations[MAX_GYROS] {};
+
+	Vector3f offset[MAX_GYROS] {};
+
+	math::MedianFilter<float, 9> filter[3] {};
+};
+
+static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
+{
+	const hrt_abstime calibration_started = hrt_absolute_time();
+	unsigned calibration_counter[MAX_GYROS] {};
+	static constexpr unsigned CALIBRATION_COUNT = 250;
+	unsigned poll_errcount = 0;
+
+	uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
+		{ORB_ID(sensor_gyro), 0, 0},
+		{ORB_ID(sensor_gyro), 0, 1},
+		{ORB_ID(sensor_gyro), 0, 2},
+		{ORB_ID(sensor_gyro), 0, 3},
+	};
+
+	/* use slowest gyro to pace, but count correctly per-gyro for statistics */
+	unsigned slow_count = 0;
+
+	while (slow_count < CALIBRATION_COUNT) {
+		if (calibrate_cancel_check(worker_data.mavlink_log_pub, calibration_started)) {
+			return calibrate_return_cancelled;
+		}
+
+		if (gyro_sub[0].updatedBlocking(100000)) {
+			unsigned update_count = CALIBRATION_COUNT;
+
+			for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) {
+				if (worker_data.calibrations[gyro_index].device_id() != 0) {
+
+					if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
+						// Skip if instance has enough samples
+						continue;
+					}
+
+					sensor_gyro_s gyro_report;
+
+					while (gyro_sub[gyro_index].update(&gyro_report)) {
+						// fetch optional thermal offset corrections in sensor frame
+						const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()};
+
+						worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset;
+						calibration_counter[gyro_index]++;
+
+						if (gyro_index == 0) {
+							worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
+							worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
+							worker_data.filter[2].insert(gyro_report.z - thermal_offset(2));
+						}
+					}
+
+					// Maintain the sample count of the slowest sensor
+					if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) {
+						update_count = calibration_counter[gyro_index];
+					}
+				}
+			}
+
+			if (update_count % (CALIBRATION_COUNT / 20) == 0) {
+				calibration_log_info(worker_data.mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / CALIBRATION_COUNT);
+			}
+
+			// Propagate out the slowest sensor's count
+			if (slow_count < update_count) {
+				slow_count = update_count;
+			}
+
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			calibration_log_critical(worker_data.mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
+			return calibrate_return_error;
+		}
+	}
+
+	for (unsigned s = 0; s < MAX_GYROS; s++) {
+		if ((worker_data.calibrations[s].device_id() != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) {
+			calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", s)
+			return calibrate_return_error;
+		}
+
+		worker_data.offset[s] /= calibration_counter[s];
+	}
+
+	return calibrate_return_ok;
+}
+
+int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
+{
+	int res = PX4_OK;
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
+
+	gyro_worker_data_t worker_data{};
+	worker_data.mavlink_log_pub = mavlink_log_pub;
+
+	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
+	const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));
+
+	// Warn that we will not calibrate more than MAX_GYROS gyroscopes
+	if (orb_gyro_count > MAX_GYROS) {
+		calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, MAX_GYROS);
+
+	} else if (orb_gyro_count < 1) {
+		calibration_log_critical(mavlink_log_pub, "No gyros found");
+		return PX4_ERROR;
+	}
+
+	for (uint8_t cur_gyro = 0; cur_gyro < MAX_GYROS; cur_gyro++) {
+		uORB::SubscriptionData<sensor_gyro_s> gyro_sub{ORB_ID(sensor_gyro), cur_gyro};
+
+		if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) {
+			worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id);
+		}
+
+		// reset calibration index to match uORB numbering
+		worker_data.calibrations[cur_gyro].set_calibration_index(cur_gyro);
+	}
+
+	unsigned try_count = 0;
+	unsigned max_tries = 20;
+	res = PX4_ERROR;
+
+	do {
+		// Calibrate gyro and ensure user didn't move
+		calibrate_return cal_return = gyro_calibration_worker(worker_data);
+
+		if (cal_return == calibrate_return_cancelled) {
+			// Cancel message already sent, we are done here
+			res = PX4_ERROR;
+			break;
+
+		} else if (cal_return == calibrate_return_error) {
+			res = PX4_ERROR;
+
+		} else {
+			/* check offsets using a median filter */
+			float xdiff = worker_data.filter[0].median() - worker_data.offset[0](0);
+			float ydiff = worker_data.filter[1].median() - worker_data.offset[0](1);
+			float zdiff = worker_data.filter[2].median() - worker_data.offset[0](2);
+
+			/* maximum allowable calibration error */
+			static constexpr float maxoff = math::radians(0.6f);
+
+			if (!PX4_ISFINITE(worker_data.offset[0](0)) ||
+			    !PX4_ISFINITE(worker_data.offset[0](1)) ||
+			    !PX4_ISFINITE(worker_data.offset[0](2)) ||
+			    fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) {
+
+				calibration_log_critical(mavlink_log_pub, "motion, retrying..");
+				res = PX4_ERROR;
+
+			} else {
+				res = PX4_OK;
+			}
+		}
+
+		try_count++;
+
+	} while (res == PX4_ERROR && try_count <= max_tries);
+
+	if (try_count >= max_tries) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: Motion during calibration");
+		res = PX4_ERROR;
+	}
+
+	FactoryCalibrationStorage factory_storage;
+
+	if (factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		res = PX4_ERROR;
+	}
+
+	if (res == PX4_OK) {
+		// set offset parameters to new values
+		bool param_save = false;
+		bool failed = true;
+
+		for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
+
+			auto &calibration = worker_data.calibrations[uorb_index];
+
+			if (calibration.device_id() != 0) {
+				calibration.set_offset(worker_data.offset[uorb_index]);
+
+				calibration.set_calibration_index(uorb_index);
+
+				calibration.PrintStatus();
+
+				if (calibration.ParametersSave()) {
+					param_save = true;
+					failed = false;
+
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, "calibration save failed");
+					break;
+				}
+			}
+		}
+
+		if (!failed && factory_storage.store() != PX4_OK) {
+			failed = true;
+		}
+
+		if (param_save) {
+			param_notify_changes();
+		}
+
+		if (!failed) {
+			calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
+			px4_usleep(600000); // give this message enough time to propagate
+			return PX4_OK;
+		}
+	}
+
+	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
+	px4_usleep(600000); // give this message enough time to propagate
+
+	return PX4_ERROR;
+}

src/prometheus_px4/src/modules/commander/gyro_calibration.h

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   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 gyro_calibration.h
+ * Gyroscope calibration routine
+ */
+
+#ifndef GYRO_CALIBRATION_H_
+#define GYRO_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_gyro_calibration(orb_advert_t *mavlink_log_pub);
+
+#endif /* GYRO_CALIBRATION_H_ */

src/prometheus_px4/src/modules/commander/level_calibration.cpp

@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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 "level_calibration.h"
+#include "calibration_messages.h"
+#include "calibration_routines.h"
+#include "commander_helper.h"
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+#include <matrix/math.hpp>
+#include <lib/conversion/rotation.h>
+#include <lib/parameters/param.h>
+#include <systemlib/err.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionBlocking.hpp>
+
+using namespace time_literals;
+using namespace matrix;
+using math::radians;
+
+int do_level_calibration(orb_advert_t *mavlink_log_pub)
+{
+	bool success = false;
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
+
+	param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
+	param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
+
+	// get old values
+	float roll_offset_current = 0.f;
+	float pitch_offset_current = 0.f;
+	param_get(roll_offset_handle, &roll_offset_current);
+	param_get(pitch_offset_handle, &pitch_offset_current);
+
+	int32_t board_rot_current = 0;
+	param_get(param_find("SENS_BOARD_ROT"), &board_rot_current);
+
+	const Dcmf board_rotation_offset{Eulerf{radians(roll_offset_current), radians(pitch_offset_current), 0.f}};
+
+	float roll_mean = 0.f;
+	float pitch_mean = 0.f;
+	unsigned counter = 0;
+	bool had_motion = true;
+	int num_retries = 0;
+
+	uORB::SubscriptionBlocking<vehicle_attitude_s> att_sub{ORB_ID(vehicle_attitude)};
+
+	while (had_motion && num_retries++ < 50) {
+		Vector2f min_angles{100.f, 100.f};
+		Vector2f max_angles{-100.f, -100.f};
+		roll_mean = 0.0f;
+		pitch_mean = 0.0f;
+		counter = 0;
+		int last_progress_report = -100;
+		static constexpr hrt_abstime calibration_duration = 500_ms;
+		const hrt_abstime start = hrt_absolute_time();
+
+		while (hrt_elapsed_time(&start) < calibration_duration) {
+
+			vehicle_attitude_s att{};
+
+			if (!att_sub.updateBlocking(att, 100000)) {
+				// attitude estimator is not running
+				calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
+				calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
+				goto out;
+			}
+
+			int progress = 100 * hrt_elapsed_time(&start) / calibration_duration;
+
+			if (progress >= last_progress_report + 20) {
+				calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress);
+				last_progress_report = progress;
+			}
+
+			Eulerf att_euler{Quatf{att.q}};
+
+			// keep min + max angles
+			for (int i = 0; i < 2; ++i) {
+				if (att_euler(i) < min_angles(i)) { min_angles(i) = att_euler(i); }
+
+				if (att_euler(i) > max_angles(i)) { max_angles(i) = att_euler(i); }
+			}
+
+			att_euler(2) = 0.f; // ignore yaw
+
+			att_euler = Eulerf{board_rotation_offset *Dcmf{att_euler}};  // subtract existing board rotation
+			roll_mean += att_euler.phi();
+			pitch_mean += att_euler.theta();
+			++counter;
+		}
+
+		// motion detection: check that (max-min angle) is within a threshold.
+		// The difference is typically <0.1 deg while at rest
+		if (max_angles(0) - min_angles(0) < math::radians(0.5f) &&
+		    max_angles(1) - min_angles(1) < math::radians(0.5f)) {
+
+			had_motion = false;
+		}
+	}
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
+
+	roll_mean /= counter;
+	pitch_mean /= counter;
+
+	if (had_motion) {
+		calibration_log_critical(mavlink_log_pub, "motion during calibration");
+
+	} else if (fabsf(roll_mean) > 0.8f) {
+		calibration_log_critical(mavlink_log_pub, "excess roll angle");
+
+	} else if (fabsf(pitch_mean) > 0.8f) {
+		calibration_log_critical(mavlink_log_pub, "excess pitch angle");
+
+	} else {
+
+		float roll_mean_degrees = math::degrees(roll_mean);
+		float pitch_mean_degrees = math::degrees(pitch_mean);
+
+		if (fabsf(roll_offset_current - roll_mean_degrees) > 0.1f) {
+			PX4_INFO("Updating SENS_BOARD_X_OFF %.1f -> %.1f degrees", (double)roll_offset_current, (double)roll_mean_degrees);
+		}
+
+		if (fabsf(pitch_offset_current - pitch_mean_degrees) > 0.1f) {
+			PX4_INFO("Updating SENS_BOARD_Y_OFF %.1f -> %.1f degrees", (double)pitch_offset_current, (double)pitch_mean_degrees);
+		}
+
+		param_set_no_notification(roll_offset_handle, &roll_mean_degrees);
+		param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees);
+		param_notify_changes();
+		success = true;
+	}
+
+out:
+
+	if (success) {
+		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
+		px4_usleep(600000); // give this message enough time to propagate
+		return 0;
+
+	} else {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
+		px4_usleep(600000); // give this message enough time to propagate
+		return 1;
+	}
+}

src/prometheus_px4/src/modules/commander/level_calibration.h

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_level_calibration(orb_advert_t *mavlink_log_pub);

src/prometheus_px4/src/modules/commander/lm_fit.cpp

@@ -0,0 +1,356 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "lm_fit.hpp"
+
+struct iteration_result {
+	float gradient_damping;
+	float cost;
+	enum class STATUS {
+		SUCCESS, FAILURE
+	} result = STATUS::SUCCESS;
+};
+
+void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
+			     unsigned int samples_collected, sphere_params &params, iteration_result &result)
+{
+	// Run Sphere Fit using Levenberg Marquardt LSq Fit
+	const float lma_damping = 10.f;
+	float fitness = result.cost;
+	float fit1 = 0.f;
+	float fit2 = 0.f;
+
+	matrix::SquareMatrix<float, 4> JTJ;
+	float JTFI[4] {};
+	float residual = 0.0f;
+
+	// Gauss Newton Part common for all kind of extensions including LM
+	for (uint16_t k = 0; k < samples_collected; k++) {
+
+		float sphere_jacob[4];
+		//Calculate Jacobian
+		float A = (params.diag(0)    * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) +
+			  (params.offdiag(1) * (z[k] - params.offset(2)));
+		float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1)    * (y[k] - params.offset(1))) +
+			  (params.offdiag(2) * (z[k] - params.offset(2)));
+		float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) +
+			  (params.diag(2)    * (z[k] - params.offset(2)));
+		float length = sqrtf(A * A + B * B + C * C);
+
+		// 0: partial derivative (radius wrt fitness fn) fn operated on sample
+		sphere_jacob[0] = 1.0f;
+		// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
+		sphere_jacob[1] = 1.0f * (((params.diag(0)    * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length);
+		sphere_jacob[2] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1)    * B) + (params.offdiag(2) * C)) / length);
+		sphere_jacob[3] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2)    * C)) / length);
+		residual = params.radius - length;
+
+		for (uint8_t i = 0; i < 4; i++) {
+			// compute JTJ
+			for (uint8_t j = 0; j < 4; j++) {
+				JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
+			}
+
+			JTFI[i] += sphere_jacob[i] * residual;
+		}
+	}
+
+
+	//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
+	// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
+	float fit1_params[4] = {params.radius, params.offset(0), params.offset(1), params.offset(2)};
+	float fit2_params[4];
+	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
+	JTJ = (JTJ + JTJ.transpose()) * 0.5f; // fix numerical issues
+	matrix::SquareMatrix<float, 4> JTJ2 = JTJ;
+
+	for (uint8_t i = 0; i < 4; i++) {
+		JTJ(i, i) += result.gradient_damping;
+		JTJ2(i, i) += result.gradient_damping / lma_damping;
+	}
+
+	if (!JTJ.I(JTJ)) {
+		result.result = iteration_result::STATUS::FAILURE;
+		return;
+	}
+
+	if (!JTJ2.I(JTJ2)) {
+		result.result = iteration_result::STATUS::FAILURE;
+		return;
+
+	}
+
+	for (uint8_t row = 0; row < 4; row++) {
+		for (uint8_t col = 0; col < 4; col++) {
+			fit1_params[row] -= JTFI[col] * JTJ(row, col);
+			fit2_params[row] -= JTFI[col] * JTJ2(row, col);
+		}
+	}
+
+	// Calculate mean squared residuals
+	for (uint16_t k = 0; k < samples_collected; k++) {
+		float A = (params.diag(0)    * (x[k] - fit1_params[1])) + (params.offdiag(0) * (y[k] - fit1_params[2])) +
+			  (params.offdiag(1) *
+			   (z[k] + fit1_params[3]));
+		float B = (params.offdiag(0) * (x[k] - fit1_params[1])) + (params.diag(1)    * (y[k] - fit1_params[2])) +
+			  (params.offdiag(2) *
+			   (z[k] + fit1_params[3]));
+		float C = (params.offdiag(1) * (x[k] - fit1_params[1])) + (params.offdiag(2) * (y[k] - fit1_params[2])) + (params.diag(
+					2)    *
+				(z[k] - fit1_params[3]));
+		float length = sqrtf(A * A + B * B + C * C);
+		residual = fit1_params[0] - length;
+		fit1 += residual * residual;
+
+		A = (params.diag(0)    * (x[k] - fit2_params[1])) + (params.offdiag(0) * (y[k] - fit2_params[2])) + (params.offdiag(1) *
+				(z[k] - fit2_params[3]));
+		B = (params.offdiag(0) * (x[k] - fit2_params[1])) + (params.diag(1)    * (y[k] - fit2_params[2])) + (params.offdiag(2) *
+				(z[k] - fit2_params[3]));
+		C = (params.offdiag(1) * (x[k] - fit2_params[1])) + (params.offdiag(2) * (y[k] - fit2_params[2])) + (params.diag(2)    *
+				(z[k] - fit2_params[3]));
+		length = sqrtf(A * A + B * B + C * C);
+		residual = fit2_params[0] - length;
+		fit2 += residual * residual;
+	}
+
+	fit1 = sqrtf(fit1) / samples_collected;
+	fit2 = sqrtf(fit2) / samples_collected;
+
+	if (fit1 > result.cost && fit2 > result.cost) {
+		result.gradient_damping *= lma_damping;
+
+	} else if (fit2 < result.cost && fit2 < fit1) {
+		result.gradient_damping /= lma_damping;
+		memcpy(fit1_params, fit2_params, sizeof(fit1_params));
+		fitness = fit2;
+
+	} else if (fit1 < result.cost) {
+		fitness = fit1;
+	}
+
+	//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
+
+	if (PX4_ISFINITE(fitness) && fitness <= result.cost) {
+		result.cost = fitness;
+		params.radius = fit1_params[0];
+		params.offset(0) = fit1_params[1];
+		params.offset(1) = fit1_params[2];
+		params.offset(2) = fit1_params[3];
+		result.result = iteration_result::STATUS::SUCCESS;
+
+	} else {
+		result.result = iteration_result::STATUS::FAILURE;
+	}
+}
+
+void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[],
+				unsigned int samples_collected, sphere_params &params, iteration_result &result)
+{
+	// Run Sphere Fit using Levenberg Marquardt LSq Fit
+	const float lma_damping = 10.0f;
+	float fitness = result.cost;
+	float fit1 = 0.0f;
+	float fit2 = 0.0f;
+
+	matrix::SquareMatrix<float, 9> JTJ;
+	float JTFI[9] {};
+	float residual = 0.0f;
+	float ellipsoid_jacob[9];
+
+	// Gauss Newton Part common for all kind of extensions including LM
+	for (uint16_t k = 0; k < samples_collected; k++) {
+
+		// Calculate Jacobian
+		float A = (params.diag(0)    * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) +
+			  (params.offdiag(1) * (z[k] - params.offset(2)));
+		float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1)    * (y[k] - params.offset(1))) +
+			  (params.offdiag(2) * (z[k] - params.offset(2)));
+		float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) +
+			  (params.diag(2)    * (z[k] - params.offset(2)));
+		float length = sqrtf(A * A + B * B + C * C);
+		residual = params.radius - length;
+		fit1 += residual * residual;
+		// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
+		ellipsoid_jacob[0] = 1.0f * (((params.diag(0)    * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length);
+		ellipsoid_jacob[1] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1)    * B) + (params.offdiag(2) * C)) / length);
+		ellipsoid_jacob[2] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2)    * C)) / length);
+		// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
+		ellipsoid_jacob[3] = -1.0f * ((x[k] - params.offset(0)) * A) / length;
+		ellipsoid_jacob[4] = -1.0f * ((y[k] - params.offset(1)) * B) / length;
+		ellipsoid_jacob[5] = -1.0f * ((z[k] - params.offset(2)) * C) / length;
+		// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
+		ellipsoid_jacob[6] = -1.0f * (((y[k] - params.offset(1)) * A) + ((x[k] - params.offset(0)) * B)) / length;
+		ellipsoid_jacob[7] = -1.0f * (((z[k] - params.offset(2)) * A) + ((x[k] - params.offset(0)) * C)) / length;
+		ellipsoid_jacob[8] = -1.0f * (((z[k] - params.offset(2)) * B) + ((y[k] - params.offset(1)) * C)) / length;
+
+		for (uint8_t i = 0; i < 9; i++) {
+			// compute JTJ
+			for (uint8_t j = 0; j < 9; j++) {
+				JTJ(i, j) += ellipsoid_jacob[i] * ellipsoid_jacob[j];
+			}
+
+			JTFI[i] += ellipsoid_jacob[i] * residual;
+		}
+	}
+
+
+	//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
+	// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
+	float fit1_params[9] = {params.offset(0), params.offset(1), params.offset(2), params.diag(0), params.diag(1), params.diag(2), params.offdiag(0), params.offdiag(1), params.offdiag(2)};
+	float fit2_params[9];
+	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
+	matrix::SquareMatrix<float, 9> JTJ2 = JTJ;
+
+	for (uint8_t i = 0; i < 9; i++) {
+		JTJ(i, i) += result.gradient_damping;
+		JTJ2(i, i) += result.gradient_damping / lma_damping;
+	}
+
+
+	if (!JTJ.I(JTJ)) {
+		result.result = iteration_result::STATUS::FAILURE;
+		return;
+	}
+
+	if (!JTJ2.I(JTJ2)) {
+		result.result = iteration_result::STATUS::FAILURE;
+		return;
+	}
+
+	for (uint8_t row = 0; row < 9; row++) {
+		for (uint8_t col = 0; col < 9; col++) {
+			fit1_params[row] -= JTFI[col] * JTJ(row, col);
+			fit2_params[row] -= JTFI[col] * JTJ2(row, col);
+		}
+	}
+
+	// Calculate mean squared residuals
+	for (uint16_t k = 0; k < samples_collected; k++) {
+		float A = (fit1_params[3]    * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] *
+				(z[k] - fit1_params[2]));
+		float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4]   * (y[k] - fit1_params[1])) + (fit1_params[8] *
+				(z[k] - fit1_params[2]));
+		float C = (fit1_params[7] * (x[k] - fit1_params[0])) + (fit1_params[8] * (y[k] - fit1_params[1])) + (fit1_params[5]    *
+				(z[k] - fit1_params[2]));
+		float length = sqrtf(A * A + B * B + C * C);
+		residual = params.radius - length;
+		fit1 += residual * residual;
+
+		A = (fit2_params[3]    * (x[k] - fit2_params[0])) + (fit2_params[6] * (y[k] - fit2_params[1])) + (fit2_params[7] *
+				(z[k] - fit2_params[2]));
+		B = (fit2_params[6] * (x[k] - fit2_params[0])) + (fit2_params[4]   * (y[k] - fit2_params[1])) + (fit2_params[8] *
+				(z[k] - fit2_params[2]));
+		C = (fit2_params[7] * (x[k] - fit2_params[0])) + (fit2_params[8] * (y[k] - fit2_params[1])) + (fit2_params[5]    *
+				(z[k] - fit2_params[2]));
+		length = sqrtf(A * A + B * B + C * C);
+		residual = params.radius - length;
+		fit2 += residual * residual;
+	}
+
+	fit1 = sqrtf(fit1) / samples_collected;
+	fit2 = sqrtf(fit2) / samples_collected;
+
+	if (fit1 > result.cost && fit2 > result.cost) {
+		result.gradient_damping *= lma_damping;
+
+	} else if (fit2 < result.cost && fit2 < fit1) {
+		result.gradient_damping /= lma_damping;
+		memcpy(fit1_params, fit2_params, sizeof(fit1_params));
+		fitness = fit2;
+
+	} else if (fit1 < result.cost) {
+		fitness = fit1;
+	}
+
+	//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
+	if (PX4_ISFINITE(fitness) && fitness <= result.cost) {
+		result.cost = fitness;
+		params.offset(0) = fit1_params[0];
+		params.offset(1) = fit1_params[1];
+		params.offset(2) = fit1_params[2];
+		params.diag(0) = fit1_params[3];
+		params.diag(1) = fit1_params[4];
+		params.diag(2) = fit1_params[5];
+		params.offdiag(0) = fit1_params[6];
+		params.offdiag(1) = fit1_params[7];
+		params.offdiag(2) = fit1_params[8];
+		result.result = iteration_result::STATUS::SUCCESS;
+
+	} else {
+		result.result = iteration_result::STATUS::FAILURE;
+	}
+}
+
+
+
+int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params &params,
+	       bool full_ellipsoid)
+{
+
+	const int max_iterations = 100;
+	const int min_iterations = 10;
+	const float cost_threshold = 0.01;
+	const float step_threshold = 0.001;
+
+	const float min_radius = 0.2;
+	const float max_radius = 0.7;
+
+	iteration_result iter;
+	iter.cost = 1e30f;
+	iter.gradient_damping = 1;
+
+	bool success = false;
+
+	for (int i = 0; i < max_iterations; i++) {
+		if (full_ellipsoid) {
+			lm_ellipsoid_fit_iteration(x, y, z, samples_collected, params, iter);
+
+		} else {
+			lm_sphere_fit_iteration(x, y, z, samples_collected, params, iter);
+		}
+
+		if (iter.result == iteration_result::STATUS::SUCCESS
+		    && min_radius < params.radius && params.radius < max_radius
+		    && i > min_iterations && (iter.cost < cost_threshold || iter.gradient_damping < step_threshold)) {
+			success = true;
+			break;
+		}
+	}
+
+	if (success) {
+		return PX4_OK;
+	}
+
+	return 1;
+}
+

src/prometheus_px4/src/modules/commander/lm_fit.hpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/defines.h>
+
+struct sphere_params {
+	matrix::Vector3f offset, diag{1.f, 1.f, 1.f}, offdiag;
+	float radius{0.2f};
+};
+
+
+/**
+ * Least-squares fit of a sphere to a set of points.
+ *
+ * Fits a sphere to a set of points on the sphere surface.
+ *
+ * @param x point coordinates on the X axis
+ * @param y point coordinates on the Y axis
+ * @param z point coordinates on the Z axis
+ * @param samples_collected number of points
+ * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
+ * @param params the values to be optimized
+ * @param full_ellipsoid whether to just optimize a sphere, or do an ellipsoid optimization
+ *
+ * NB!! If you optimize the full ellipsoid, you must have already optimized without the full ellipsoid
+ *
+ * @return 0 on success, 1 on failure
+ */
+int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params &params,
+	       bool full_ellipsoid);

src/prometheus_px4/src/modules/commander/mag_calibration.h

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 mag_calibration.h
+ * Magnetometer calibration routine
+ */
+
+#ifndef MAG_CALIBRATION_H_
+#define MAG_CALIBRATION_H_
+
+#include <math.h>
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_mag_calibration(orb_advert_t *mavlink_log_pub);
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians = 0, float latitude = NAN,
+			     float longitude = NAN);
+
+#endif /* MAG_CALIBRATION_H_ */

src/prometheus_px4/src/modules/commander/mag_calibration_test.cpp

@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Magnetometer calibration routine
+ * Run this test only using make tests TESTFILTER=mag_calibration
+ *
+ * @author Mathieu Bresciani <mathieu@auterion.com>
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/defines.h>
+
+#include "lm_fit.hpp"
+#include "mag_calibration_test_data.h"
+
+using matrix::Vector3f;
+
+class MagCalTest : public ::testing::Test
+{
+public:
+	void generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
+
+	/* Generate regularly spaced data on a sphere
+	 * Ref.: How to generate equidistributed points on the surface of a sphere, Markus Deserno, 2004
+	 */
+	void generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
+
+	void modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets, Vector3f scale_factors);
+};
+
+void MagCalTest::generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
+{
+	float psi = 0.f;
+	float theta = 0.f;
+	const float d_angle = 2.f * M_PI_F / float(n_samples / 2);
+
+	for (int i = 0; i < int(n_samples / 2); i++) {
+		x[i] = mag_str * sinf(psi);
+		y[i] = mag_str * cosf(psi);
+		z[i] = 0.f;
+		psi += d_angle;
+	}
+
+	for (int i = int(n_samples / 2); i < int(n_samples); i++) {
+		x[i] = mag_str * sinf(theta);
+		y[i] = 0.f;
+		z[i] = mag_str * cosf(theta);
+		theta += d_angle;
+	}
+}
+
+void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
+{
+	const float a = 4.f * M_PI_F * mag_str * mag_str / n_samples;
+	const float d = sqrtf(a);
+	const int m_theta = static_cast<int>(M_PI_F / d);
+	const float d_theta = M_PI_F / static_cast<float>(m_theta);
+	const float d_phi = a / d_theta;
+
+	unsigned int n_count = 0;
+
+	for (int m = 0; m < m_theta; m++) {
+		const float theta = M_PI_F * (m + 0.5f) / static_cast<float>(m_theta);
+		const int m_phi = static_cast<int>(2.f * M_PI_F * sinf(theta / d_phi));
+
+		for (int n = 0; n < m_phi; n++) {
+			const float phi = 2.f * M_PI_F * n / static_cast<float>(m_phi);
+			x[n_count] = mag_str * sinf(theta) * cosf(phi);
+			y[n_count] = mag_str * sinf(theta) * sinf(phi);
+			z[n_count] = mag_str * cosf(theta);
+			n_count++;
+		}
+	}
+
+	if (n_count > n_samples) {
+		printf("Error placing samples, n = %d\n", n_count);
+		return;
+	}
+
+	// Padd with constant data
+	while (n_count < n_samples) {
+		x[n_count] = x[n_count - 1];
+		y[n_count] = y[n_count - 1];
+		z[n_count] = z[n_count - 1];
+		n_count++;
+	}
+}
+
+void MagCalTest::modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets,
+				   Vector3f scale_factors)
+{
+	for (unsigned int k = 0; k < n_samples; k++) {
+		x[k] = x[k] * scale_factors(0) + offsets(0);
+		y[k] = y[k] * scale_factors(1) + offsets(1);
+		z[k] = z[k] * scale_factors(2) + offsets(2);
+	}
+}
+
+TEST_F(MagCalTest, sphere2Sides)
+{
+	// GIVEN: a dataset of points located on two orthogonal circles
+	// perfectly centered on the origin
+	static constexpr unsigned int N_SAMPLES = 240;
+
+	const float mag_str_true = 0.4f;
+	const Vector3f offset_true;
+	const Vector3f scale_true = {1.f, 1.f, 1.f};
+
+	float x[N_SAMPLES];
+	float y[N_SAMPLES];
+	float z[N_SAMPLES];
+
+	generate2SidesMagData(x, y, z, N_SAMPLES, mag_str_true);
+
+	// WHEN: fitting a sphere with the data and given a wrong initial radius
+	sphere_params sphere;
+	sphere.diag = {1.f, 1.f, 1.f};
+	sphere.radius = 0.2;
+	int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
+
+	// THEN: the algorithm should converge in a single step
+	EXPECT_EQ(success, PX4_OK);
+	EXPECT_NEAR(sphere.radius, mag_str_true, 0.001f) << "radius: " << sphere.radius;
+	EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.001f) << "offset X: " << sphere.offset(0);
+	EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.001f) << "offset Y: " << sphere.offset(1);
+	EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.001f) << "offset Z: " << sphere.offset(2);
+	EXPECT_NEAR(sphere.diag(0), scale_true(0), 0.001f) << "scale X: " << sphere.diag(0);
+	EXPECT_NEAR(sphere.diag(1), scale_true(1), 0.001f) << "scale Y: " << sphere.diag(1);
+	EXPECT_NEAR(sphere.diag(2), scale_true(2), 0.001f) << "scale Z: " << sphere.diag(2);
+}
+
+TEST_F(MagCalTest, sphereRegularlySpaced)
+{
+	// GIVEN: a dataset of regularly spaced points
+	// on a perfect sphere but not centered on the origin
+	static constexpr unsigned int N_SAMPLES = 240;
+
+	const float mag_str_true = 0.4f;
+	const Vector3f offset_true = {-1.07f, 0.35f, -0.78f};
+	const Vector3f scale_true = {1.f, 1.f, 1.f};
+
+	float x[N_SAMPLES];
+	float y[N_SAMPLES];
+	float z[N_SAMPLES];
+	generateRegularData(x, y, z, N_SAMPLES, mag_str_true);
+	modifyOffsetScale(x, y, z, N_SAMPLES, offset_true, scale_true);
+
+	// WHEN: fitting a sphere to the data
+	sphere_params sphere;
+	sphere.diag = {1.f, 1.f, 1.f};
+	sphere.radius = 0.2;
+	int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
+
+	// THEN: the algorithm should converge in a few iterations and
+	// find the correct parameters
+	EXPECT_EQ(success, PX4_OK);
+	EXPECT_NEAR(sphere.radius, mag_str_true, 0.001f) << "radius: " << sphere.radius;
+	EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.001f) << "offset X: " << sphere.offset(0);
+	EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.001f) << "offset Y: " << sphere.offset(1);
+	EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.001f) << "offset Z: " << sphere.offset(2);
+	EXPECT_NEAR(sphere.diag(0), scale_true(0), 0.001f) << "scale X: " << scale_true(0);
+	EXPECT_NEAR(sphere.diag(1), scale_true(1), 0.001f) << "scale Y: " << scale_true(1);
+	EXPECT_NEAR(sphere.diag(2), scale_true(2), 0.001f) << "scale Z: " << scale_true(2);
+}
+
+TEST_F(MagCalTest, replayTestData)
+{
+	// GIVEN: a real test dataset with large offsets
+	// and where the two first iterations of the LM algorithm
+	// produces a negative radius and a constant fitness value
+	constexpr unsigned int N_SAMPLES = 231;
+
+	const float mag_str_true = 0.4f;
+	const Vector3f offset_true = {-0.18f, 0.05f, -0.58f};
+
+	// WHEN: fitting a sphere to the data
+	sphere_params sphere;
+	sphere.diag = {1.f, 1.f, 1.f};
+	sphere.radius = 0.2;
+	int sphere_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, sphere, false);
+
+	// THEN: the algorithm should converge and find the correct parameters
+	EXPECT_EQ(sphere_success, PX4_OK);
+	EXPECT_NEAR(sphere.radius, mag_str_true, 0.1f) << "radius: " << sphere.radius;
+	EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.01f) << "offset X: " << sphere.offset(0);
+	EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.01f) << "offset Y: " << sphere.offset(1);
+	EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.01f) << "offset Z: " << sphere.offset(2);
+
+	printf("Ellipsoid fit\n");
+	sphere_params ellipsoid;
+	ellipsoid.diag = {1.f, 1.f, 1.f};
+	ellipsoid.radius = 0.2;
+	int ellipsoid_step_1_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, false);
+	int ellipsoid_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, true);
+	const Vector3f scale_true = {1.f, 1.06f, 0.94f};
+
+	EXPECT_EQ(ellipsoid_step_1_success, PX4_OK);
+	EXPECT_EQ(ellipsoid_success, PX4_OK);
+	EXPECT_NEAR(ellipsoid.radius, mag_str_true, 0.1f) << "radius: " << sphere.radius;
+	EXPECT_NEAR(ellipsoid.offset(0), offset_true(0), 0.01f) << "offset X: " << ellipsoid.offset(0);
+	EXPECT_NEAR(ellipsoid.offset(1), offset_true(1), 0.01f) << "offset Y: " << ellipsoid.offset(1);
+	EXPECT_NEAR(ellipsoid.offset(2), offset_true(2), 0.01f) << "offset Z: " << ellipsoid.offset(2);
+	EXPECT_NEAR(ellipsoid.diag(0), scale_true(0), 0.01f) << "scale X: " << ellipsoid.diag(0);
+	EXPECT_NEAR(ellipsoid.diag(1), scale_true(1), 0.01f) << "scale Y: " << ellipsoid.diag(1);
+	EXPECT_NEAR(ellipsoid.diag(2), scale_true(2), 0.01f) << "scale Z: " << ellipsoid.diag(2);
+}

src/prometheus_px4/src/modules/commander/px4_custom_mode.h

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2016 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 px4_custom_mode.h
+ * PX4 custom flight modes
+ *
+ */
+
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_H_
+
+#include <stdint.h>
+
+enum PX4_CUSTOM_MAIN_MODE {
+	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
+	PX4_CUSTOM_MAIN_MODE_ALTCTL,
+	PX4_CUSTOM_MAIN_MODE_POSCTL,
+	PX4_CUSTOM_MAIN_MODE_AUTO,
+	PX4_CUSTOM_MAIN_MODE_ACRO,
+	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
+	PX4_CUSTOM_MAIN_MODE_STABILIZED,
+	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
+	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
+};
+
+enum PX4_CUSTOM_SUB_MODE_AUTO {
+	PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
+	PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
+	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
+	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+	PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
+	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
+	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
+};
+
+enum PX4_CUSTOM_SUB_MODE_POSCTL {
+	PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
+	PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
+};
+
+union px4_custom_mode {
+	struct {
+		uint16_t reserved;
+		uint8_t main_mode;
+		uint8_t sub_mode;
+	};
+	uint32_t data;
+	float data_float;
+	struct {
+		uint16_t reserved_hl;
+		uint16_t custom_mode_hl;
+	};
+};
+
+#endif /* PX4_CUSTOM_MODE_H_ */

src/prometheus_px4/src/modules/commander/rc_calibration.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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 rc_calibration.cpp
+ * Remote Control calibration routine
+ */
+
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/defines.h>
+
+#include "rc_calibration.h"
+#include "commander_helper.h"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <systemlib/mavlink_log.h>
+#include <parameters/param.h>
+#include <systemlib/err.h>
+
+int do_trim_calibration(orb_advert_t *mavlink_log_pub)
+{
+	uORB::Subscription manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	px4_usleep(400000);
+	manual_control_setpoint_s manual_control_setpoint{};
+	bool changed = manual_control_setpoint_sub.updated();
+
+	if (!changed) {
+		mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
+		return PX4_ERROR;
+	}
+
+	manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	/* load trim values which are active */
+	float roll_trim_active;
+	param_get(param_find("TRIM_ROLL"), &roll_trim_active);
+	float pitch_trim_active;
+	param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
+	float yaw_trim_active;
+	param_get(param_find("TRIM_YAW"), &yaw_trim_active);
+
+	/* get manual control scale values */
+	float roll_scale;
+	param_get(param_find("FW_MAN_R_SC"), &roll_scale);
+	float pitch_scale;
+	param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
+	float yaw_scale;
+	param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);
+
+	/* set parameters: the new trim values are the combination of active trim values
+	   and the values coming from the remote control of the user
+	*/
+	float p = manual_control_setpoint.y * roll_scale + roll_trim_active;
+	int p1r = param_set(param_find("TRIM_ROLL"), &p);
+	/*
+	 we explicitly swap sign here because the trim is added to the actuator controls
+	 which are moving in an inverse sense to manual pitch inputs
+	*/
+	p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
+	int p2r = param_set(param_find("TRIM_PITCH"), &p);
+	p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
+	int p3r = param_set(param_find("TRIM_YAW"), &p);
+
+	if (p1r != 0 || p2r != 0 || p3r != 0) {
+		mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
+		return PX4_ERROR;
+	}
+
+	mavlink_log_info(mavlink_log_pub, "trim cal done");
+
+	return PX4_OK;
+}

src/prometheus_px4/src/modules/commander/rc_calibration.h

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   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 rc_calibration.h
+ * Remote Control calibration routine
+ */
+
+#ifndef RC_CALIBRATION_H_
+#define RC_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+int do_trim_calibration(orb_advert_t *mavlink_log_pub);
+
+#endif /* RC_CALIBRATION_H_ */

src/prometheus_px4/src/modules/commander/state_machine_helper.h

@@ -0,0 +1,164 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 state_machine_helper.h
+ * State machine helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef STATE_MACHINE_HELPER_H_
+#define STATE_MACHINE_HELPER_H_
+
+#include <drivers/drv_hrt.h>
+
+#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/commander_state.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+typedef enum {
+	TRANSITION_DENIED = -1,
+	TRANSITION_NOT_CHANGED = 0,
+	TRANSITION_CHANGED
+} transition_result_t;
+
+enum class link_loss_actions_t {
+	DISABLED = 0,
+	AUTO_LOITER = 1,	// Hold mode
+	AUTO_RTL = 2,		// Return mode
+	AUTO_LAND = 3,		// Land mode
+	TERMINATE = 5,		// Terminate flight (set actuator outputs to failsafe values, and stop controllers)
+	LOCKDOWN = 6,		// Lock actuators (set actuator outputs to disarmed values)
+};
+
+enum class offboard_loss_actions_t {
+	DISABLED = -1,
+	AUTO_LAND = 0,		// Land mode
+	AUTO_LOITER = 1,	// Hold mode
+	AUTO_RTL = 2,		// Return mode
+	TERMINATE = 3,		// Terminate flight (set actuator outputs to failsafe values, and stop controllers)
+	LOCKDOWN = 4,		// Lock actuators (set actuator outputs to disarmed values)
+};
+
+enum class offboard_loss_rc_actions_t {
+	DISABLED = -1, 		// Disabled
+	MANUAL_POSITION = 0, 	// Position mode
+	MANUAL_ALTITUDE = 1, 	// Altitude mode
+	MANUAL_ATTITUDE = 2, 	// Manual
+	AUTO_RTL = 3, 		// Return mode
+	AUTO_LAND = 4, 		// Land mode
+	AUTO_LOITER = 5, 	// Hold mode
+	TERMINATE = 6, 		// Terminate flight (set actuator outputs to failsafe values, and stop controllers)
+	LOCKDOWN = 7, 		// Lock actuators (set actuator outputs to disarmed values)
+};
+
+enum class position_nav_loss_actions_t {
+	ALTITUDE_MANUAL = 0,	// Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
+	LAND_TERMINATE = 1,	// Land/Terminate.  Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
+};
+
+extern const char *const arming_state_names[];
+extern const char *const nav_state_names[];
+
+enum class arm_disarm_reason_t {
+	TRANSITION_TO_STANDBY = 0,
+	RC_STICK = 1,
+	RC_SWITCH = 2,
+	COMMAND_INTERNAL = 3,
+	COMMAND_EXTERNAL = 4,
+	MISSION_START = 5,
+	SAFETY_BUTTON = 6,
+	AUTO_DISARM_LAND = 7,
+	AUTO_DISARM_PREFLIGHT = 8,
+	KILL_SWITCH = 9,
+	LOCKDOWN = 10,
+	FAILURE_DETECTOR = 11,
+	SHUTDOWN = 12,
+	UNIT_TEST = 13
+};
+
+enum RCLossExceptionBits {
+	RCL_EXCEPT_MISSION = (1 << 0),
+	RCL_EXCEPT_HOLD = (1 << 1),
+	RCL_EXCEPT_OFFBOARD = (1 << 2)
+};
+
+transition_result_t
+arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state,
+			actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
+			vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
+			const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
+
+transition_result_t
+main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
+		      const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);
+
+void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason);
+
+bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state,
+		   orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
+		   const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
+		   const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
+		   const offboard_loss_rc_actions_t offb_loss_rc_act,
+		   const position_nav_loss_actions_t posctl_nav_loss_act,
+		   const float param_com_rcl_act_t, const int param_com_rcl_except);
+
+/*
+ * Checks the validty of position data against the requirements of the current navigation
+ * mode and switches mode if position data required is not available.
+ */
+bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
+				 const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
+
+
+// COM_LOW_BAT_ACT parameter values
+typedef enum LOW_BAT_ACTION {
+	WARNING = 0,		// Warning
+	RETURN = 1,			// Return mode
+	LAND = 2,			// Land mode
+	RETURN_OR_LAND = 3	// Return mode at critically low level, Land mode at current position if reaching dangerously low levels
+} low_battery_action_t;
+
+void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+		      const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning,
+		      const low_battery_action_t low_bat_action);
+
+#endif /* STATE_MACHINE_HELPER_H_ */

src/prometheus_px4/src/modules/commander/worker_thread.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "worker_thread.hpp"
+#include "accelerometer_calibration.h"
+#include "airspeed_calibration.h"
+#include "calibration_routines.h"
+#include "esc_calibration.h"
+#include "gyro_calibration.h"
+#include "level_calibration.h"
+#include "mag_calibration.h"
+#include "rc_calibration.h"
+
+#include <px4_platform_common/log.h>
+#include <parameters/param.h>
+
+WorkerThread::~WorkerThread()
+{
+	if (_state.load() == (int)State::Running) {
+		/* wait for thread to complete */
+		int ret = pthread_join(_thread_handle, nullptr);
+
+		if (ret) {
+			PX4_ERR("join failed: %d", ret);
+		}
+	}
+}
+
+void WorkerThread::startTask(Request request)
+{
+	if (isBusy()) {
+		return;
+	}
+
+	_request = request;
+
+	/* initialize low priority thread */
+	pthread_attr_t low_prio_attr;
+	pthread_attr_init(&low_prio_attr);
+	pthread_attr_setstacksize(&low_prio_attr, PX4_STACK_ADJUSTED(4804));
+
+#ifndef __PX4_QURT
+	// This is not supported by QURT (yet).
+	struct sched_param param;
+	pthread_attr_getschedparam(&low_prio_attr, &param);
+
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+	pthread_attr_setschedparam(&low_prio_attr, &param);
+#endif
+	int ret = pthread_create(&_thread_handle, &low_prio_attr, &threadEntryTrampoline, this);
+	pthread_attr_destroy(&low_prio_attr);
+
+	if (ret == 0) {
+		_state.store((int)State::Running);
+
+	} else {
+		PX4_ERR("Failed to start thread (%i)", ret);
+		_state.store((int)State::Finished);
+		_ret_value = ret;
+	}
+}
+
+void *WorkerThread::threadEntryTrampoline(void *arg)
+{
+	WorkerThread *worker_thread = (WorkerThread *)arg;
+	worker_thread->threadEntry();
+	return nullptr;
+}
+
+void WorkerThread::threadEntry()
+{
+	px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid());
+
+	switch (_request) {
+	case Request::GyroCalibration:
+		_ret_value = do_gyro_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::MagCalibration:
+		_ret_value = do_mag_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::RCTrimCalibration:
+		_ret_value = do_trim_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::AccelCalibration:
+		_ret_value = do_accel_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::LevelCalibration:
+		_ret_value = do_level_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::AccelCalibrationQuick:
+		_ret_value = do_accel_calibration_quick(&_mavlink_log_pub);
+		break;
+
+	case Request::AirspeedCalibration:
+		_ret_value = do_airspeed_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::ESCCalibration:
+		_ret_value = do_esc_calibration(&_mavlink_log_pub);
+		break;
+
+	case Request::MagCalibrationQuick:
+		_ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude);
+		break;
+
+	case Request::ParamLoadDefault:
+		_ret_value = param_load_default();
+
+		if (_ret_value != 0) {
+			mavlink_log_critical(&_mavlink_log_pub, "Error loading settings");
+		}
+
+		break;
+
+	case Request::ParamSaveDefault:
+		_ret_value = param_save_default();
+
+		if (_ret_value != 0) {
+			mavlink_log_critical(&_mavlink_log_pub, "Error saving settings");
+		}
+
+		break;
+
+	case Request::ParamResetAll:
+		param_reset_all();
+		_ret_value = 0;
+		break;
+	}
+
+	_state.store((int)State::Finished); // set this last to signal the main thread we're done
+}
+
+void WorkerThread::setMagQuickData(float heading_rad, float lat, float lon)
+{
+	_heading_radians = heading_rad;
+	_latitude = lat;
+	_longitude = lon;
+}

src/prometheus_px4/src/modules/commander/worker_thread.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/posix.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/uORB.h>
+
+/**
+ * @class WorkerThread
+ * low priority background thread, started on demand, used for:
+ * - calibration
+ * - param saving
+ */
+class WorkerThread
+{
+public:
+	enum class Request {
+		GyroCalibration,
+		MagCalibration,
+		RCTrimCalibration,
+		AccelCalibration,
+		LevelCalibration,
+		AccelCalibrationQuick,
+		AirspeedCalibration,
+		ESCCalibration,
+		MagCalibrationQuick,
+
+		ParamLoadDefault,
+		ParamSaveDefault,
+		ParamResetAll,
+	};
+
+	WorkerThread() = default;
+	~WorkerThread();
+
+	void setMagQuickData(float heading_rad, float lat, float lon);
+
+	void startTask(Request request);
+
+	bool isBusy() const { return _state.load() != (int)State::Idle; }
+	bool hasResult() const { return _state.load() == (int)State::Finished; }
+	int getResultAndReset() { _state.store((int)State::Idle); return _ret_value; }
+
+private:
+	enum class State {
+		Idle,
+		Running,
+		Finished
+	};
+
+	static void *threadEntryTrampoline(void *arg);
+	void threadEntry();
+
+	px4::atomic_int _state{(int)State::Idle};
+	pthread_t _thread_handle{};
+	int _ret_value{};
+	Request _request;
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	// extra arguments
+	float _heading_radians;
+	float _latitude;
+	float _longitude;
+
+};
+

src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 ActuatorEffectiveness.hpp
+ *
+ * Interface for Actuator Effectiveness
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <ControlAllocation/ControlAllocation.hpp>
+
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/vehicle_actuator_setpoint.h>
+
+class ActuatorEffectiveness
+{
+public:
+	ActuatorEffectiveness() = default;
+	virtual ~ActuatorEffectiveness() = default;
+
+	static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
+	static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	virtual void setFlightPhase(const FlightPhase &flight_phase)
+	{
+		_flight_phase = flight_phase;
+	}
+
+	/**
+	 * Get the control effectiveness matrix if updated
+	 *
+	 * @return true if updated and matrix is set
+	 */
+	virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) = 0;
+
+	/**
+	 * Get the actuator trims
+	 *
+	 * @return Actuator trims
+	 */
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
+	{
+		return _trim;
+	}
+
+	/**
+	 * Get the current flight phase
+	 *
+	 * @return Flight phase
+	 */
+	const FlightPhase &getFlightPhase() const
+	{
+		return _flight_phase;
+	}
+
+	/**
+	 * Get the number of actuators
+	 */
+	virtual int numActuators() const = 0;
+
+protected:
+	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
+};

src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 ActuatorEffectivenessMultirotor.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ActuatorEffectivenessMultirotor.hpp"
+
+ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
+	ModuleParams(nullptr)
+{
+}
+
+bool
+ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
+{
+	// Check if parameters have changed
+	if (_updated || _parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		_updated = false;
+
+		// Get multirotor geometry
+		MultirotorGeometry geometry = {};
+		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
+		geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
+		geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
+		geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
+		geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
+		geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
+		geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
+		geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
+
+		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
+		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
+		geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
+		geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
+		geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
+		geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
+		geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
+		geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
+
+		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
+		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
+		geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
+		geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
+		geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
+		geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
+		geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
+		geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
+
+		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
+		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
+		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
+		geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
+		geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
+		geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
+		geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
+		geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
+
+		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
+		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
+		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
+		geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
+		geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
+		geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
+		geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
+		geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
+
+		geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
+		geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
+		geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
+		geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
+		geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
+		geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
+		geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
+		geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
+
+		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
+		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
+		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
+		geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
+		geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
+		geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
+		geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
+		geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
+
+		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
+		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
+		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
+		geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
+		geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
+		geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
+		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
+		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
+
+		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
+		return true;
+	}
+
+	return false;
+}
+
+int
+ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+		matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
+{
+	int num_actuators = 0;
+
+	effectiveness.setZero();
+
+	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
+		// Get rotor axis
+		matrix::Vector3f axis(
+			geometry.rotors[i].axis_x,
+			geometry.rotors[i].axis_y,
+			geometry.rotors[i].axis_z
+		);
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		matrix::Vector3f position(
+			geometry.rotors[i].position_x,
+			geometry.rotors[i].position_y,
+			geometry.rotors[i].position_z
+		);
+
+		// Get coefficients
+		float ct = geometry.rotors[i].thrust_coef;
+		float km = geometry.rotors[i].moment_ratio;
+
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i) = moment(j);
+			effectiveness(j + 3, i) = thrust(j);
+		}
+
+		num_actuators = i + 1;
+	}
+
+	return num_actuators;
+}