create temperature_compensation module

- this is a new module for temperature compensation that consolidates the functionality previously handled in the sensors module (calculating runtime thermal corrections) and the events module (online thermal calibration)
 - by collecting this functionality into a single module we can optionally disable it on systems where it's not used and save some flash (if disabled at build time) or memory (disabled at run time)

src/modules/temperature_compensation/temperature_calibration/accel.cpp

@@ -0,0 +1,230 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 accel.cpp
+ * Implementation of the Accel Temperature Calibration for onboard sensors.
+ *
+ * @author Siddharth Bharat Purohit
+ * @author Paul Riseborough
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include "accel.h"
+#include <uORB/topics/sensor_accel.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+
+TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature,
+		float max_start_temperature)
+	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
+{
+	// init subscriptions
+	_num_sensor_instances = orb_group_count(ORB_ID(sensor_accel));
+
+	if (_num_sensor_instances > SENSOR_COUNT_MAX) {
+		_num_sensor_instances = SENSOR_COUNT_MAX;
+	}
+
+	for (unsigned i = 0; i < _num_sensor_instances; i++) {
+		_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
+	}
+}
+
+TemperatureCalibrationAccel::~TemperatureCalibrationAccel()
+{
+	for (unsigned i = 0; i < _num_sensor_instances; i++) {
+		orb_unsubscribe(_sensor_subs[i]);
+	}
+}
+
+void TemperatureCalibrationAccel::reset_calibration()
+{
+	/* reset all driver level calibrations */
+	float offset = 0.0f;
+	float scale = 1.0f;
+
+	for (unsigned s = 0; s < 3; s++) {
+		set_parameter("CAL_ACC%u_XOFF", s, &offset);
+		set_parameter("CAL_ACC%u_YOFF", s, &offset);
+		set_parameter("CAL_ACC%u_ZOFF", s, &offset);
+		set_parameter("CAL_ACC%u_XSCALE", s, &scale);
+		set_parameter("CAL_ACC%u_YSCALE", s, &scale);
+		set_parameter("CAL_ACC%u_ZSCALE", s, &scale);
+	}
+}
+
+int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
+{
+	bool finished = data.hot_soaked;
+
+	bool updated;
+	orb_check(sensor_sub, &updated);
+
+	if (!updated) {
+		return finished ? 0 : 1;
+	}
+
+	sensor_accel_s accel_data;
+	orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data);
+
+	if (finished) {
+		// if we're done, return, but we need to return after orb_copy because of poll()
+		return 0;
+	}
+
+	data.device_id = accel_data.device_id;
+
+	data.sensor_sample_filt[0] = accel_data.x;
+	data.sensor_sample_filt[1] = accel_data.y;
+	data.sensor_sample_filt[2] = accel_data.z;
+	data.sensor_sample_filt[3] = accel_data.temperature;
+
+	// wait for min start temp to be reached before starting calibration
+	if (data.sensor_sample_filt[3] < _min_start_temperature) {
+		return 1;
+	}
+
+	if (!data.cold_soaked) {
+		// allow time for sensors and filters to settle
+		if (hrt_absolute_time() > 10E6) {
+			// If intial temperature exceeds maximum declare an error condition and exit
+			if (data.sensor_sample_filt[3] > _max_start_temperature) {
+				return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
+
+			} else {
+				data.cold_soaked = true;
+				data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
+				data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
+				data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
+				return 1;
+			}
+
+		} else {
+			return 1;
+		}
+	}
+
+	// check if temperature increased
+	if (data.sensor_sample_filt[3] > data.high_temp) {
+		data.high_temp = data.sensor_sample_filt[3];
+		data.hot_soak_sat = 0;
+
+	} else {
+		return 1;
+	}
+
+	//TODO: Detect when temperature has stopped rising for more than TBD seconds
+	if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
+		data.hot_soaked = true;
+	}
+
+	if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
+		TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
+			 (double)data.sensor_sample_filt[1],
+			 (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
+			 (double)(data.high_temp - data.low_temp));
+	}
+
+	//update linear fit matrices
+	double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
+	data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
+	data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
+	data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
+
+	return 1;
+}
+
+int TemperatureCalibrationAccel::finish()
+{
+	for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+		finish_sensor_instance(_data[uorb_index], uorb_index);
+	}
+
+	int32_t enabled = 1;
+	int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
+
+	if (result != PX4_OK) {
+		PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
+	}
+
+	return result;
+}
+
+int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index)
+{
+	if (!data.hot_soaked || data.tempcal_complete) {
+		return 0;
+	}
+
+	double res[3][4] = {};
+	data.P[0].fit(res[0]);
+	res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
+	PX4_INFO("Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
+		 (double)res[0][2],
+		 (double)res[0][3]);
+	data.P[1].fit(res[1]);
+	res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
+	PX4_INFO("Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
+		 (double)res[1][2],
+		 (double)res[1][3]);
+	data.P[2].fit(res[2]);
+	res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
+	PX4_INFO("Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
+		 (double)res[2][2],
+		 (double)res[2][3]);
+	data.tempcal_complete = true;
+
+	char str[30];
+	float param = 0.0f;
+	int result = PX4_OK;
+
+	set_parameter("TC_A%d_ID", sensor_index, &data.device_id);
+
+	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
+		for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
+			sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
+			param = (float)res[axis_index][coef_index];
+			result = param_set_no_notification(param_find(str), &param);
+
+			if (result != PX4_OK) {
+				PX4_ERR("unable to reset %s", str);
+			}
+		}
+	}
+
+	set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
+	set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
+	set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
+	return 0;
+}

src/modules/temperature_compensation/temperature_calibration/accel.h

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "common.h"
+#include "polyfit.hpp"
+
+class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3>
+{
+public:
+	TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
+	virtual ~TemperatureCalibrationAccel();
+
+	/**
+	 * @see TemperatureCalibrationBase::finish()
+	 */
+	int finish();
+
+	/**
+	 * @see TemperatureCalibrationBase::reset_calibration()
+	 */
+	void reset_calibration();
+
+private:
+
+	virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
+
+	inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
+};

src/modules/temperature_compensation/temperature_calibration/baro.cpp

@@ -0,0 +1,203 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 baro.cpp
+ * Implementation of the Baro Temperature Calibration for onboard sensors.
+ *
+ * @author Siddharth Bharat Purohit
+ * @author Paul Riseborough
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include "baro.h"
+#include <uORB/topics/sensor_baro.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+
+TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature,
+		float max_start_temperature)
+	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
+{
+	// init subscriptions
+	_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
+
+	if (_num_sensor_instances > SENSOR_COUNT_MAX) {
+		_num_sensor_instances = SENSOR_COUNT_MAX;
+	}
+
+	for (unsigned i = 0; i < _num_sensor_instances; i++) {
+		_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
+	}
+}
+
+TemperatureCalibrationBaro::~TemperatureCalibrationBaro()
+{
+	for (unsigned i = 0; i < _num_sensor_instances; i++) {
+		orb_unsubscribe(_sensor_subs[i]);
+	}
+}
+
+void TemperatureCalibrationBaro::reset_calibration()
+{
+	// nothing to do
+}
+
+int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
+{
+	bool finished = data.hot_soaked;
+
+	bool updated;
+	orb_check(sensor_sub, &updated);
+
+	if (!updated) {
+		return finished ? 0 : 1;
+	}
+
+	sensor_baro_s baro_data;
+	orb_copy(ORB_ID(sensor_baro), sensor_sub, &baro_data);
+
+	if (finished) {
+		// if we're done, return, but we need to return after orb_copy because of poll()
+		return 0;
+	}
+
+	data.device_id = baro_data.device_id;
+
+	data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
+	data.sensor_sample_filt[1] = baro_data.temperature;
+
+
+	// wait for min start temp to be reached before starting calibration
+	if (data.sensor_sample_filt[1] < _min_start_temperature) {
+		return 1;
+	}
+
+	if (!data.cold_soaked) {
+		// allow time for sensors and filters to settle
+		if (hrt_absolute_time() > 10E6) {
+			// If intial temperature exceeds maximum declare an error condition and exit
+			if (data.sensor_sample_filt[1] > _max_start_temperature) {
+				return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
+
+			} else {
+				data.cold_soaked = true;
+				data.low_temp = data.sensor_sample_filt[1]; // Record the low temperature
+				data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
+				data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise;
+				return 1;
+			}
+
+		} else {
+			return 1;
+		}
+	}
+
+	// check if temperature increased
+	if (data.sensor_sample_filt[1] > data.high_temp) {
+		data.high_temp = data.sensor_sample_filt[1];
+		data.hot_soak_sat = 0;
+
+	} else {
+		return 1;
+	}
+
+	// TODO: Detect when temperature has stopped rising for more than TBD seconds
+	if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
+		data.hot_soaked = true;
+	}
+
+	if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
+		TC_DEBUG("\nBaro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
+			 (double)data.sensor_sample_filt[1], (double)data.low_temp, (double)data.high_temp,
+			 (double)(data.high_temp - data.low_temp));
+	}
+
+	// update linear fit matrices
+	double relative_temperature = (double)data.sensor_sample_filt[1] - (double)data.ref_temp;
+	data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
+
+	return 1;
+}
+
+int TemperatureCalibrationBaro::finish()
+{
+	for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+		finish_sensor_instance(_data[uorb_index], uorb_index);
+	}
+
+	int32_t enabled = 1;
+	int result = param_set_no_notification(param_find("TC_B_ENABLE"), &enabled);
+
+	if (result != PX4_OK) {
+		PX4_ERR("unable to reset TC_B_ENABLE (%i)", result);
+	}
+
+	return result;
+}
+
+int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
+{
+	if (!data.hot_soaked || data.tempcal_complete) {
+		return 0;
+	}
+
+	double res[POLYFIT_ORDER + 1] {};
+	data.P[0].fit(res);
+	res[POLYFIT_ORDER] =
+		0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero
+	PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0],
+		 (double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
+	data.tempcal_complete = true;
+
+	char str[30];
+	float param = 0.0f;
+	int result = PX4_OK;
+
+	set_parameter("TC_B%d_ID", sensor_index, &data.device_id);
+
+	for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
+		sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
+		param = (float)res[coef_index];
+		result = param_set_no_notification(param_find(str), &param);
+
+		if (result != PX4_OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+	}
+
+	set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
+	set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
+	set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
+	return 0;
+}

