From 7e6daaf3d120047c6b55d0cc2950f79b23c6f8e1 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Wed, 25 Jan 2017 16:54:27 +0530 Subject: [PATCH] tempcal: remove separate temperature calibration module --- src/modules/tempcal/CMakeLists.txt | 44 --- src/modules/tempcal/polyfit.hpp | 151 ----------- src/modules/tempcal/tempcal_main.cpp | 382 --------------------------- src/modules/tempcal/tempcal_params.c | 40 --- 4 files changed, 617 deletions(-) delete mode 100644 src/modules/tempcal/CMakeLists.txt delete mode 100644 src/modules/tempcal/polyfit.hpp delete mode 100644 src/modules/tempcal/tempcal_main.cpp delete mode 100644 src/modules/tempcal/tempcal_params.c diff --git a/src/modules/tempcal/CMakeLists.txt b/src/modules/tempcal/CMakeLists.txt deleted file mode 100644 index 80e9759d25..0000000000 --- a/src/modules/tempcal/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################# -px4_add_module( - MODULE modules__tempcal - MAIN tempcal - COMPILE_FLAGS - STACK_MAIN 2500 - STACK_MAX 4000 - SRCS - tempcal_main.cpp - DEPENDS - platforms__common -) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/tempcal/polyfit.hpp b/src/modules/tempcal/polyfit.hpp deleted file mode 100644 index 613842c1d4..0000000000 --- a/src/modules/tempcal/polyfit.hpp +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** - * - * 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/tempcal/tempcal_main.cpp b/src/modules/tempcal/tempcal_main.cpp deleted file mode 100644 index 996b07bf06..0000000000 --- a/src/modules/tempcal/tempcal_main.cpp +++ /dev/null @@ -1,382 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @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 -#include "polyfit.hpp" - -#define DEBUG 0 -#if 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 -{ -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 task_main_trampoline(int argc, char *argv[]); - - 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::print_status() -{ - //TODO: Implement Print Status -} - -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; - } - - // 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::task_main_trampoline(int argc, char *argv[]) -{ - tempcal::instance->task_main(); -} - -int Tempcal::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = px4_task_spawn_cmd("tempcal", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 5800, - (px4_main_t)&Tempcal::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - PX4_WARN("task start failed"); - return -errno; - - } else { - PX4_WARN("Tempcal proc started"); - } - - return OK; -} - -int tempcal_main(int argc, char *argv[]) -{ - if (argc < 2) { - PX4_WARN("usage: tempcal {start|stop|status}"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (tempcal::instance != nullptr) { - PX4_WARN("already running"); - return 1; - } - - tempcal::instance = new Tempcal(); - - if (tempcal::instance == nullptr) { - PX4_WARN("alloc failed"); - return 1; - } - - if (OK != tempcal::instance->start()) { - delete tempcal::instance; - tempcal::instance = nullptr; - PX4_WARN("start failed"); - return 1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (tempcal::instance == nullptr) { - PX4_WARN("not running"); - return 1; - } - - tempcal::instance->exit(); - - // wait for the destruction of the instance - while (tempcal::instance != nullptr) { - usleep(50000); - } - - return 0; - } - - if (!strcmp(argv[1], "print")) { - if (tempcal::instance != nullptr) { - - return 0; - } - - return 1; - } - - if (!strcmp(argv[1], "status")) { - if (tempcal::instance) { - PX4_WARN("running"); - tempcal::instance->print_status(); - return 0; - - } else { - PX4_WARN("not running"); - return 1; - } - } - - PX4_WARN("unrecognized command"); - return 1; -} diff --git a/src/modules/tempcal/tempcal_params.c b/src/modules/tempcal/tempcal_params.c deleted file mode 100644 index d6eb96f856..0000000000 --- a/src/modules/tempcal/tempcal_params.c +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file tempcal_params.c - * Parameter definition for tempcal. - * - * @author Siddharth B Purohit - * - */