accel & gyro calibration helpers

src/lib/CMakeLists.txt

@@ -59,6 +59,7 @@ add_subdirectory(output_limit)
 add_subdirectory(perf)
 add_subdirectory(pid)
 add_subdirectory(rc)
+add_subdirectory(sensor_calibration)
 add_subdirectory(systemlib)
 add_subdirectory(tecs)
 add_subdirectory(terrain_estimation)

src/lib/sensor_calibration/Accelerometer.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 "Accelerometer.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+namespace calibration
+{
+
+Accelerometer::Accelerometer()
+{
+	Reset();
+}
+
+Accelerometer::Accelerometer(uint32_t device_id)
+{
+	Reset();
+	set_device_id(device_id);
+}
+
+void Accelerometer::set_device_id(uint32_t device_id)
+{
+	if (_device_id != device_id) {
+		_device_id = device_id;
+		ParametersUpdate();
+		SensorCorrectionsUpdate(true);
+	}
+}
+
+void Accelerometer::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		// valid device id required
+		if (_device_id == 0) {
+			return;
+		}
+
+		sensor_correction_s corrections;
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.accel_device_ids[i] == _device_id) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.accel_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.accel_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.accel_offset_2};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
+	}
+}
+
+void Accelerometer::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		Reset();
+		return;
+	}
+
+	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+
+	if (_calibration_index >= 0) {
+
+		if (!_external) {
+			_rotation = GetBoardRotation();
+
+		} else {
+			// TODO: per sensor external rotation
+			_rotation.setIdentity();
+		}
+
+		// CAL_ACCx_EN
+		int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
+		_enabled = (enabled == 1);
+
+		// CAL_ACCx_OFF{X,Y,Z}
+		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+
+		// CAL_ACCx_SCALE{X,Y,Z}
+		_scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
+
+	} else {
+		Reset();
+	}
+}
+
+void Accelerometer::Reset()
+{
+	_rotation.setIdentity();
+	_offset.zero();
+	_scale = Vector3f{1.f, 1.f, 1.f};
+	_thermal_offset.zero();
+
+	_enabled = true;
+
+	_calibration_index = -1;
+}
+
+bool Accelerometer::ParametersSave()
+{
+	if (_calibration_index >= 0) {
+		// save calibration
+		SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
+		SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
+		SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
+		SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
+
+		return true;
+	}
+
+	return false;
+}
+
+void Accelerometer::PrintStatus()
+{
+	PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), device_id(), enabled(),
+		 (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2));
+
+	if (_thermal_offset.norm() > 0.f) {
+		PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id,
+			 (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
+	}
+}
+
+} // namespace calibration

src/lib/sensor_calibration/Accelerometer.hpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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 <lib/conversion/rotation.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_correction.h>
+
+namespace calibration
+{
+class Accelerometer
+{
+public:
+	static constexpr int MAX_SENSOR_COUNT = 3;
+
+	static constexpr const char *SensorString() { return "ACC"; }
+
+	Accelerometer();
+	explicit Accelerometer(uint32_t device_id);
+
+	~Accelerometer() = default;
+
+	void PrintStatus();
+
+	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
+	void set_device_id(uint32_t device_id);
+	void set_external(bool external = true) { _external = external; }
+	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
+	void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
+
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return _enabled; }
+	bool external() const { return _external; }
+	const matrix::Dcmf &rotation() const { return _rotation; }
+
+	// apply offsets and scale
+	// rotate corrected measurements from sensor to body frame
+	inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
+	{
+		return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
+	}
+
+	bool ParametersSave();
+	void ParametersUpdate();
+
+	void Reset();
+
+	void SensorCorrectionsUpdate(bool force = false);
+
+private:
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
+
+	matrix::Dcmf _rotation;
+	matrix::Vector3f _offset;
+	matrix::Vector3f _scale;
+	matrix::Vector3f _thermal_offset;
+
+	int8_t _calibration_index{-1};
+	uint32_t _device_id{0};
+
+	bool _enabled{true};
+	bool _external{false};
+};
+} // namespace calibration

src/lib/sensor_calibration/CMakeLists.txt

@@ -32,8 +32,10 @@
 ############################################################################
 
 px4_add_library(sensor_calibration
-	SensorCalibration.cpp
-	SensorCalibration.hpp
+	Accelerometer.cpp
+	Accelerometer.hpp
+	Gyroscope.cpp
+	Gyroscope.hpp
+	Utilities.cpp
+	Utilities.hpp
 )
-target_compile_options(sensor_calibration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_include_directories(sensor_calibration PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..)

src/lib/sensor_calibration/Gyroscope.cpp

@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   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 "Gyroscope.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+namespace calibration
+{
+
+Gyroscope::Gyroscope()
+{
+	Reset();
+}
+
+Gyroscope::Gyroscope(uint32_t device_id)
+{
+	Reset();
+	set_device_id(device_id);
+}
+
+void Gyroscope::set_device_id(uint32_t device_id)
+{
+	if (_device_id != device_id) {
+		_device_id = device_id;
+		ParametersUpdate();
+		SensorCorrectionsUpdate(true);
+	}
+}
+
+void Gyroscope::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		// valid device id required
+		if (_device_id == 0) {
+			return;
+		}
+
+		sensor_correction_s corrections;
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.gyro_device_ids[i] == _device_id) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.gyro_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.gyro_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.gyro_offset_2};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
+	}
+}
+
+void Gyroscope::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		Reset();
+		return;
+	}
+
+	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+
+	if (_calibration_index >= 0) {
+
+		if (!_external) {
+			_rotation = GetBoardRotation();
+
+		} else {
+			// TODO: per sensor external rotation
+			_rotation.setIdentity();
+		}
+
+		// CAL_GYROx_EN
+		int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
+		_enabled = (enabled == 1);
+
+		// CAL_GYROx_OFF{X,Y,Z}
+		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+
+	} else {
+		Reset();
+	}
+}
+
+void Gyroscope::Reset()
+{
+	_rotation.setIdentity();
+	_offset.zero();
+	_thermal_offset.zero();
+
+	_enabled = true;
+
+	_calibration_index = -1;
+}
+
+bool Gyroscope::ParametersSave()
+{
+	if (_calibration_index >= 0) {
+		// save calibration
+		SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
+		SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
+		SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
+
+		return true;
+	}
+
+	return false;
+}
+
+void Gyroscope::PrintStatus()
+{
+	PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), device_id(), enabled(),
+		 (double)_offset(0), (double)_offset(1), (double)_offset(2));
+
+	if (_thermal_offset.norm() > 0.f) {
+		PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id,
+			 (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
+	}
+}
+
+} // namespace calibration

