fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 04:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

tempcal: sandbox app for temperature calibration

Browse Source
This commit is contained in:
Siddharth Bharat Purohit
2017-01-25 22:43:29 +01:00
committed by Lorenz Meier
parent 0909c32262
commit 269d05ff22
7 changed files with 1112 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/tempcal/CMakeLists.txt
+46
View File
@@ -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 :
src/modules/tempcal/matrix_alg.cpp
+403
View File
@@ -0,0 +1,403 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Siddharth Bharat Purohit <sidbpurohit@gmail.com>
*
* 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 <stdio.h>
/*
* 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);
}
}
src/modules/tempcal/matrix_alg.h
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Siddharth Bharat Purohit <sibpurohit@gmail.com>
*
* 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 <inttypes.h>
#include <string.h>
#include <math.h>
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);
src/modules/tempcal/polyfit.cpp
+122
View File
@@ -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;
}
src/modules/tempcal/polyfit.h
+77
View File
@@ -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 <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>
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);
};
src/modules/tempcal/tempcal_main.cpp
+373
View File
@@ -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 <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 <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>
#include <uORB/topics/sensor_gyro.h>
#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;
}
src/modules/tempcal/tempcal_params.c
+40
View File
@@ -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 <siddharthbharatpurohit@gmail.com>
*
*/