From 269d05ff22c125eb8647d789f2a5ec94f0261243 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Wed, 14 Dec 2016 11:56:37 +0530 Subject: [PATCH] tempcal: sandbox app for temperature calibration --- src/modules/tempcal/CMakeLists.txt | 46 +++ src/modules/tempcal/matrix_alg.cpp | 403 +++++++++++++++++++++++++++ src/modules/tempcal/matrix_alg.h | 51 ++++ src/modules/tempcal/polyfit.cpp | 122 ++++++++ src/modules/tempcal/polyfit.h | 77 +++++ src/modules/tempcal/tempcal_main.cpp | 373 +++++++++++++++++++++++++ src/modules/tempcal/tempcal_params.c | 40 +++ 7 files changed, 1112 insertions(+) create mode 100644 src/modules/tempcal/CMakeLists.txt create mode 100644 src/modules/tempcal/matrix_alg.cpp create mode 100644 src/modules/tempcal/matrix_alg.h create mode 100644 src/modules/tempcal/polyfit.cpp create mode 100644 src/modules/tempcal/polyfit.h create mode 100644 src/modules/tempcal/tempcal_main.cpp create mode 100644 src/modules/tempcal/tempcal_params.c diff --git a/src/modules/tempcal/CMakeLists.txt b/src/modules/tempcal/CMakeLists.txt new file mode 100644 index 0000000000..e4bac1edb7 --- /dev/null +++ b/src/modules/tempcal/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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 + polyfit.cpp + matrix_alg.cpp + DEPENDS + platforms__common +) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/tempcal/matrix_alg.cpp b/src/modules/tempcal/matrix_alg.cpp new file mode 100644 index 0000000000..99cf4b9989 --- /dev/null +++ b/src/modules/tempcal/matrix_alg.cpp @@ -0,0 +1,403 @@ +/**************************************************************************** + * + * 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(float m[], float invOut[]) +{ + float 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.0f / det; + + for (i = 0; i < 16; i++) { + invOut[i] = inv[i] * det; + } + + return true; +} + +bool inverse(float *A, float *inv, uint8_t n) +{ + printf("Got Here!!\n"); + + if (n == 4) { + printf("Got Here4!!\n"); + return inverse4x4(A, inv); + + } else { + 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 new file mode 100644 index 0000000000..b8b29bfb7c --- /dev/null +++ b/src/modules/tempcal/matrix_alg.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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(float m[], float invOut[]); +bool inverse(float *A, float *inv, uint8_t n); diff --git a/src/modules/tempcal/polyfit.cpp b/src/modules/tempcal/polyfit.cpp new file mode 100644 index 0000000000..ebca6a6dd3 --- /dev/null +++ b/src/modules/tempcal/polyfit.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 +*/ +#include "polyfit.h" +#include "matrix_alg.h" + +int polyfitter::init(uint8_t order) +{ + _forder = order + 1; + VTV = new float[_forder * _forder]; + + if (VTV == NULL) { + return -1; + } + + VTY = new float[_forder]; + + if (VTY == NULL) { + return -1; + } + + memset(VTV, 0, sizeof(float)*_forder * _forder); + memset(VTY, 0, sizeof(float)*_forder); + return 0; +} + +void polyfitter::update(float x, float y) +{ + update_VTV(x); + update_VTY(x, y); +} + +void polyfitter::update_VTY(float x, float y) +{ + float temp = 1.0f; + + for (int8_t i = _forder - 1; i >= 0; i--) { + VTY[i] += y * temp; + temp *= x; + } +} + + +void polyfitter::update_VTV(float x) +{ + float temp = 1.0f; + int8_t z; + + 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] += temp; + } + + temp *= x; + } +} + +bool polyfitter::fit(float res[]) +{ + //Do inverse of VTV + float *IVTV = new float[_forder * _forder]; + + if (VTV == NULL) { + return false; + } + + if (inverse(VTV, IVTV, _forder)) { + for (uint8_t i = 0; i < _forder; i++) { + res[i] = 0.0f; + + for (int j = 0; j < _forder; j++) { + res[i] += IVTV[i * _forder + j] * VTY[j]; + } + } + + return true; + } + + return false; +} \ No newline at end of file diff --git a/src/modules/tempcal/polyfit.h b/src/modules/tempcal/polyfit.h new file mode 100644 index 0000000000..33762099b1 --- /dev/null +++ b/src/modules/tempcal/polyfit.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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(float x, float y); + int init(uint8_t order); + bool fit(float res[]); +private: + float *VTV; + float *VTY; + uint8_t _forder; + void update_VTV(float x); + void update_VTY(float x, float y); +}; \ No newline at end of file diff --git a/src/modules/tempcal/tempcal_main.cpp b/src/modules/tempcal/tempcal_main.cpp new file mode 100644 index 0000000000..ceb361c29d --- /dev/null +++ b/src/modules/tempcal/tempcal_main.cpp @@ -0,0 +1,373 @@ +/**************************************************************************** + * + * 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 "polyfit.h" + +#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 */ + math::LowPassFilter2p _lp_roll_rate; + math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; +}; + +Tempcal::Tempcal(): + SuperBlock(NULL, "EKF"), + _lp_roll_rate(250.0f, 1.0f), + _lp_pitch_rate(250.0f, 1.0f), + _lp_yaw_rate(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 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}; + + float _low_temp[SENSOR_COUNT_MAX], _high_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; + P[i][0].init(3); + P[i][1].init(3); + P[i][2].init(3); + } + + // initialize data structures outside of loop + // because they will else not always be + // properly populated + sensor_gyro_s gyro_data = {}; + + 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] = _lp_roll_rate.apply(gyro_data.x); + gyro_sample_filt[i][1] = _lp_roll_rate.apply(gyro_data.y); + gyro_sample_filt[i][2] = _lp_roll_rate.apply(gyro_data.z); + gyro_sample_filt[i][3] = gyro_data.temperature; + + if (!_cold_soaked[i]) { + _cold_soaked[i] = true; + _low_temp[i] = gyro_data.temperature; //Record the low temperature + } + + num_samples[i]++; + } + } + + bool _collection_complete = true; + + for (uint8_t i = 0; i < num_gyro; i++) { + if (_hot_soaked[i]) { + continue; + + } else { + _collection_complete = false; + } + + if (num_samples[i] < 250) { + continue; + } + + if (gyro_sample_filt[i][3] <= _low_temp[i]) { + //We are not hot soaking increment hot soak saturation count + _hot_soak_sat[i]++; + } + + if (_hot_soak_sat[i] == 10) { + _hot_soaked[i] = true; + _high_temp[i] = gyro_sample_filt[i][3]; + } + + printf("Got Here!! %.6f %.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]); + //update linear fit matrices + P[i][0].update(gyro_sample_filt[i][0], gyro_sample_filt[i][3]); + P[i][1].update(gyro_sample_filt[i][1], gyro_sample_filt[i][3]); + P[i][2].update(gyro_sample_filt[i][2], gyro_sample_filt[i][3]); + num_samples[i] = 0; + } + + if (_collection_complete) { + for (uint8_t i = 0; i < num_gyro; i++) { + if (_high_temp[i] - _low_temp[i] > 20) { + PX4_WARN("Cal Failed for Gyro %d", i); + + } else { + float res[4]; + P[i][0].fit(res); + PX4_WARN("Result Gyro %d Axis 0: %.6f %.6f %.6f %.6f", 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: %.6f %.6f %.6f %.6f", 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: %.6f %.6f %.6f %.6f", i, (double)res[0], (double)res[1], (double)res[2], + (double)res[3]); + } + } + + break; //complete temp cal + } + } + + 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 new file mode 100644 index 0000000000..d6eb96f856 --- /dev/null +++ b/src/modules/tempcal/tempcal_params.c @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + * + */