diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index b174e8f5231abd24eee04afb0aecbe479b518e2e..4dcfb341ba42c75303984e77e83879683be90828 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/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
diff --git a/src/modules/events/polyfit.hpp b/src/modules/events/polyfit.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..613842c1d4d0c8366a6e1592ac8cbd891d5abe11
--- /dev/null
+++ b/src/modules/events/polyfit.hpp
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ * 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
diff --git a/src/modules/events/temperature_calibration.cpp b/src/modules/events/temperature_calibration.cpp
index a6c29643ed539d37ae69e086570dbac8649de0e4..9a7ae6dfb775b8cfb764de7399177e3f4d7751fe 100644
--- a/src/modules/events/temperature_calibration.cpp
+++ b/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();
}