diff --git a/src/modules/tempcal/CMakeLists.txt b/src/modules/tempcal/CMakeLists.txt index e4bac1edb7..80e9759d25 100644 --- a/src/modules/tempcal/CMakeLists.txt +++ b/src/modules/tempcal/CMakeLists.txt @@ -38,8 +38,6 @@ px4_add_module( STACK_MAX 4000 SRCS tempcal_main.cpp - polyfit.cpp - matrix_alg.cpp DEPENDS platforms__common ) diff --git a/src/modules/tempcal/matrix_alg.cpp b/src/modules/tempcal/matrix_alg.cpp deleted file mode 100644 index 18a6dfbfb5..0000000000 --- a/src/modules/tempcal/matrix_alg.cpp +++ /dev/null @@ -1,394 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Siddharth Bharat Purohit - * - * 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 matrix_alg.cpp - * - * Matrix algebra on raw arrays - */ - -#include "matrix_alg.h" -#include -/* - * Does matrix multiplication of two regular/square matrices - * - * @param A, Matrix A - * @param B, Matrix B - * @param n, dimemsion of square matrices - * @returns multiplied matrix i.e. A*B - */ - -float *mat_mul(float *A, float *B, uint8_t n) -{ - float *ret = new float[n * n]; - memset(ret, 0.0f, n * n * sizeof(float)); - - for (uint8_t i = 0; i < n; i++) { - for (uint8_t j = 0; j < n; j++) { - for (uint8_t k = 0; k < n; k++) { - ret[i * n + j] += A[i * n + k] * B[k * n + j]; - } - } - } - - return ret; -} - -static inline void swap(float &a, float &b) -{ - float c; - c = a; - a = b; - b = c; -} - -/* - * calculates pivot matrix such that all the larger elements in the row are on diagonal - * - * @param A, input matrix matrix - * @param pivot - * @param n, dimenstion of square matrix - * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful - */ - -static void mat_pivot(float *A, float *pivot, uint8_t n) -{ - for (uint8_t i = 0; i < n; i++) { - for (uint8_t j = 0; j < n; j++) { - pivot[i * n + j] = (i == j); - } - } - - for (uint8_t i = 0; i < n; i++) { - uint8_t max_j = i; - - for (uint8_t j = i; j < n; j++) { - if (fabsf(A[j * n + i]) > fabsf(A[max_j * n + i])) { - max_j = j; - } - } - - if (max_j != i) { - for (uint8_t k = 0; k < n; k++) { - swap(pivot[i * n + k], pivot[max_j * n + k]); - } - } - } -} - -/* - * calculates matrix inverse of Lower trangular matrix using forward substitution - * - * @param L, lower triangular matrix - * @param out, Output inverted lower triangular matrix - * @param n, dimension of matrix - */ - -static void mat_forward_sub(float *L, float *out, uint8_t n) -{ - // Forward substitution solve LY = I - for (int i = 0; i < n; i++) { - out[i * n + i] = 1 / L[i * n + i]; - - for (int j = i + 1; j < n; j++) { - for (int k = i; k < j; k++) { - out[j * n + i] -= L[j * n + k] * out[k * n + i]; - } - - out[j * n + i] /= L[j * n + j]; - } - } -} - -/* - * calculates matrix inverse of Upper trangular matrix using backward substitution - * - * @param U, upper triangular matrix - * @param out, Output inverted upper triangular matrix - * @param n, dimension of matrix - */ - -static void mat_back_sub(float *U, float *out, uint8_t n) -{ - // Backward Substitution solve UY = I - for (int i = n - 1; i >= 0; i--) { - out[i * n + i] = 1 / U[i * n + i]; - - for (int j = i - 1; j >= 0; j--) { - for (int k = i; k > j; k--) { - out[j * n + i] -= U[j * n + k] * out[k * n + i]; - } - - out[j * n + i] /= U[j * n + j]; - } - } -} - -/* - * Decomposes square matrix into Lower and Upper triangular matrices such that - * A*P = L*U, where P is the pivot matrix - * ref: http://rosettacode.org/wiki/LU_decomposition - * @param U, upper triangular matrix - * @param out, Output inverted upper triangular matrix - * @param n, dimension of matrix - */ - -static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n) -{ - memset(L, 0, n * n * sizeof(float)); - memset(U, 0, n * n * sizeof(float)); - memset(P, 0, n * n * sizeof(float)); - mat_pivot(A, P, n); - - float *APrime = mat_mul(P, A, n); - - for (uint8_t i = 0; i < n; i++) { - L[i * n + i] = 1; - } - - for (uint8_t i = 0; i < n; i++) { - for (uint8_t j = 0; j < n; j++) { - if (j <= i) { - U[j * n + i] = APrime[j * n + i]; - - for (uint8_t k = 0; k < j; k++) { - U[j * n + i] -= L[j * n + k] * U[k * n + i]; - } - } - - if (j >= i) { - L[j * n + i] = APrime[j * n + i]; - - for (uint8_t k = 0; k < i; k++) { - L[j * n + i] -= L[j * n + k] * U[k * n + i]; - } - - L[j * n + i] /= U[i * n + i]; - } - } - } - - delete[] APrime; -} - -/* - * matrix inverse code for any square matrix using LU decomposition - * inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix - * ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf - * @param m, input 4x4 matrix - * @param inv, Output inverted 4x4 matrix - * @param n, dimension of square matrix - * @returns false = matrix is Singular, true = matrix inversion successful - */ -bool mat_inverse(float *A, float *inv, uint8_t n) -{ - float *L, *U, *P; - bool ret = true; - L = new float[n * n]; - U = new float[n * n]; - P = new float[n * n]; - mat_LU_decompose(A, L, U, P, n); - - float *L_inv = new float[n * n]; - float *U_inv = new float[n * n]; - - memset(L_inv, 0, n * n * sizeof(float)); - mat_forward_sub(L, L_inv, n); - - memset(U_inv, 0, n * n * sizeof(float)); - mat_back_sub(U, U_inv, n); - - // decomposed matrices no longer required - delete[] L; - delete[] U; - - float *inv_unpivoted = mat_mul(U_inv, L_inv, n); - float *inv_pivoted = mat_mul(inv_unpivoted, P, n); - - //check sanity of results - for (uint8_t i = 0; i < n; i++) { - for (uint8_t j = 0; j < n; j++) { - if (isnan(inv_pivoted[i * n + j]) || isinf(inv_pivoted[i * n + j])) { - ret = false; - } - } - } - - memcpy(inv, inv_pivoted, n * n * sizeof(float)); - - //free memory - delete[] inv_pivoted; - delete[] inv_unpivoted; - delete[] P; - delete[] U_inv; - delete[] L_inv; - return ret; -} - -bool inverse4x4(double m[], double invOut[]) -{ - double inv[16], det; - uint8_t i; - - inv[0] = m[5] * m[10] * m[15] - - m[5] * m[11] * m[14] - - m[9] * m[6] * m[15] + - m[9] * m[7] * m[14] + - m[13] * m[6] * m[11] - - m[13] * m[7] * m[10]; - - inv[4] = -m[4] * m[10] * m[15] + - m[4] * m[11] * m[14] + - m[8] * m[6] * m[15] - - m[8] * m[7] * m[14] - - m[12] * m[6] * m[11] + - m[12] * m[7] * m[10]; - - inv[8] = m[4] * m[9] * m[15] - - m[4] * m[11] * m[13] - - m[8] * m[5] * m[15] + - m[8] * m[7] * m[13] + - m[12] * m[5] * m[11] - - m[12] * m[7] * m[9]; - - inv[12] = -m[4] * m[9] * m[14] + - m[4] * m[10] * m[13] + - m[8] * m[5] * m[14] - - m[8] * m[6] * m[13] - - m[12] * m[5] * m[10] + - m[12] * m[6] * m[9]; - - inv[1] = -m[1] * m[10] * m[15] + - m[1] * m[11] * m[14] + - m[9] * m[2] * m[15] - - m[9] * m[3] * m[14] - - m[13] * m[2] * m[11] + - m[13] * m[3] * m[10]; - - inv[5] = m[0] * m[10] * m[15] - - m[0] * m[11] * m[14] - - m[8] * m[2] * m[15] + - m[8] * m[3] * m[14] + - m[12] * m[2] * m[11] - - m[12] * m[3] * m[10]; - - inv[9] = -m[0] * m[9] * m[15] + - m[0] * m[11] * m[13] + - m[8] * m[1] * m[15] - - m[8] * m[3] * m[13] - - m[12] * m[1] * m[11] + - m[12] * m[3] * m[9]; - - inv[13] = m[0] * m[9] * m[14] - - m[0] * m[10] * m[13] - - m[8] * m[1] * m[14] + - m[8] * m[2] * m[13] + - m[12] * m[1] * m[10] - - m[12] * m[2] * m[9]; - - inv[2] = m[1] * m[6] * m[15] - - m[1] * m[7] * m[14] - - m[5] * m[2] * m[15] + - m[5] * m[3] * m[14] + - m[13] * m[2] * m[7] - - m[13] * m[3] * m[6]; - - inv[6] = -m[0] * m[6] * m[15] + - m[0] * m[7] * m[14] + - m[4] * m[2] * m[15] - - m[4] * m[3] * m[14] - - m[12] * m[2] * m[7] + - m[12] * m[3] * m[6]; - - inv[10] = m[0] * m[5] * m[15] - - m[0] * m[7] * m[13] - - m[4] * m[1] * m[15] + - m[4] * m[3] * m[13] + - m[12] * m[1] * m[7] - - m[12] * m[3] * m[5]; - - inv[14] = -m[0] * m[5] * m[14] + - m[0] * m[6] * m[13] + - m[4] * m[1] * m[14] - - m[4] * m[2] * m[13] - - m[12] * m[1] * m[6] + - m[12] * m[2] * m[5]; - - inv[3] = -m[1] * m[6] * m[11] + - m[1] * m[7] * m[10] + - m[5] * m[2] * m[11] - - m[5] * m[3] * m[10] - - m[9] * m[2] * m[7] + - m[9] * m[3] * m[6]; - - inv[7] = m[0] * m[6] * m[11] - - m[0] * m[7] * m[10] - - m[4] * m[2] * m[11] + - m[4] * m[3] * m[10] + - m[8] * m[2] * m[7] - - m[8] * m[3] * m[6]; - - inv[11] = -m[0] * m[5] * m[11] + - m[0] * m[7] * m[9] + - m[4] * m[1] * m[11] - - m[4] * m[3] * m[9] - - m[8] * m[1] * m[7] + - m[8] * m[3] * m[5]; - - inv[15] = m[0] * m[5] * m[10] - - m[0] * m[6] * m[9] - - m[4] * m[1] * m[10] + - m[4] * m[2] * m[9] + - m[8] * m[1] * m[6] - - m[8] * m[2] * m[5]; - - det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; - - if (fabsf(det) < 1.1755e-38f) { - return false; - } - - det = 1.0 / det; - - for (i = 0; i < 16; i++) { - invOut[i] = inv[i] * det; - } - - return true; -} - -bool inverse(float *A, float *inv, uint8_t n) -{ - return mat_inverse(A, inv, n); -} \ No newline at end of file diff --git a/src/modules/tempcal/matrix_alg.h b/src/modules/tempcal/matrix_alg.h deleted file mode 100644 index ed62a55927..0000000000 --- a/src/modules/tempcal/matrix_alg.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Siddharth Bharat Purohit - * - * 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 matrix_alg.h - * - * Matrix algebra on raw arrays - */ - - -#pragma once - -#include -#include -#include - -float *mat_mul(float *A, float *B, uint8_t n); -bool mat_inverse(float *A, float *inv, uint8_t n); -bool inverse4x4(double m[], double invOut[]); -bool inverse(float *A, float *inv, uint8_t n); diff --git a/src/modules/tempcal/polyfit.h b/src/modules/tempcal/polyfit.h deleted file mode 100644 index 87dfb00413..0000000000 --- a/src/modules/tempcal/polyfit.h +++ /dev/null @@ -1,77 +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 - -class polyfitter -{ -public: - polyfitter() {} - ~polyfitter() - { - if (VTV != nullptr) { - delete[] VTV; - } - - if (VTY != nullptr) { - delete[] VTY; - } - } - void update(double x, double y); - int init(uint8_t order); - bool fit(double res[]); -private: - double *VTV; - double *VTY; - uint8_t _forder; - void update_VTV(double x); - void update_VTY(double x, double y); -}; \ No newline at end of file diff --git a/src/modules/tempcal/polyfit.cpp b/src/modules/tempcal/polyfit.hpp similarity index 60% rename from src/modules/tempcal/polyfit.cpp rename to src/modules/tempcal/polyfit.hpp index 2a1c55c253..613842c1d4 100644 --- a/src/modules/tempcal/polyfit.cpp +++ b/src/modules/tempcal/polyfit.hpp @@ -34,8 +34,24 @@ Polygon linear fit Author: Siddharth Bharat Purohit */ -#include "polyfit.h" -#include "matrix_alg.h" + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #define DEBUG 0 #if DEBUG @@ -44,91 +60,28 @@ Author: Siddharth Bharat Purohit #define PF_DEBUG(fmt, ...) #endif -int polyfitter::init(uint8_t order) +template +class polyfitter { - _forder = order + 1; - VTV = new double[_forder * _forder]; +public: + polyfitter() {} - if (VTV == NULL) { - return -1; + void update(double x, double y) + { + update_VTV(x); + update_VTY(x, y); } - VTY = new double[_forder]; + bool fit(double res[]) + { + //Do inverse of VTV + matrix::SquareMatrix IVTV; - if (VTY == NULL) { - return -1; - } + IVTV = VTV.I(); - memset(VTV, 0, sizeof(double)*_forder * _forder); - memset(VTY, 0, sizeof(double)*_forder); - return 0; -} - -void polyfitter::update(double x, double y) -{ - update_VTV(x); - update_VTY(x, y); -} - -void polyfitter::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 polyfitter::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 * _forder + 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 * _forder + col] += (double)temp; - } - - temp *= x; - } -} - -bool polyfitter::fit(double res[]) -{ - //Do inverse of VTV - double *IVTV = new double[_forder * _forder]; - - if (VTV == NULL) { - return false; - } - - if (inverse4x4(VTV, IVTV)) { for (uint8_t i = 0; i < _forder; i++) { for (int j = 0; j < _forder; j++) { - PF_DEBUG("%.10f ", (double)IVTV[i * _forder + j]); + PF_DEBUG("%.10f ", (double)IVTV(i, j)); } PF_DEBUG("\n"); @@ -138,7 +91,7 @@ bool polyfitter::fit(double res[]) res[i] = 0.0f; for (int j = 0; j < _forder; j++) { - res[i] += IVTV[i * _forder + j] * (double)VTY[j]; + res[i] += IVTV(i, j) * (double)VTY(j); } PF_DEBUG("%.10f ", res[i]); @@ -147,5 +100,52 @@ bool polyfitter::fit(double res[]) return true; } - return false; -} \ No newline at end of file +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 index 68b9933df6..996b07bf06 100644 --- a/src/modules/tempcal/tempcal_main.cpp +++ b/src/modules/tempcal/tempcal_main.cpp @@ -65,7 +65,7 @@ #include #include -#include "polyfit.h" +#include "polyfit.hpp" #define DEBUG 0 #if DEBUG @@ -149,7 +149,7 @@ void Tempcal::task_main() // subscribe to relevant topics int gyro_sub[SENSOR_COUNT_MAX]; float gyro_sample_filt[SENSOR_COUNT_MAX][4]; - polyfitter P[SENSOR_COUNT_MAX][3]; + 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)); @@ -171,9 +171,6 @@ void Tempcal::task_main() for (uint8_t i = 0; i < num_gyro; i++) { fds[i].fd = gyro_sub[i]; fds[i].events = POLLIN; - P[i][0].init(3); - P[i][1].init(3); - P[i][2].init(3); } // initialize data structures outside of loop @@ -210,8 +207,8 @@ void Tempcal::task_main() if (!_cold_soaked[i]) { _cold_soaked[i] = true; - _low_temp[i] = gyro_data.temperature; //Record the low temperature - _ref_temp[i] = gyro_data.temperature + 12.0f; + _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]++; @@ -235,6 +232,7 @@ void Tempcal::task_main() } else { continue; } + //TODO: Hot Soak Saturation if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) { _hot_soaked[i] = true;