src/lib/sensor_calibration/Gyroscope.hpp

@@ -40,67 +40,57 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_correction.h>
 
-namespace sensors
+namespace calibration
 {
-
-class SensorCalibration
+class Gyroscope
 {
 public:
+	static constexpr int MAX_SENSOR_COUNT = 3;
 
-	enum class SensorType : uint8_t {
-		Accelerometer,
-		Gyroscope,
-	};
+	static constexpr const char *SensorString() { return "GYRO"; }
 
-	SensorCalibration() = delete;
-	explicit SensorCalibration(SensorType type) : _type(type) {}
-	~SensorCalibration() = default;
+	Gyroscope();
+	explicit Gyroscope(uint32_t device_id);
+
+	~Gyroscope() = default;
 
 	void PrintStatus();
 
+	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
 	void set_device_id(uint32_t device_id);
 	void set_external(bool external = true) { _external = external; }
+	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return _enabled; }
 	bool external() const { return _external; }
+	const matrix::Dcmf &rotation() const { return _rotation; }
 
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
-	matrix::Vector3f Correct(const matrix::Vector3f &data);
+	inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
+	{
+		return _rotation * matrix::Vector3f{data - _thermal_offset - _offset};
+	}
 
+	bool ParametersSave();
 	void ParametersUpdate();
+
+	void Reset();
+
 	void SensorCorrectionsUpdate(bool force = false);
 
-	const matrix::Dcmf &getBoardRotation() const { return _rotation; }
-
 private:
-
-	static constexpr int MAX_SENSOR_COUNT = 3;
-
-	int FindCalibrationIndex(uint32_t device_id) const;
-
-	bool CalibrationEnabled(uint8_t calibration_index) const;
-	matrix::Vector3f CalibrationOffset(uint8_t calibration_index) const;
-	matrix::Vector3f CalibrationScale(uint8_t calibration_index) const;
-
-	const char *SensorString() const;
-
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	matrix::Dcmf _rotation;
+	matrix::Vector3f _offset;
+	matrix::Vector3f _thermal_offset;
 
-	matrix::Vector3f _offset{0.f, 0.f, 0.f};
-	matrix::Vector3f _scale{1.f, 1.f, 1.f};
-
-	matrix::Vector3f _thermal_offset{0.f, 0.f, 0.f};
-
+	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
 
-	const SensorType _type;
-
 	bool _enabled{true};
 	bool _external{false};
 };
-
-} // namespace sensors
+} // namespace calibration

src/lib/sensor_calibration/Utilities.cpp

@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <lib/conversion/rotation.h>
+#include <lib/parameters/param.h>
+
+using math::radians;
+using matrix::Eulerf;
+using matrix::Dcmf;
+using matrix::Vector3f;
+
+namespace calibration
+{
+
+int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id)
+{
+	if (device_id == 0) {
+		return -1;
+	}
+
+	for (unsigned i = 0; i < 4; ++i) {
+		char str[20] {};
+		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
+
+		int32_t device_id_val = 0;
+
+		param_t param_handle = param_find_no_notification(str);
+
+		if (param_handle == PARAM_INVALID) {
+			continue;
+		}
+
+		// find again and get value, but this time mark it active
+		if (param_get(param_find(str), &device_id_val) != OK) {
+			continue;
+		}
+
+		if ((uint32_t)device_id_val == device_id) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance)
+{
+	// eg CAL_MAGn_ID/CAL_MAGn_ROT
+	char str[20] {};
+	sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type);
+
+	int32_t value = 0;
+
+	if (param_get(param_find(str), &value) != 0) {
+		PX4_ERR("failed to get %s", str);
+	}
+
+	return value;
+}
+
+int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value)
+{
+	char str[20] {};
+
+	// eg CAL_MAGn_ID/CAL_MAGn_ROT
+	sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type);
+
+	int ret = param_set_no_notification(param_find(str), &value);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("failed to set %s = %d", str, value);
+	}
+
+	return ret;
+}
+
+Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
+{
+	Vector3f values{0.f, 0.f, 0.f};
+
+	char str[20] {};
+
+	for (int axis = 0; axis < 3; axis++) {
+		char axis_char = 'X' + axis;
+
+		// eg CAL_MAGn_{X,Y,Z}OFF
+		sprintf(str, "CAL_%s%u_%c%s", sensor_type, instance, axis_char, cal_type);
+
+		if (param_get(param_find(str), &values(axis)) != 0) {
+			PX4_ERR("failed to get %s", str);
+		}
+	}
+
+	return values;
+}
+
+int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values)
+{
+	int ret = PX4_OK;
+	char str[20] {};
+
+	for (int axis = 0; axis < 3; axis++) {
+		char axis_char = 'X' + axis;
+
+		// eg CAL_MAGn_{X,Y,Z}OFF
+		sprintf(str, "CAL_%s%u_%c%s", sensor_type, instance, axis_char, cal_type);
+
+		if (param_set_no_notification(param_find(str), &values(axis)) != 0) {
+			PX4_ERR("failed to set %s = %.4f", str, (double)values(axis));
+			ret = PX4_ERROR;
+		}
+	}
+
+	return ret;
+}
+
+Dcmf GetBoardRotation()
+{
+	float x_offset = 0.f;
+	float y_offset = 0.f;
+	float z_offset = 0.f;
+	param_get(param_find("SENS_BOARD_X_OFF"), &x_offset);
+	param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset);
+	param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset);
+
+	const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset)));
+
+	// get transformation matrix from sensor/board to body frame
+	int32_t board_rot = 0;
+	param_get(param_find("SENS_BOARD_ROT"), &board_rot);
+
+	return board_rotation_offset * get_rot_matrix((enum Rotation)board_rot);
+}
+
+} // namespace calibration

