events: add temperature calibration scheme

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -98,7 +98,7 @@ set(config_module_list
 	modules/gpio_led
 	#modules/uavcan
 	modules/land_detector
-	modules/tempcal
+	#modules/tempcal
 
 	#
 	# Estimation modules

src/modules/events/polyfit.hpp

+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/*
+Polygon linear fit
+Author: Siddharth Bharat Purohit
+*/
+
+#pragma once
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <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<size_t _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 (uint8_t i = 0; i < _forder; i++) {
+			for (int j = 0; j < _forder; j++) {
+				PF_DEBUG("%.10f ", (double)IVTV(i, j));
+			}
+
+			PF_DEBUG("\n");
+		}
+
+		for (uint8_t i = 0; i < _forder; i++) {
+			res[i] = 0.0f;
+
+			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.0f;
+		PF_DEBUG("O %.6f\n", (double)x);
+
+		for (int8_t 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.0f;
+		int8_t z;
+
+		for (uint8_t i = 0; i < _forder; i++) {
+			for (int j = 0; j < _forder; j++) {
+				PF_DEBUG("%.10f ", (double)VTV(i, j));
+			}
+
+			PF_DEBUG("\n");
+		}
+
+		for (int8_t i = 2 * _forder - 2; i >= 0; i--) {
+			if (i < _forder) {
+				z = 0.0f;
+
+			} else {
+				z = i - _forder + 1;
+			}
+
+			for (int8_t j = i - z; j >= z; j--) {
+				uint8_t row = j;
+				uint8_t col = i - j;
+				VTV(row, col) += (double)temp;
+			}
+
+			temp *= x;
+		}
+	}
+};
\ No newline at end of file

src/modules/events/temperature_calibration.cpp

@@ -31,35 +31,286 @@
  *
  ****************************************************************************/
 
-#include "temperature_calibration.h"
+/**
+ * @file tempcal_main.cpp
+ * Implementation of the Temperature Calibration for onboard sensors.
+ *
+ * @author Siddharth Bharat Purohit
+ */
 
-#include <px4_log.h>
+#include <px4_config.h>
+#include <px4_defines.h>
 #include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <float.h>
+#include <vector>
+#include <arch/board/board.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <platforms/px4_defines.h>
+#include <drivers/drv_hrt.h>
+#include <controllib/uorb/blocks.hpp>
 
-static void do_temperature_calibration(int argc, char *argv[]);
+#include <uORB/topics/sensor_gyro.h>
+#include "polyfit.hpp"
+#include "temperature_calibration.h"
 
-int run_temperature_calibration()
+#define TC_PRINT_DEBUG 0
+#if TC_PRINT_DEBUG
+#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
+#else
+#define TC_DEBUG(fmt, ...)
+#endif
+
+
+#define SENSOR_COUNT_MAX		3
+
+extern "C" __EXPORT int tempcal_main(int argc, char *argv[]);
+
+
+class Tempcal;
+
+namespace tempcal
 {
-	PX4_INFO("Starting Temperature calibration task");
+Tempcal *instance = nullptr;
+}
+
+
+class Tempcal : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Tempcal();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~Tempcal();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	static void do_temperature_calibration(int argc, char *argv[]);
 
-	char *const args[1] = { NULL };
-	int task = px4_task_spawn_cmd("temperature_calib",
-				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_MAX - 5,
-				      2000,
-				      (px4_main_t)&do_temperature_calibration,
-				      args);
+	void		task_main();
+
+	void print_status();
+
+	void exit() { _task_should_exit = true; }
+
+private:
+	bool	_task_should_exit = false;
+	int	_control_task = -1;		// task handle for task
+
+	/* Low pass filter for attitude rates */
+	//std::vector<math::LowPassFilter2p> _lp_roll_rate;
+	//std::vector<math::LowPassFilter2p> _lp_pitch_rate;
+	//std::vector<math::LowPassFilter2p> _lp_yaw_rate;
+};
+
+Tempcal::Tempcal():
+	SuperBlock(NULL, "Tempcal")
+	//_lp_roll_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)),
+	//_lp_pitch_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)),
+	//_lp_yaw_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f))
+{
+}
+
+Tempcal::~Tempcal()
+{
+
+}
+
+void Tempcal::task_main()
+{
+	// subscribe to relevant topics
+	int gyro_sub[SENSOR_COUNT_MAX];
+	float gyro_sample_filt[SENSOR_COUNT_MAX][4];
+	polyfitter<4> P[SENSOR_COUNT_MAX][3];
+	px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
+	uint8_t _hot_soak_sat[SENSOR_COUNT_MAX] = {};
+	unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
+	uint16_t num_samples[SENSOR_COUNT_MAX] = {0};
+
+	bool _cold_soaked[SENSOR_COUNT_MAX] = {false};
+	bool _hot_soaked[SENSOR_COUNT_MAX] = {false};
+	bool _tempcal_complete[SENSOR_COUNT_MAX] = {false};
+	float _low_temp[SENSOR_COUNT_MAX];
+	float _high_temp[SENSOR_COUNT_MAX] = {0};
+	float _ref_temp[SENSOR_COUNT_MAX];
+
+	for (unsigned i = 0; i < num_gyro; i++) {
+		if (gyro_sub[i] < 0) {
+			gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
+		}
+	}
+
+	for (uint8_t i = 0; i < num_gyro; i++) {
+		fds[i].fd = gyro_sub[i];
+		fds[i].events = POLLIN;
+	}
 
-	if (task < 0) {
+	// initialize data structures outside of loop
+	// because they will else not always be
+	// properly populated
+	sensor_gyro_s gyro_data = {};
+	//uint16_t l = 0;
+
+	while (!_task_should_exit) {
+		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
+
+		if (ret < 0) {
+			// Poll error, sleep and try again
+			usleep(10000);
+			continue;
+
+		} else if (ret == 0) {
+			// Poll timeout or no new data, do nothing
+			continue;
+		}
+
+		for (uint8_t i = 0; i < num_gyro; i++) {
+			if (_hot_soaked[i]) {
+				continue;
+			}
+
+			if (fds[i].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
+
+				gyro_sample_filt[i][0] = gyro_data.x;
+				gyro_sample_filt[i][1] = gyro_data.y;
+				gyro_sample_filt[i][2] = gyro_data.z;
+				gyro_sample_filt[i][3] = gyro_data.temperature;
+
+				if (!_cold_soaked[i]) {
+					_cold_soaked[i] = true;
+					_low_temp[i] = gyro_sample_filt[i][3];	//Record the low temperature
+					_ref_temp[i] = gyro_sample_filt[i][3] + 12.0f;
+				}
+
+				num_samples[i]++;
+			}
+		}
+
+
+		for (uint8_t i = 0; i < 1; i++) {
+			if (_hot_soaked[i]) {
+				continue;
+			}
+
+			//if (num_samples[i] < 250) {
+			//	continue;
+			//}
+
+			if (gyro_sample_filt[i][3] > _high_temp[i]) {
+				_high_temp[i] = gyro_sample_filt[i][3];
+				_hot_soak_sat[i] = 0;
+
+			} else {
+				continue;
+			}
+
+			//TODO: Hot Soak Saturation
+			if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) {
+				_hot_soaked[i] = true;
+			}
+
+			if (i == 0) {
+				TC_DEBUG("%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n", (double)gyro_sample_filt[i][0], (double)gyro_sample_filt[i][1],
+					 (double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3], (double)_low_temp[i], (double)_high_temp[i],
+					 (double)(_high_temp[i] - _low_temp[i]));
+			}
+
+			//update linear fit matrices
+			gyro_sample_filt[i][3] -= _ref_temp[i];
+			P[i][0].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][0]);
+			P[i][1].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][1]);
+			P[i][2].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][2]);
+			num_samples[i] = 0;
+		}
+
+		for (uint8_t i = 0; i < 1; i++) {
+			if (_hot_soaked[i] && !_tempcal_complete[i]) {
+				double res[4];
+				P[i][0].fit(res);
+				PX4_WARN("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
+					 (double)res[3]);
+				P[i][1].fit(res);
+				PX4_WARN("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
+					 (double)res[3]);
+				P[i][2].fit(res);
+				PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
+					 (double)res[3]);
+				_tempcal_complete[i] = true;
+			}
+		}
+
+		usleep(100);
+	}
+
+	for (uint8_t i = 0; i < num_gyro; i++) {
+		orb_unsubscribe(gyro_sub[i]);
+	}
+
+	delete tempcal::instance;
+	tempcal::instance = nullptr;
+	PX4_WARN("Tempcal process stopped");
+}
+
+void Tempcal::do_temperature_calibration(int argc, char *argv[])
+{
+	tempcal::instance->task_main();
+}
+
+int Tempcal::start()
+{
+
+	ASSERT(_control_task == -1);
+	_control_task = px4_task_spawn_cmd("temperature_calib",
+					   SCHED_DEFAULT,
+					   SCHED_PRIORITY_MAX - 5,
+					   5800,
+					   (px4_main_t)&Tempcal::do_temperature_calibration,
+					   nullptr);
+
+	if (_control_task < 0) {
+		delete tempcal::instance;
+		tempcal::instance = nullptr;
+		PX4_WARN("start failed");
 		return -errno;
 	}
 
 	return 0;
 }
 
-
-void do_temperature_calibration(int argc, char *argv[])
+int run_temperature_calibration()
 {
-	//TODO ...
+	PX4_INFO("Starting Temperature calibration task");
+	tempcal::instance = new Tempcal();
+
+	if (tempcal::instance == nullptr) {
+		PX4_WARN("alloc failed");
+		return 1;
+	}
 
+	return tempcal::instance->start();
 }