From 917a8f63f6969688d41265743cdc1627db80ebed Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Tue, 24 Jan 2017 13:17:42 +0530 Subject: [PATCH] events: add temperature calibration scheme --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 +- src/modules/events/polyfit.hpp | 151 ++++++++++ .../events/temperature_calibration.cpp | 283 +++++++++++++++++- 3 files changed, 419 insertions(+), 17 deletions(-) create mode 100644 src/modules/events/polyfit.hpp diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index b174e8f523..4dcfb341ba 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 0000000000..613842c1d4 --- /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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define DEBUG 0 +#if DEBUG +#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); +#else +#define PF_DEBUG(fmt, ...) +#endif + +template +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 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 VTV; + matrix::Vector 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 a6c29643ed..9a7ae6dfb7 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 +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -static void do_temperature_calibration(int argc, char *argv[]); +#include +#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 _lp_roll_rate; + //std::vector _lp_pitch_rate; + //std::vector _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(); }