src/lib/sensor_calibration/Utilities.hpp

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   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 <matrix/math.hpp>
+
+namespace calibration
+{
+
+int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id);
+
+int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance);
+int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value);
+
+matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance);
+int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance,
+				 matrix::Vector3f values);
+
+matrix::Dcmf GetBoardRotation();
+
+
+} // namespace calibration

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

@@ -50,4 +50,4 @@ px4_add_library(PreFlightCheck
 	checks/cpuResourceCheck.cpp
 )
 target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags)
+target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)

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

@@ -301,38 +301,3 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	/* Report status */
 	return !failed;
 }
-
-bool PreFlightCheck::check_calibration(const char *param_template, const int32_t device_id)
-{
-	bool calibration_found = false;
-
-	char s[20];
-	int instance = 0;
-
-	/* old style transition: check param values */
-	while (!calibration_found) {
-		sprintf(s, param_template, instance);
-		const param_t parm = param_find_no_notification(s);
-
-		/* if the calibration param is not present, abort */
-		if (parm == PARAM_INVALID) {
-			break;
-		}
-
-		/* if param get succeeds */
-		int32_t calibration_devid = -1;
-
-		if (param_get(parm, &calibration_devid) == PX4_OK) {
-
-			/* if the devid matches, exit early */
-			if (device_id == calibration_devid) {
-				calibration_found = true;
-				break;
-			}
-		}
-
-		instance++;
-	}
-
-	return calibration_found;
-}

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

@@ -112,7 +112,7 @@ private:
 			      const bool report_fail, const bool enforce_gps_required);
 	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
 					 const bool prearm);
-	static bool check_calibration(const char *param_template, const int32_t device_id);
+
 	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);

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

@@ -37,7 +37,8 @@
 #include <HealthFlags.h>
 #include <math.h>
 #include <px4_defines.h>
-#include <systemlib/mavlink_log.h>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/subsystem_info.h>
@@ -65,7 +66,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s
 
 		device_id = accel.get().device_id;
 
-		calibration_valid = check_calibration("CAL_ACC%u_ID", device_id);
+		calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
 
 		if (!calibration_valid) {
 			if (report_fail) {

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

@@ -36,7 +36,8 @@
 #include <drivers/drv_hrt.h>
 #include <HealthFlags.h>
 #include <px4_defines.h>
-#include <systemlib/mavlink_log.h>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/subsystem_info.h>
@@ -64,7 +65,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 
 		device_id = gyro.get().device_id;
 
-		calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id);
+		calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
 
 		if (!calibration_valid) {
 			if (report_fail) {

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

@@ -36,7 +36,8 @@
 #include <drivers/drv_hrt.h>
 #include <HealthFlags.h>
 #include <px4_defines.h>
-#include <systemlib/mavlink_log.h>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/subsystem_info.h>
@@ -64,7 +65,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
 
 		device_id = magnetometer.get().device_id;
 
-		calibration_valid = check_calibration("CAL_MAG%u_ID", device_id);
+		calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
 
 		if (!calibration_valid) {
 			if (report_fail) {

src/modules/commander/CMakeLists.txt

@@ -46,6 +46,7 @@ px4_add_module(
 		commander_helper.cpp
 		esc_calibration.cpp
 		gyro_calibration.cpp
+		level_calibration.cpp
 		mag_calibration.cpp
 		rc_calibration.cpp
 		state_machine_helper.cpp
@@ -58,6 +59,7 @@ px4_add_module(
 		PreFlightCheck
 		ArmAuthorization
 		HealthFlags
+		sensor_calibration
 	)
 
 if(PX4_TESTING)

src/modules/commander/Commander.cpp

@@ -54,6 +54,7 @@
 #include "commander_helper.h"
 #include "esc_calibration.h"
 #include "gyro_calibration.h"
+#include "level_calibration.h"
 #include "mag_calibration.h"
 #include "px4_custom_mode.h"
 #include "rc_calibration.h"

src/modules/commander/accelerometer_calibration.h

@@ -46,6 +46,5 @@
 #include <uORB/uORB.h>
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub);
-int do_level_calibration(orb_advert_t *mavlink_log_pub);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */

src/modules/commander/airspeed_calibration.cpp

@@ -65,6 +65,8 @@ static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
 
 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;
@@ -99,8 +101,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 		px4_close(fd);
 	}
 
-	int cancel_sub = calibrate_cancel_subscribe();
-
 	if (!paramreset_successful) {
 
 		/* only warn if analog scaling is zero */
@@ -124,7 +124,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 	while (calibration_counter < calibration_count) {
 
-		if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
+		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
 			goto error_return;
 		}
 
@@ -206,7 +206,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	/* 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, cancel_sub)) {
+		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
 			goto error_return;
 		}
 
@@ -279,7 +279,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	px4_usleep(2e6);
 
 normal_return:
-	calibrate_cancel_unsubscribe(cancel_sub);
 	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

src/modules/commander/calibration_routines.cpp

@@ -50,9 +50,11 @@
 #include <lib/systemlib/mavlink_log.h>
 #include <matrix/math.hpp>
 
+#include <uORB/Publication.hpp>
 #include <uORB/SubscriptionBlocking.hpp>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 #include "calibration_routines.h"
 #include "calibration_messages.h"
@@ -616,49 +618,49 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 		if (poll_errcount > 1000) {
 			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
-			return DETECT_ORIENTATION_ERROR;
+			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 DETECT_ORIENTATION_TAIL_DOWN;        // [ g, 0, 0 ]
+		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 DETECT_ORIENTATION_NOSE_DOWN;        // [ -g, 0, 0 ]
+		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 DETECT_ORIENTATION_LEFT;        // [ 0, g, 0 ]
+		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 DETECT_ORIENTATION_RIGHT;        // [ 0, -g, 0 ]
+		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 DETECT_ORIENTATION_UPSIDE_DOWN;        // [ 0, 0, g ]
+		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 DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
+		return ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
 	}
 
 	calibration_log_critical(mavlink_log_pub, "ERROR: invalid orientation");
 
-	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
+	return ORIENTATION_ERROR;	// Can't detect orientation
 }
 
 const char *detect_orientation_str(enum detect_orientation_return orientation)
@@ -677,19 +679,17 @@ const char *detect_orientation_str(enum detect_orientation_return orientation)
 }
 
 calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
-		int		cancel_sub,
-		bool	side_data_collected[detect_orientation_side_count],
-		calibration_from_orientation_worker_t calibration_worker,
-		void	*worker_data,
-		bool	lenient_still_position)
+		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, cancel_sub)) {
+		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
 			result = calibrate_return_cancelled;
 			break;
 		}
@@ -731,7 +731,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 		px4_usleep(20000);
 		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
 
-		if (orient == DETECT_ORIENTATION_ERROR) {
+		if (orient == ORIENTATION_ERROR) {
 			orientation_failures++;
 			calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
 			px4_usleep(20000);
@@ -754,7 +754,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 		orientation_failures = 0;
 
 		// Call worker routine
-		result = calibration_worker(orient, cancel_sub, worker_data);
+		result = calibration_worker(orient, worker_data);
 
 		if (result != calibrate_return_ok) {
 			break;
@@ -779,74 +779,51 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 	return result;
 }
 
-int calibrate_cancel_subscribe()
+bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started)
 {
-	int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	bool ret = false;
 
-	if (vehicle_command_sub >= 0) {
-		// make sure we won't read any old messages
-		struct vehicle_command_s cmd;
-		bool update;
+	uORB::Subscription vehicle_command_sub{ORB_ID(vehicle_command)};
+	vehicle_command_s cmd;
 
-		while (orb_check(vehicle_command_sub, &update) == 0 && update) {
-			orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &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) {
 
-	return vehicle_command_sub;
-}
+				vehicle_command_ack_s command_ack{};
 
-void calibrate_cancel_unsubscribe(int cmd_sub)
-{
-	orb_unsubscribe(cmd_sub);
-}
+				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) {
 
-static void calibrate_answer_command(orb_advert_t *mavlink_log_pub, struct vehicle_command_s &cmd, unsigned result)
-{
-	switch (result) {
-	case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
-		tune_positive(true);
-		break;
+					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;
 
-	case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command);
-		tune_negative(true);
-		break;
+				} else {
+					command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+					mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command);
+					tune_negative(true);
+					ret = false;
+				}
 
-	default:
-		break;
-	}
-}
+				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();
 
-bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
-{
-	px4_pollfd_struct_t fds[1];
-	fds[0].fd = cancel_sub;
-	fds[0].events = POLLIN;
+				uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+				command_ack_pub.publish(command_ack);
 
-	if (px4_poll(&fds[0], 1, 0) > 0) {
-		struct vehicle_command_s cmd;
-
-		orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
-
-		// ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount
-		if (cmd.from_external) {
-			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
-			    (int)cmd.param1 == 0 &&
-			    (int)cmd.param2 == 0 &&
-			    (int)cmd.param3 == 0 &&
-			    (int)cmd.param4 == 0 &&
-			    (int)cmd.param5 == 0 &&
-			    (int)cmd.param6 == 0) {
-				calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
-				mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
-				return true;
-
-			} else {
-				calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+				return ret;
 			}
 		}
 	}
 
-	return false;
+	return ret;
 }

src/modules/commander/calibration_routines.h

@@ -36,6 +36,8 @@
 
 #pragma once
 
+#include <drivers/drv_hrt.h>
+
 /**
  * Least-squares fit of a sphere to a set of points.
  *
@@ -74,22 +76,21 @@ bool mat_inverse(float *A, float *inv, uint8_t n);
 
 // The order of these cannot change since the calibration calculations depend on them in this order
 enum detect_orientation_return {
-	DETECT_ORIENTATION_TAIL_DOWN,
-	DETECT_ORIENTATION_NOSE_DOWN,
-	DETECT_ORIENTATION_LEFT,
-	DETECT_ORIENTATION_RIGHT,
-	DETECT_ORIENTATION_UPSIDE_DOWN,
-	DETECT_ORIENTATION_RIGHTSIDE_UP,
-	DETECT_ORIENTATION_ERROR
+	ORIENTATION_TAIL_DOWN,
+	ORIENTATION_NOSE_DOWN,
+	ORIENTATION_LEFT,
+	ORIENTATION_RIGHT,
+	ORIENTATION_UPSIDE_DOWN,
+	ORIENTATION_RIGHTSIDE_UP,
+	ORIENTATION_ERROR
 };
-static const unsigned detect_orientation_side_count = 6;
+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
-		int	cancel_sub,			///< Cancel subscription from calibration_cancel_subscribe
-		bool	lenient_still_detection);	///< true: Use more lenient still position detection
+		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
@@ -103,29 +104,18 @@ enum calibrate_return {
 
 typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return
 		orientation,	///< Orientation which was detected
-		int				cancel_sub,	///< Cancel subscription from calibration_cancel_subscribe
-		void				*worker_data);	///< Opaque worker data
+		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
-		int		cancel_sub,						///< Cancel subscription from calibration_cancel_subscribe
-		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
-
-/// Called at the beginning of calibration in order to subscribe to the cancel command
-///	@return Handle to vehicle_command subscription
-int calibrate_cancel_subscribe(void);
-
-/// Called to cancel the subscription to the cancel command
-///	@param cancel_sub Cancel subcription from calibration_cancel_subscribe
-void calibrate_cancel_unsubscribe(int cancel_sub);
+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,	///< uORB handle to write output to
-			    int cancel_sub);	///< Cancel subcription fromcalibration_cancel_subscribe
+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

src/modules/commander/gyro_calibration.cpp

@@ -48,6 +48,8 @@
 #include <px4_platform_common/time.h>
 
 #include <drivers/drv_hrt.h>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/systemlib/mavlink_log.h>
@@ -65,7 +67,9 @@ using matrix::Vector3f;
 /// Data passed to calibration worker routine
 struct gyro_worker_data_t {
 	orb_advert_t *mavlink_log_pub{nullptr};
-	int32_t device_id[MAX_GYROS] {};
+
+	calibration::Gyroscope calibrations[MAX_GYROS] {};
+
 	Vector3f offset[MAX_GYROS] {};
 
 	static constexpr int last_num_samples = 9; ///< number of samples for the motion detection median filter
@@ -84,14 +88,15 @@ static int float_cmp(const void *elem1, const void *elem2)
 	return *(const float *)elem1 > *(const float *)elem2;
 }
 
-static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data_t &worker_data)
+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::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
-	sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
+	sensor_correction_s sensor_correction{};
 
 	uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
 		{ORB_ID(sensor_gyro), 0, 0},
@@ -108,7 +113,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data
 	unsigned slow_count = 0;
 
 	while (slow_count < CALIBRATION_COUNT) {
-		if (calibrate_cancel_check(worker_data.mavlink_log_pub, cancel_sub)) {
+		if (calibrate_cancel_check(worker_data.mavlink_log_pub, calibration_started)) {
 			return calibrate_return_cancelled;
 		}
 
@@ -116,52 +121,55 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data
 			unsigned update_count = CALIBRATION_COUNT;
 
 			for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) {
-				if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
-					// Skip if instance has enough samples
-					continue;
-				}
+				if (worker_data.calibrations[gyro_index].device_id() != 0) {
 
-				sensor_gyro_s gyro_report;
+					if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
+						// Skip if instance has enough samples
+						continue;
+					}
 
-				if (gyro_sub[gyro_index].update(&gyro_report)) {
+					sensor_gyro_s gyro_report;
 
-					// fetch optional thermal offset corrections in sensor/board frame
-					Vector3f offset{0, 0, 0};
-					sensor_correction_sub.update(&sensor_correction);
+					if (gyro_sub[gyro_index].update(&gyro_report)) {
 
-					if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) {
-						for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) {
-							if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) {
-								switch (correction_index) {
-								case 0:
-									offset = Vector3f{sensor_correction.gyro_offset_0};
-									break;
-								case 1:
-									offset = Vector3f{sensor_correction.gyro_offset_1};
-									break;
-								case 2:
-									offset = Vector3f{sensor_correction.gyro_offset_2};
-									break;
+						// 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 && gyro_report.device_id != 0) {
+							for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) {
+								if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) {
+									switch (correction_index) {
+									case 0:
+										offset = Vector3f{sensor_correction.gyro_offset_0};
+										break;
+									case 1:
+										offset = Vector3f{sensor_correction.gyro_offset_1};
+										break;
+									case 2:
+										offset = Vector3f{sensor_correction.gyro_offset_2};
+										break;
+									}
 								}
 							}
 						}
+
+						worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset;
+						calibration_counter[gyro_index]++;
+
+						if (gyro_index == 0) {
+							worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0);
+							worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1);
+							worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2);
+							worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
+						}
 					}
 
-					worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset;
-					calibration_counter[gyro_index]++;
-
-					if (gyro_index == 0) {
-						worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0);
-						worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1);
-						worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2);
-						worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
+					// 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];
 					}
 				}
-
-				// 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) {
@@ -184,7 +192,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data
 	}
 
 	for (unsigned s = 0; s < MAX_GYROS; s++) {
-		if ((worker_data.device_id[s] != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) {
+		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;
 		}
@@ -204,7 +212,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	gyro_worker_data_t worker_data{};
 	worker_data.mavlink_log_pub = mavlink_log_pub;
 
-	enum ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED;
+	ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED;
 	int32_t device_id_primary = 0;
 
 	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
@@ -213,31 +221,30 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	// 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 < orb_gyro_count && cur_gyro < MAX_GYROS; cur_gyro++) {
+	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};
 
-		uORB::Subscription gyro_sensor_sub{ORB_ID(sensor_gyro), cur_gyro};
-		sensor_gyro_s report{};
-		gyro_sensor_sub.copy(&report);
+		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);
 
-		worker_data.device_id[cur_gyro] = report.device_id;
-
-		if (worker_data.device_id[cur_gyro] != 0) {
 			// Get priority
-			enum ORB_PRIO prio = gyro_sensor_sub.get_priority();
+			const ORB_PRIO prio = gyro_sub.get_priority();
 
 			if (prio > device_prio_max) {
 				device_prio_max = prio;
-				device_id_primary = worker_data.device_id[cur_gyro];
+				device_id_primary = gyro_sub.get().device_id;
 			}
-
-		} else {
-			calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro);
 		}
-	}
 
-	int cancel_sub = calibrate_cancel_subscribe();
+		// 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;
@@ -245,7 +252,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 	do {
 		// Calibrate gyro and ensure user didn't move
-		calibrate_return cal_return = gyro_calibration_worker(cancel_sub, worker_data);
+		calibrate_return cal_return = gyro_calibration_worker(worker_data);
 
 		if (cal_return == calibrate_return_cancelled) {
 			// Cancel message already sent, we are done here
@@ -266,7 +273,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 			float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](2);
 
 			/* maximum allowable calibration error */
-			const float maxoff = math::radians(0.6f);
+			static constexpr float maxoff = math::radians(0.6f);
 
 			if (!PX4_ISFINITE(worker_data.offset[0](0)) ||
 			    !PX4_ISFINITE(worker_data.offset[0](1)) ||
@@ -290,49 +297,28 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		res = PX4_ERROR;
 	}
 
-	calibrate_cancel_unsubscribe(cancel_sub);
-
 	if (res == PX4_OK) {
 
 		/* set offset parameters to new values */
-		bool failed = (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary));
+		bool failed = false;
 
 		for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
 
-			char str[30] {};
+			auto &calibration = worker_data.calibrations[uorb_index];
 
-			if (uorb_index < orb_gyro_count) {
-				float x_offset = worker_data.offset[uorb_index](0);
-				sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &x_offset));
-
-				float y_offset = worker_data.offset[uorb_index](1);
-				sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &y_offset));
-
-				float z_offset = worker_data.offset[uorb_index](2);
-				sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &z_offset));
-
-				int32_t device_id = worker_data.device_id[uorb_index];
-				sprintf(str, "CAL_GYRO%u_ID", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &device_id));
+			if (calibration.device_id() != 0) {
+				calibration.set_offset(worker_data.offset[uorb_index]);
 
 			} else {
-				// reset unused calibration offsets
-				sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
-				param_reset(param_find(str));
-				sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
-				param_reset(param_find(str));
-				sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
-				param_reset(param_find(str));
-
-				// reset unused calibration device ID
-				sprintf(str, "CAL_GYRO%u_ID", uorb_index);
-				param_reset(param_find(str));
+				calibration.Reset();
 			}