src/modules/temperature_compensation/temperature_calibration/baro.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "common.h"
+#include "polyfit.hpp"
+
+#define POLYFIT_ORDER			5
+
+
+class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFIT_ORDER>
+{
+public:
+	TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
+	virtual ~TemperatureCalibrationBaro();
+
+	/**
+	 * @see TemperatureCalibrationBase::finish()
+	 */
+	int finish();
+
+	/**
+	 * @see TemperatureCalibrationBase::reset_calibration()
+	 */
+	void reset_calibration();
+
+private:
+
+	virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);
+
+	inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
+};

src/modules/temperature_compensation/temperature_calibration/common.h

@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#define TC_PRINT_DEBUG 0
+#if TC_PRINT_DEBUG
+#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
+#else
+#define TC_DEBUG(fmt, ...)
+#endif
+
+#include <px4_platform_common/log.h>
+#include <mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+
+#include "polyfit.hpp"
+
+#define SENSOR_COUNT_MAX		3
+
+
+#define TC_ERROR_INITIAL_TEMP_TOO_HIGH 110 ///< starting temperature was above the configured allowed temperature
+#define TC_ERROR_COMMUNICATION         112 ///< no sensors found
+
+/**
+ * Base class for temperature calibration types with abstract methods (for all different sensor types)
+ */
+class TemperatureCalibrationBase
+{
+public:
+	TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
+		: _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature),
+		  _max_start_temperature(max_start_temperature) {}
+
+	virtual ~TemperatureCalibrationBase() = default;
+
+	/**
+	 * check & update new sensor data.
+	 * @return progress in range [0, 100], 110 when finished, <0 on error,
+	 *         -TC_ERROR_INITIAL_TEMP_TOO_HIGH if starting temperature is too hot
+	 *         -TC_ERROR_COMMUNICATION if no sensors found
+	 */
+	virtual int update() = 0;
+
+	/**
+	 * do final fitting & write the parameters. Call this exactly once after update() returned 110
+	 * @return 0 on success, <0 otherwise
+	 */
+	virtual int finish() = 0;
+
+	/** reset all driver-level calibration parameters */
+	virtual void reset_calibration() = 0;
+
+protected:
+
+	/**
+	 * set a system parameter (without system notification) and print an error if it fails
+	 * @param format_str for example "CAL_GYRO%u_XOFF"
+	 * @param index which index (will replace %u in format_str)
+	 * @param value
+	 * @return 0 on success
+	 */
+	inline int set_parameter(const char *format_str, unsigned index, const void *value);
+
+	float _min_temperature_rise; ///< minimum difference in temperature before the process finishes
+	float _min_start_temperature; ///< minimum temperature before the process starts
+	float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared
+};
+
+int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value)
+{
+	char param_str[30] {};
+	(void)sprintf(param_str, format_str, index);
+	int result = param_set_no_notification(param_find(param_str), value);
+
+	if (result != 0) {
+		PX4_ERR("unable to reset %s (%i)", param_str, result);
+	}
+
+	return result;
+}
+
+/**
+ ** class TemperatureCalibrationCommon
+ * Common base class for all sensor types, contains shared code & data.
+ */
+template <int Dim, int PolyfitOrder>
+class TemperatureCalibrationCommon : public TemperatureCalibrationBase
+{
+public:
+	TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
+		: TemperatureCalibrationBase(min_temperature_rise, min_start_temperature, max_start_temperature) {}
+
+	virtual ~TemperatureCalibrationCommon() = default;
+
+	/**
+	 * @see TemperatureCalibrationBase::update()
+	 */
+	int update()
+	{
+		int num_not_complete = 0;
+
+		if (_num_sensor_instances == 0) {
+			return -TC_ERROR_COMMUNICATION;
+		}
+
+		for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+			int status = update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
+
+			if (status < 0) {
+				return status;
+			}
+
+			num_not_complete += status;
+		}
+
+		if (num_not_complete > 0) {
+			// calculate progress
+			float min_diff = _min_temperature_rise;
+
+			for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+				float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
+
+				if (cur_diff < min_diff) {
+					min_diff = cur_diff;
+				}
+			}
+
+			return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
+		}
+
+		return 110;
+	}
+
+protected:
+
+	struct PerSensorData {
+		float sensor_sample_filt[Dim + 1]; ///< last value is the temperature
+		polyfitter < PolyfitOrder + 1 > P[Dim];
+		unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces
+									from the last reading */
+		uint32_t device_id = 0; ///< ID for the sensor being calibrated
+		bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been
+		/// verified and the starting temperature set
+		bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase
+		bool tempcal_complete = false; ///< true when the calibration has been completed
+		float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C)
+		float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C)
+		float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the
+							calibration temperature range (deg C) */
+	};
+
+	PerSensorData _data[SENSOR_COUNT_MAX];
+
+	/**
+	 * update a single sensor instance
+	 * @return 0 when done, 1 not finished yet, <0 for an error
+	 */
+	virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0;
+
+	unsigned _num_sensor_instances{0};
+	int _sensor_subs[SENSOR_COUNT_MAX];
+};