+
+			calibration.set_calibration_index(uorb_index);
+			calibration.ParametersSave();
 		}
 
+		param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary);
+
 		if (failed) {
 			calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params");
 			res = PX4_ERROR;

src/modules/commander/level_calibration.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   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;
+		const 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);
+		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");
+		return 0;
+
+	} else {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
+		return 1;
+	}
+}

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/modules/commander/mag_calibration.cpp

@@ -342,8 +342,9 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
 	return calibrate_return_ok;
 }
 
-static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
+static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data)
 {
+	const hrt_abstime calibration_started = hrt_absolute_time();
 	calibrate_return result = calibrate_return_ok;
 
 	unsigned int calibration_counter_side;
@@ -380,7 +381,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	       fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
 
 		/* abort on request */
-		if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
+		if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) {
 			result = calibrate_return_cancelled;
 			px4_close(sub_gyro);
 			return result;
@@ -430,7 +431,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	while (hrt_absolute_time() < calibration_deadline &&
 	       calibration_counter_side < worker_data->calibration_points_perside) {
 
-		if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
+		if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) {
 			result = calibrate_return_cancelled;
 			break;
 		}
@@ -669,15 +670,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 	}
 
 	if (result == calibrate_return_ok) {
-		int cancel_sub  = calibrate_cancel_subscribe();
-
 		result = calibrate_from_orientation(mavlink_log_pub,                    // uORB handle to write output
-						    cancel_sub,                         // Subscription to vehicle_command for cancel support
 						    worker_data.side_data_collected,    // Sides to calibrate
 						    mag_calibration_worker,             // Calibration worker
 						    &worker_data,			// Opaque data for calibration worked
 						    true);				// true: lenient still detection
-		calibrate_cancel_unsubscribe(cancel_sub);
 	}
 
 	// Close subscriptions

src/modules/sensors/CMakeLists.txt

@@ -33,7 +33,6 @@
 
 add_subdirectory(data_validator)
 
-add_subdirectory(sensor_calibration) # used by vehicle_{acceleration, angular_velocity, imu}
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
@@ -54,6 +53,7 @@ px4_add_module(
 		drivers__device
 		git_ecl
 		mathlib
+		sensor_calibration
 		vehicle_acceleration
 		vehicle_angular_velocity
 		vehicle_air_data

src/modules/sensors/sensor_calibration/SensorCalibration.cpp

@@ -1,252 +0,0 @@
-/****************************************************************************
- *
- *   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 "SensorCalibration.hpp"
-
-#include <lib/parameters/param.h>
-
-using namespace matrix;
-using namespace time_literals;
-using math::radians;
-
-namespace sensors
-{
-
-void SensorCalibration::set_device_id(uint32_t device_id)
-{
-	if (_device_id != device_id) {
-		_device_id = device_id;
-		SensorCorrectionsUpdate(true);
-		ParametersUpdate();
-	}
-}
-
-matrix::Vector3f SensorCalibration::Correct(const matrix::Vector3f &data)
-{
-	SensorCorrectionsUpdate();
-	return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
-}
-
-const char *SensorCalibration::SensorString() const
-{
-	switch (_type) {
-	case SensorType::Accelerometer:
-		return "ACC";
-
-	case SensorType::Gyroscope:
-		return "GYRO";
-	}
-
-	return nullptr;
-}
-
-int SensorCalibration::FindCalibrationIndex(uint32_t device_id) const
-{
-	if (device_id == 0) {
-		return -1;
-	}
-
-	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
-		char str[16] {};
-		sprintf(str, "CAL_%s%u_ID", SensorString(), i);
-
-		int32_t device_id_val = 0;
-
-		if (param_get(param_find(str), &device_id_val) != OK) {
-			PX4_ERR("Could not access param %s", str);
-			continue;
-		}
-
-		if ((uint32_t)device_id_val == device_id) {
-			return i;
-		}
-	}
-
-	return -1;
-}
-
-bool SensorCalibration::CalibrationEnabled(uint8_t calibration_index) const
-{
-	char str[20] {};
-	sprintf(str, "CAL_%s%u_EN", SensorString(), calibration_index);
-	int32_t enabled_val = 1;
-	param_get(param_find(str), &enabled_val);
-
-	return (enabled_val != 0);
-}
-
-Vector3f SensorCalibration::CalibrationOffset(uint8_t calibration_index) const
-{
-	// offsets (x, y, z)
-	Vector3f offset{0.f, 0.f, 0.f};
-	char str[20] {};
-
-	sprintf(str, "CAL_%s%u_XOFF", SensorString(), calibration_index);
-	param_get(param_find(str), &offset(0));
-
-	sprintf(str, "CAL_%s%u_YOFF", SensorString(), calibration_index);
-	param_get(param_find(str), &offset(1));
-
-	sprintf(str, "CAL_%s%u_ZOFF", SensorString(), calibration_index);
-	param_get(param_find(str), &offset(2));
-
-	return offset;
-}
-
-Vector3f SensorCalibration::CalibrationScale(uint8_t calibration_index) const
-{
-	// scale factors (x, y, z)
-	Vector3f scale{1.f, 1.f, 1.f};
-
-	// gyroscope doesn't have a scale factor calibration
-	if (_type != SensorType::Gyroscope) {
-		char str[20] {};
-
-		sprintf(str, "CAL_%s%u_XSCALE", SensorString(), calibration_index);
-		param_get(param_find(str), &scale(0));
-
-		sprintf(str, "CAL_%s%u_YSCALE", SensorString(), calibration_index);
-		param_get(param_find(str), &scale(1));
-
-		sprintf(str, "CAL_%s%u_ZSCALE", SensorString(), calibration_index);
-		param_get(param_find(str), &scale(2));
-	}
-
-	return scale;
-}
-
-void SensorCalibration::SensorCorrectionsUpdate(bool force)
-{
-	// check if the selected sensor has updated
-	if (_sensor_correction_sub.updated() || force) {
-
-		// valid device id required
-		if (_device_id == 0) {
-			return;
-		}
-
-		sensor_correction_s corrections;
-
-		if (_sensor_correction_sub.copy(&corrections)) {
-			// find sensor_corrections index
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if ((_type == SensorType::Accelerometer) && (corrections.accel_device_ids[i] == _device_id)) {
-					switch (i) {
-					case 0:
-						_thermal_offset = Vector3f{corrections.accel_offset_0};
-						return;
-					case 1:
-						_thermal_offset = Vector3f{corrections.accel_offset_1};
-						return;
-					case 2:
-						_thermal_offset = Vector3f{corrections.accel_offset_2};
-						return;
-					}
-
-				} else if ((_type == SensorType::Gyroscope) && (corrections.gyro_device_ids[i] == _device_id)) {
-					switch (i) {
-					case 0:
-						_thermal_offset = Vector3f{corrections.gyro_offset_0};
-						return;
-					case 1:
-						_thermal_offset = Vector3f{corrections.gyro_offset_1};
-						return;
-					case 2:
-						_thermal_offset = Vector3f{corrections.gyro_offset_2};
-						return;
-					}
-				}
-			}
-		}
-
-		// zero thermal offset if not found
-		_thermal_offset.zero();
-	}
-}
-
-void SensorCalibration::ParametersUpdate()
-{
-	if (!_external) {
-		// fine tune the rotation
-		float x_offset = 0.f;
-		float y_offset = 0.f;
-		float z_offset = 0.f;
-		param_get(param_find("SENS_BOARD_X_OFF"), &x_offset);
-		param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset);
-		param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset);
-
-		const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset)));
-
-		// get transformation matrix from sensor/board to body frame
-		int32_t board_rot = 0;
-		param_get(param_find("SENS_BOARD_ROT"), &board_rot);
-		_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rot);
-
-	} else {
-		// TODO: per sensor external rotation
-		_rotation.setIdentity();
-	}
-
-
-	int calibration_index = FindCalibrationIndex(_device_id);
-
-	if (calibration_index >= 0) {
-		_enabled = CalibrationEnabled(calibration_index);
-		_offset = CalibrationOffset(calibration_index);
-		_scale = CalibrationScale(calibration_index);
-
-	} else {
-		_enabled = true;
-		_offset.zero();
-		_scale = Vector3f{1.f, 1.f, 1.f};
-	}
-}
-
-void SensorCalibration::PrintStatus()
-{
-	if (_type != SensorType::Gyroscope) {
-		PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled,
-			 (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2));
-
-	} else {
-		PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled,
-			 (double)_offset(0), (double)_offset(1), (double)_offset(2));
-	}
-
-	if (_thermal_offset.norm() > 0.f) {
-		PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id,
-			 (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
-	}
-}
-
-} // namespace sensors

src/modules/sensors/sensor_params_mag.c

@@ -47,12 +47,12 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
  * recommended.
  *
  * Bits:
- * DETECT_ORIENTATION_TAIL_DOWN = 1
- * DETECT_ORIENTATION_NOSE_DOWN = 2
- * DETECT_ORIENTATION_LEFT = 4
- * DETECT_ORIENTATION_RIGHT = 8
- * DETECT_ORIENTATION_UPSIDE_DOWN = 16
- * DETECT_ORIENTATION_RIGHTSIDE_UP = 32
+ * ORIENTATION_TAIL_DOWN = 1
+ * ORIENTATION_NOSE_DOWN = 2
+ * ORIENTATION_LEFT = 4
+ * ORIENTATION_RIGHT = 8
+ * ORIENTATION_UPSIDE_DOWN = 16
+ * ORIENTATION_RIGHTSIDE_UP = 32
  *
  * @min 34
  * @max 63

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -41,6 +41,6 @@ target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_acceleration
 	PRIVATE
 		mathlib
-		sensor_calibration
 		px4_work_queue
+		sensor_calibration
 )

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -33,8 +33,7 @@
 
 #pragma once
 
-#include <sensor_calibration/SensorCalibration.hpp>
-
+#include <lib/sensor_calibration/Accelerometer.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
@@ -88,7 +87,7 @@ private:
 		{this, ORB_ID(sensor_accel), 2}
 	};
 
-	SensorCalibration _calibration{SensorCalibration::SensorType::Accelerometer};
+	calibration::Accelerometer _calibration{};
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -41,6 +41,6 @@ target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_angular_velocity
 	PRIVATE
 		mathlib
-		sensor_calibration
 		px4_work_queue
+		sensor_calibration
 )

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -33,8 +33,7 @@
 
 #pragma once
 
-#include <sensor_calibration/SensorCalibration.hpp>
-
+#include <lib/sensor_calibration/Gyroscope.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
@@ -91,7 +90,7 @@ private:
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
-	SensorCalibration _calibration{SensorCalibration::SensorType::Gyroscope};
+	calibration::Gyroscope _calibration{};
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -38,4 +38,4 @@ px4_add_library(vehicle_imu
 	VehicleIMU.hpp
 )
 target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(vehicle_imu PRIVATE sensor_calibration px4_work_queue)
+target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration)

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -191,6 +191,11 @@ void VehicleIMU::Run()
 
 	ParametersUpdate();
 
+	if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
+		return;
+	}
+
+
 	bool sensor_data_gap = false;
 	bool update_integrator_config = false;
 	bool publish_status = false;
@@ -269,7 +274,7 @@ void VehicleIMU::Run()
 		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
 
 			// rotate sensor clip counts into vehicle body frame
-			const Vector3f clipping{_accel_calibration.getBoardRotation() *
+			const Vector3f clipping{_accel_calibration.rotation() *
 				Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
 
 			// round to get reasonble clip counts per axis (after board rotation)
@@ -332,42 +337,45 @@ void VehicleIMU::Run()
 		if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
 		    && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
 
-			// delta angle: apply offsets, scale, and board rotation
-			_gyro_calibration.SensorCorrectionsUpdate();
-			const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
-			const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
+			if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
 
-			// delta velocity: apply offsets, scale, and board rotation
-			_accel_calibration.SensorCorrectionsUpdate();
-			const float accel_dt_inv = 1.e6f / accel_integral_dt;
-			Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
+				// delta angle: apply offsets, scale, and board rotation
+				_gyro_calibration.SensorCorrectionsUpdate();
+				const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
+				const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
 
-			UpdateAccelVibrationMetrics(delta_velocity_corrected);
-			UpdateGyroVibrationMetrics(delta_angle_corrected);
+				// delta velocity: apply offsets, scale, and board rotation
+				_accel_calibration.SensorCorrectionsUpdate();
+				const float accel_dt_inv = 1.e6f / accel_integral_dt;
+				Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
 
-			// vehicle_imu_status
-			//  publish before vehicle_imu so that error counts are available synchronously if needed
-			if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
-				_status.accel_device_id = _accel_calibration.device_id();
-				_status.gyro_device_id = _gyro_calibration.device_id();
-				_status.timestamp = hrt_absolute_time();
-				_vehicle_imu_status_pub.publish(_status);
+				UpdateAccelVibrationMetrics(delta_velocity_corrected);
+				UpdateGyroVibrationMetrics(delta_angle_corrected);
+
+				// vehicle_imu_status
+				//  publish before vehicle_imu so that error counts are available synchronously if needed
+				if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
+					_status.accel_device_id = _accel_calibration.device_id();
+					_status.gyro_device_id = _gyro_calibration.device_id();
+					_status.timestamp = hrt_absolute_time();
+					_vehicle_imu_status_pub.publish(_status);
+				}
+
+
+				// publish vehicle_imu
+				vehicle_imu_s imu;
+				imu.timestamp_sample = _last_timestamp_sample_gyro;
+				imu.accel_device_id = _accel_calibration.device_id();
+				imu.gyro_device_id = _gyro_calibration.device_id();
+				delta_angle_corrected.copyTo(imu.delta_angle);
+				delta_velocity_corrected.copyTo(imu.delta_velocity);
+				imu.delta_angle_dt = gyro_integral_dt;
+				imu.delta_velocity_dt = accel_integral_dt;
+				imu.delta_velocity_clipping = _delta_velocity_clipping;
+				imu.timestamp = hrt_absolute_time();
+				_vehicle_imu_pub.publish(imu);
 			}
 
-
-			// publish vehicle_imu
-			vehicle_imu_s imu;
-			imu.timestamp_sample = _last_timestamp_sample_gyro;
-			imu.accel_device_id = _accel_calibration.device_id();
-			imu.gyro_device_id = _gyro_calibration.device_id();
-			delta_angle_corrected.copyTo(imu.delta_angle);
-			delta_velocity_corrected.copyTo(imu.delta_velocity);
-			imu.delta_angle_dt = gyro_integral_dt;
-			imu.delta_velocity_dt = accel_integral_dt;
-			imu.delta_velocity_clipping = _delta_velocity_clipping;
-			imu.timestamp = hrt_absolute_time();
-			_vehicle_imu_pub.publish(imu);
-
 			// reset clip counts
 			_delta_velocity_clipping = 0;
 

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -35,11 +35,11 @@
 
 #include "Integrator.hpp"
 
-#include <sensor_calibration/SensorCalibration.hpp>
-
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor_calibration/Gyroscope.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -91,8 +91,8 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
-	SensorCalibration _accel_calibration{SensorCalibration::SensorType::Accelerometer};
-	SensorCalibration _gyro_calibration{SensorCalibration::SensorType::Gyroscope};
+	calibration::Accelerometer _accel_calibration{};
+	calibration::Gyroscope _gyro_calibration{};
 
 	Integrator _accel_integrator{}; // 200 Hz default
 	Integrator _gyro_integrator{true};   // 200 Hz default, coning compensation enabled