src/modules/temperature_compensation/temperature_calibration/gyro.cpp

@@ -0,0 +1,211 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 gyro.cpp
+ * Implementation of the Gyro Temperature Calibration for onboard sensors.
+ *
+ * @author Siddharth Bharat Purohit
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <mathlib/mathlib.h>
+#include <uORB/topics/sensor_gyro.h>
+#include "gyro.h"
+#include <drivers/drv_hrt.h>
+
+TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature,
+		float max_start_temperature, int gyro_subs[], int num_gyros)
+	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
+{
+	for (int i = 0; i < num_gyros; ++i) {
+		_sensor_subs[i] = gyro_subs[i];
+	}
+
+	_num_sensor_instances = num_gyros;
+}
+
+void TemperatureCalibrationGyro::reset_calibration()
+{
+	/* reset all driver level calibrations */
+	float offset = 0.0f;
+
+	for (unsigned s = 0; s < 3; s++) {
+		set_parameter("CAL_GYRO%u_XOFF", s, &offset);
+		set_parameter("CAL_GYRO%u_YOFF", s, &offset);
+		set_parameter("CAL_GYRO%u_ZOFF", s, &offset);
+	}
+}
+
+int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub)
+{
+	bool finished = data.hot_soaked;
+
+	bool updated;
+	orb_check(sensor_sub, &updated);
+
+	if (!updated) {
+		return finished ? 0 : 1;
+	}
+
+	sensor_gyro_s gyro_data{};
+	orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data);
+
+	if (finished) {
+		// if we're done, return, but we need to return after orb_copy because of poll()
+		return 0;
+	}
+
+	data.device_id = gyro_data.device_id;
+
+	data.sensor_sample_filt[0] = gyro_data.x;
+	data.sensor_sample_filt[1] = gyro_data.y;
+	data.sensor_sample_filt[2] = gyro_data.z;
+	data.sensor_sample_filt[3] = gyro_data.temperature;
+
+	// wait for min start temp to be reached before starting calibration
+	if (data.sensor_sample_filt[3] < _min_start_temperature) {
+		return 1;
+	}
+
+	if (!data.cold_soaked) {
+		// allow time for sensors and filters to settle
+		if (hrt_absolute_time() > 10E6) {
+			// If intial temperature exceeds maximum declare an error condition and exit
+			if (data.sensor_sample_filt[3] > _max_start_temperature) {
+				return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
+
+			} else {
+				data.cold_soaked = true;
+				data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
+				data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
+				data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
+				return 1;
+			}
+
+		} else {
+			return 1;
+		}
+	}
+
+	// check if temperature increased
+	if (data.sensor_sample_filt[3] > data.high_temp) {
+		data.high_temp = data.sensor_sample_filt[3];
+		data.hot_soak_sat = 0;
+
+	} else {
+		return 1;
+	}
+
+	// TODO: Detect when temperature has stopped rising for more than TBD seconds
+	if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
+		data.hot_soaked = true;
+	}
+
+	if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
+		TC_DEBUG("\nGyro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
+			 (double)data.sensor_sample_filt[1],
+			 (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
+			 (double)(data.high_temp - data.low_temp));
+	}
+
+	//update linear fit matrices
+	double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
+	data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
+	data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
+	data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
+
+	return 1;
+}
+
+int TemperatureCalibrationGyro::finish()
+{
+	for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+		finish_sensor_instance(_data[uorb_index], uorb_index);
+	}
+
+	int32_t enabled = 1;
+	int result = param_set_no_notification(param_find("TC_G_ENABLE"), &enabled);
+
+	if (result != PX4_OK) {
+		PX4_ERR("unable to reset TC_G_ENABLE (%i)", result);
+	}
+
+	return result;
+}
+
+int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index)
+{
+	if (!data.hot_soaked || data.tempcal_complete) {
+		return 0;
+	}
+
+	double res[3][4] {};
+	data.P[0].fit(res[0]);
+	PX4_INFO("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
+		 (double)res[0][2],
+		 (double)res[0][3]);
+	data.P[1].fit(res[1]);
+	PX4_INFO("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
+		 (double)res[1][2],
+		 (double)res[1][3]);
+	data.P[2].fit(res[2]);
+	PX4_INFO("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
+		 (double)res[2][2],
+		 (double)res[2][3]);
+	data.tempcal_complete = true;
+
+	char str[30] {};
+	float param = 0.0f;
+	int result = PX4_OK;
+
+	set_parameter("TC_G%d_ID", sensor_index, &data.device_id);
+
+	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
+		for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
+			sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
+			param = (float)res[axis_index][coef_index];
+			result = param_set_no_notification(param_find(str), &param);
+
+			if (result != PX4_OK) {
+				PX4_ERR("unable to reset %s", str);
+			}
+		}
+	}
+
+	set_parameter("TC_G%d_TMAX", sensor_index, &data.high_temp);
+	set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp);
+	set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp);
+
+	return 0;
+}

src/modules/temperature_compensation/temperature_calibration/gyro.h

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "common.h"
+#include "polyfit.hpp"
+
+class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3>
+{
+public:
+	TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature,
+				   int gyro_subs[], int num_gyros);
+	virtual ~TemperatureCalibrationGyro() {}
+
+	/**
+	 * @see TemperatureCalibrationBase::finish()
+	 */
+	int finish();
+
+	/**
+	 * @see TemperatureCalibrationBase::reset_calibration()
+	 */
+	void reset_calibration();
+
+private:
+
+	virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);
+
+	inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
+};

src/modules/temperature_compensation/temperature_calibration/polyfit.hpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-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.
+ *
+ ****************************************************************************/
+
+/*
+
+This algorithm performs a curve fit of m x,y data points using a polynomial
+equation of the following form:
+
+yi = a0 + a1.xi + a2.xi^2 + a3.xi^3 + .... + an.xi^n + ei , where:
+
+i = [0,m]
+xi is the x coordinate (independant variable) of the i'th measurement
+yi is the y coordinate (dependant variable) of the i'th measurement
+ei is a random fit error being the difference between the i'th y coordinate
+   and the value predicted by the polynomial.
+
+In vector form this is represented as:
+
+Y = V.A + E , where:
+
+V is Vandermonde matrix in x -> https://en.wikipedia.org/wiki/Vandermonde_matrix
+Y is a vector of length m containing the y measurements
+E is a vector of length m containing the fit errors for each measurement
+
+Use an Ordinary Least Squares derivation to minimise ∑(i=0..m)ei^2 -> https://en.wikipedia.org/wiki/Ordinary_least_squares
+
+Note: In the wikipedia reference, the X matrix in reference is equivalent to our V matrix and the Beta matrix is equivalent to our A matrix
+
+A = inv(transpose(V)*V)*(transpose(V)*Y)
+
+We can accumulate VTV and VTY recursively as they are of fixed size, where:
+
+VTV = transpose(V)*V =
+ __                                                                                                                        __
+|    m+1                      x0+x1+...+xm                   x0^2+x1^2+...+xm^3   ..........  x0^n+x1^n+...+xm^n             |
+|x0+x1+...+xm              x0^2+x1^2+...+xm^3                x0^3+x1^3+...+xm^3   ..........  x0^(n+1)+x1^(n+1)+...+xm^(n+1) |
+|      .                            .                                  .                             .                       |
+|      .                            .                                  .                             .                       |
+|      .                            .                                  .                             .                       |
+|x0^n+x1^n+...+xm^n     x0^(n+1)+x1^(n+1)+...+xm^(n+1)  x0^(n+2)+x1^(n+2)+...+xm^(n+2) ....  x0^(2n)+x1^(2n)+...+xm^(2n)     |
+|__                                                                                                                        __|
+
+and VTY = transpose(V)*Y =
+ __            __
+|  ∑(i=0..m)yi   |
+| ∑(i=0..m)yi*xi |
+|       .        |
+|       .        |
+|       .        |
+|∑(i=0..m)yi*xi^n|
+|__            __|
+
+*/
+
+/*
+Polygon linear fit
+Author: Siddharth Bharat Purohit
+*/
+
+#pragma once
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/time.h>
+
+#include <float.h>
+
+#include <matrix/math.hpp>
+
+#define DEBUG 0
+#if DEBUG
+#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
+#else
+#define PF_DEBUG(fmt, ...)
+#endif
+
+template<int _forder>
+class polyfitter
+{
+public:
+	polyfitter() {}
+
+	void update(double x, double y)
+	{
+		update_VTV(x);
+		update_VTY(x, y);
+	}
+
+	bool fit(double res[])
+	{
+		//Do inverse of VTV
+		matrix::SquareMatrix<double, _forder> IVTV;
+
+		IVTV = _VTV.I();
+
+		for (int i = 0; i < _forder; i++) {
+			for (int j = 0; j < _forder; j++) {
+				PF_DEBUG("%.10f ", (double)IVTV(i, j));
+			}
+
+			PF_DEBUG("\n");
+		}
+
+		for (int i = 0; i < _forder; i++) {
+			res[i] = 0.0;
+
+			for (int j = 0; j < _forder; j++) {
+				res[i] += IVTV(i, j) * (double)_VTY(j);
+			}
+
+			PF_DEBUG("%.10f ", res[i]);
+		}
+
+		return true;
+	}
+
+private:
+	matrix::SquareMatrix<double, _forder> _VTV;
+	matrix::Vector<double, _forder> _VTY;
+
+	void update_VTY(double x, double y)
+	{
+		double temp = 1.0;
+		PF_DEBUG("O %.6f\n", (double)x);
+
+		for (int i = _forder - 1; i >= 0; i--) {
+			_VTY(i) += y * temp;
+			temp *= x;
+			PF_DEBUG("%.6f ", (double)_VTY(i));
+		}
+
+		PF_DEBUG("\n");
+	}
+
+	void update_VTV(double x)
+	{
+		double temp = 1.0;
+		int8_t z;
+
+		for (int i = 0; i < _forder; i++) {
+			for (int j = 0; j < _forder; j++) {
+				PF_DEBUG("%.10f ", (double)_VTV(i, j));
+			}
+
+			PF_DEBUG("\n");
+		}
+
+		for (int i = 2 * _forder - 2; i >= 0; i--) {
+			if (i < _forder) {
+				z = 0.0f;
+
+			} else {
+				z = i - _forder + 1;
+			}
+
+			for (int j = i - z; j >= z; j--) {
+				int row = j;
+				int col = i - j;
+				_VTV(row, col) += (double)temp;
+			}
+
+			temp *= x;
+		}
+	}
+};

src/modules/temperature_compensation/temperature_calibration/task.cpp

@@ -0,0 +1,362 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 task.cpp
+ *
+ * Main task handling the temperature calibration process
+ *
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/sensor_gyro.h>
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <unistd.h>
+
+#include "common.h"
+#include "temperature_calibration.h"
+#include "accel.h"
+#include "baro.h"
+#include "gyro.h"
+
+class TemperatureCalibration;
+
+namespace temperature_calibration
+{
+px4::atomic<TemperatureCalibration *> instance{nullptr};
+}
+
+class TemperatureCalibration
+{
+public:
+
+	TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {}
+	~TemperatureCalibration() = default;
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	static int do_temperature_calibration(int argc, char *argv[]);
+
+	void		task_main();
+
+	void exit_task() { _force_task_exit = true; }
+
+private:
+	void publish_led_control(led_control_s &led_control);
+
+	uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
+
+	bool	_force_task_exit = false;
+	int	_control_task = -1;		// task handle for task
+
+	const bool _accel; ///< enable accel calibration?
+	const bool _baro; ///< enable baro calibration?
+	const bool _gyro; ///< enable gyro calibration?
+};
+
+void TemperatureCalibration::task_main()
+{
+	// subscribe to all gyro instances
+	int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1};
+	px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {};
+	unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
+
+	if (num_gyro > SENSOR_COUNT_MAX) {
+		num_gyro = SENSOR_COUNT_MAX;
+	}
+
+	for (unsigned i = 0; i < num_gyro; i++) {
+		gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
+		fds[i].fd = gyro_sub[i];
+		fds[i].events = POLLIN;
+	}
+
+	int32_t min_temp_rise = 24;
+	param_get(param_find("SYS_CAL_TDEL"), &min_temp_rise);
+	PX4_INFO("Waiting for %i degrees difference in sensor temperature", min_temp_rise);
+
+	int32_t min_start_temp = 5;
+	param_get(param_find("SYS_CAL_TMIN"), &min_start_temp);
+
+	int32_t max_start_temp = 10;
+	param_get(param_find("SYS_CAL_TMAX"), &max_start_temp);
+
+	//init calibrators
+	TemperatureCalibrationBase *calibrators[3] {};
+	bool error_reported[3] {};
+	int num_calibrators = 0;
+
+	if (_accel) {
+		calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp);
+
+		if (calibrators[num_calibrators]) {
+			++num_calibrators;
+
+		} else {
+			PX4_ERR("alloc failed");
+		}
+	}
+
+	if (_baro) {
+		calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp, max_start_temp);
+
+		if (calibrators[num_calibrators]) {
+			++num_calibrators;
+
+		} else {
+			PX4_ERR("alloc failed");
+		}
+	}
+
+	if (_gyro) {
+		calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub,
+				num_gyro);
+
+		if (calibrators[num_calibrators]) {
+			++num_calibrators;
+
+		} else {
+			PX4_ERR("alloc failed");
+		}
+	}
+
+	// reset params
+	for (int i = 0; i < num_calibrators; ++i) {
+		calibrators[i]->reset_calibration();
+	}
+
+	// make sure the system updates the changed parameters
+	param_notify_changes();
+
+	px4_usleep(300000); // wait a bit for the system to apply the parameters
+
+	hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
+
+	// control LED's: blink, then turn solid according to progress
+	led_control_s led_control{};
+	led_control.led_mask = 0xff;
+	led_control.mode = led_control_s::MODE_BLINK_NORMAL;
+	led_control.priority = led_control_s::MAX_PRIORITY;
+	led_control.color = led_control_s::COLOR_YELLOW;
+	led_control.num_blinks = 0;
+	publish_led_control(led_control);
+	int leds_completed = 0;
+
+	bool abort_calibration = false;
+
+	while (!_force_task_exit) {
+		/* we poll on the gyro(s), since this is the sensor with the highest update rate.
+		 * Each individual sensor will then check on its own if there's new data.
+		 */
+		int ret = px4_poll(fds, num_gyro, 1000);
+
+		if (ret < 0) {
+			// Poll error, sleep and try again
+			px4_usleep(10000);
+			continue;
+
+		} else if (ret == 0) {
+			// Poll timeout or no new data, do nothing
+			continue;
+		}
+
+		//if gyro is not enabled: we must do an orb_copy here, so that poll() does not immediately return again
+		if (!_gyro) {
+			sensor_gyro_s gyro_data;
+
+			for (unsigned i = 0; i < num_gyro; ++i) {
+				orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
+			}
+		}
+
+		int min_progress = 110;
+
+		for (int i = 0; i < num_calibrators; ++i) {
+			ret = calibrators[i]->update();
+
+			if (ret == -TC_ERROR_COMMUNICATION) {
+				abort_calibration = true;
+				PX4_ERR("Calibration won't start - sensor bad or communication error");
+				_force_task_exit = true;
+				break;
+
+			} else if (ret == -TC_ERROR_INITIAL_TEMP_TOO_HIGH) {
+				abort_calibration = true;
+				PX4_ERR("Calibration won't start - sensor temperature too high");
+				_force_task_exit = true;
+				break;
+
+			} else if (ret < 0 && !error_reported[i]) {
+				// temperature has decreased so calibration is not being updated
+				error_reported[i] = true;
+				PX4_ERR("Calibration update step failed (%i)", ret);
+
+			} else if (ret < min_progress) {
+				// temperature is stable or increasing
+				min_progress = ret;
+			}
+		}
+
+		if (min_progress == 110 || abort_calibration) {
+			break; // we are done
+		}
+
+		int led_progress = min_progress * BOARD_MAX_LEDS / 100;
+
+		for (; leds_completed < led_progress; ++leds_completed) {
+			led_control.led_mask = 1 << leds_completed;
+			led_control.mode = led_control_s::MODE_ON;
+			publish_led_control(led_control);
+		}
+
+		//print progress each second
+		hrt_abstime now = hrt_absolute_time();
+
+		if (now > next_progress_output) {
+			PX4_INFO("Calibration progress: %i%%", min_progress);
+			next_progress_output = now + 1e6;
+		}
+	}
+
+	if (abort_calibration) {
+		led_control.color = led_control_s::COLOR_RED;
+
+	} else {
+		PX4_INFO("Sensor Measurments completed");
+
+		// save params immediately so that we can check the result and don't have to wait for param save timeout
+		param_control_autosave(false);
+
+		// do final calculations & parameter storage
+		for (int i = 0; i < num_calibrators; ++i) {
+			int ret = calibrators[i]->finish();
+
+			if (ret < 0) {
+				PX4_ERR("Failed to finish calibration process (%i)", ret);
+			}
+		}
+
+		param_notify_changes();
+		int ret = param_save_default();
+
+		if (ret != 0) {
+			PX4_ERR("Failed to save params (%i)", ret);
+		}
+
+		param_control_autosave(true);
+
+		led_control.color = led_control_s::COLOR_GREEN;
+	}
+
+	// blink the LED's according to success/failure
+	led_control.led_mask = 0xff;
+	led_control.mode = led_control_s::MODE_BLINK_FAST;
+	led_control.num_blinks = 0;
+	publish_led_control(led_control);
+
+	for (int i = 0; i < num_calibrators; ++i) {
+		delete calibrators[i];
+	}
+
+	for (unsigned i = 0; i < num_gyro; i++) {
+		orb_unsubscribe(gyro_sub[i]);
+	}
+
+	delete temperature_calibration::instance.load();
+	temperature_calibration::instance.store(nullptr);
+	PX4_INFO("Exiting temperature calibration task");
+}
+
+int TemperatureCalibration::do_temperature_calibration(int argc, char *argv[])
+{
+	temperature_calibration::instance.load()->task_main();
+	return 0;
+}
+
+int TemperatureCalibration::start()
+{
+	_control_task = px4_task_spawn_cmd("temperature_calib",
+					   SCHED_DEFAULT,
+					   SCHED_PRIORITY_MAX - 5,
+					   5800,
+					   (px4_main_t)&TemperatureCalibration::do_temperature_calibration,
+					   nullptr);
+
+	if (_control_task < 0) {
+		delete temperature_calibration::instance.load();
+		temperature_calibration::instance.store(nullptr);
+		PX4_ERR("start failed");
+		return -errno;
+	}
+
+	return 0;
+}
+
+void TemperatureCalibration::publish_led_control(led_control_s &led_control)
+{
+	led_control.timestamp = hrt_absolute_time();
+	_led_control_pub.publish(led_control);
+}
+
+int run_temperature_calibration(bool accel, bool baro, bool gyro)
+{
+	if (temperature_calibration::instance.load() == nullptr) {
+		PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
+		temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro));
+
+		if (temperature_calibration::instance.load() == nullptr) {
+			PX4_ERR("alloc failed");
+			return 1;
+		}
+
+		return temperature_calibration::instance.load()->start();
+
+	} else {
+		PX4_WARN("temperature calibration task already running");
+	}
+
+	return PX4_ERROR;
+}

src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+
+/** start temperature calibration in a new task for one or multiple sensors
+ * @return 0 on success, <0 error otherwise  */
+int run_temperature_calibration(bool accel, bool baro, bool gyro);