mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 19:57:34 +08:00
events: add temperature calibration scheme
This commit is contained in:
Siddharth Bharat Purohit
committed by
Lorenz Meier
Lorenz Meier
parent
86d9ba9cde
commit
917a8f63f6
@@ -98,7 +98,7 @@ set(config_module_list
|
||||
modules/gpio_led
|
||||
#modules/uavcan
|
||||
modules/land_detector
|
||||
modules/tempcal
|
||||
#modules/tempcal
|
||||
|
||||
#
|
||||
# Estimation modules
|
||||
|
||||
@@ -0,0 +1,151 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
/*
|
||||
Polygon linear fit
|
||||
Author: Siddharth Bharat Purohit
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <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 <matrix/math.hpp>
|
||||
|
||||
#define DEBUG 0
|
||||
#if DEBUG
|
||||
#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
|
||||
#else
|
||||
#define PF_DEBUG(fmt, ...)
|
||||
#endif
|
||||
|
||||
template<size_t _forder>
|
||||
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<double, _forder> 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<double, _forder> VTV;
|
||||
matrix::Vector<double, _forder> 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;
|
||||
}
|
||||
}
|
||||
};
|
||||
@@ -31,35 +31,286 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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 <vector>
|
||||
#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.hpp"
|
||||
#include "temperature_calibration.h"
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_tasks.h>
|
||||
#define TC_PRINT_DEBUG 0
|
||||
#if TC_PRINT_DEBUG
|
||||
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
|
||||
#else
|
||||
#define TC_DEBUG(fmt, ...)
|
||||
#endif
|
||||
|
||||
static void do_temperature_calibration(int argc, char *argv[]);
|
||||
|
||||
int run_temperature_calibration()
|
||||
#define SENSOR_COUNT_MAX 3
|
||||
|
||||
extern "C" __EXPORT int tempcal_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
class Tempcal;
|
||||
|
||||
namespace tempcal
|
||||
{
|
||||
PX4_INFO("Starting Temperature calibration task");
|
||||
Tempcal *instance = nullptr;
|
||||
}
|
||||
|
||||
char *const args[1] = { NULL };
|
||||
int task = px4_task_spawn_cmd("temperature_calib",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
(px4_main_t)&do_temperature_calibration,
|
||||
args);
|
||||
|
||||
if (task < 0) {
|
||||
class Tempcal : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Tempcal();
|
||||
|
||||
/**
|
||||
* Destructor, also kills task.
|
||||
*/
|
||||
~Tempcal();
|
||||
|
||||
/**
|
||||
* Start task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
int start();
|
||||
|
||||
static void do_temperature_calibration(int argc, char *argv[]);
|
||||
|
||||
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<math::LowPassFilter2p> _lp_roll_rate;
|
||||
//std::vector<math::LowPassFilter2p> _lp_pitch_rate;
|
||||
//std::vector<math::LowPassFilter2p> _lp_yaw_rate;
|
||||
};
|
||||
|
||||
Tempcal::Tempcal():
|
||||
SuperBlock(NULL, "Tempcal")
|
||||
//_lp_roll_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)),
|
||||
//_lp_pitch_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)),
|
||||
//_lp_yaw_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f))
|
||||
{
|
||||
}
|
||||
|
||||
Tempcal::~Tempcal()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void Tempcal::task_main()
|
||||
{
|
||||
// subscribe to relevant topics
|
||||
int gyro_sub[SENSOR_COUNT_MAX];
|
||||
float gyro_sample_filt[SENSOR_COUNT_MAX][4];
|
||||
polyfitter<4> P[SENSOR_COUNT_MAX][3];
|
||||
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
|
||||
uint8_t _hot_soak_sat[SENSOR_COUNT_MAX] = {};
|
||||
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
|
||||
uint16_t num_samples[SENSOR_COUNT_MAX] = {0};
|
||||
|
||||
bool _cold_soaked[SENSOR_COUNT_MAX] = {false};
|
||||
bool _hot_soaked[SENSOR_COUNT_MAX] = {false};
|
||||
bool _tempcal_complete[SENSOR_COUNT_MAX] = {false};
|
||||
float _low_temp[SENSOR_COUNT_MAX];
|
||||
float _high_temp[SENSOR_COUNT_MAX] = {0};
|
||||
float _ref_temp[SENSOR_COUNT_MAX];
|
||||
|
||||
for (unsigned i = 0; i < num_gyro; i++) {
|
||||
if (gyro_sub[i] < 0) {
|
||||
gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < num_gyro; i++) {
|
||||
fds[i].fd = gyro_sub[i];
|
||||
fds[i].events = POLLIN;
|
||||
}
|
||||
|
||||
// initialize data structures outside of loop
|
||||
// because they will else not always be
|
||||
// properly populated
|
||||
sensor_gyro_s gyro_data = {};
|
||||
//uint16_t l = 0;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
|
||||
|
||||
if (ret < 0) {
|
||||
// Poll error, sleep and try again
|
||||
usleep(10000);
|
||||
continue;
|
||||
|
||||
} else if (ret == 0) {
|
||||
// Poll timeout or no new data, do nothing
|
||||
continue;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < num_gyro; i++) {
|
||||
if (_hot_soaked[i]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (fds[i].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
|
||||
|
||||
gyro_sample_filt[i][0] = gyro_data.x;
|
||||
gyro_sample_filt[i][1] = gyro_data.y;
|
||||
gyro_sample_filt[i][2] = gyro_data.z;
|
||||
gyro_sample_filt[i][3] = gyro_data.temperature;
|
||||
|
||||
if (!_cold_soaked[i]) {
|
||||
_cold_soaked[i] = true;
|
||||
_low_temp[i] = gyro_sample_filt[i][3]; //Record the low temperature
|
||||
_ref_temp[i] = gyro_sample_filt[i][3] + 12.0f;
|
||||
}
|
||||
|
||||
num_samples[i]++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (uint8_t i = 0; i < 1; i++) {
|
||||
if (_hot_soaked[i]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
//if (num_samples[i] < 250) {
|
||||
// continue;
|
||||
//}
|
||||
|
||||
if (gyro_sample_filt[i][3] > _high_temp[i]) {
|
||||
_high_temp[i] = gyro_sample_filt[i][3];
|
||||
_hot_soak_sat[i] = 0;
|
||||
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
|
||||
//TODO: Hot Soak Saturation
|
||||
if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) {
|
||||
_hot_soaked[i] = true;
|
||||
}
|
||||
|
||||
if (i == 0) {
|
||||
TC_DEBUG("%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n", (double)gyro_sample_filt[i][0], (double)gyro_sample_filt[i][1],
|
||||
(double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3], (double)_low_temp[i], (double)_high_temp[i],
|
||||
(double)(_high_temp[i] - _low_temp[i]));
|
||||
}
|
||||
|
||||
//update linear fit matrices
|
||||
gyro_sample_filt[i][3] -= _ref_temp[i];
|
||||
P[i][0].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][0]);
|
||||
P[i][1].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][1]);
|
||||
P[i][2].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][2]);
|
||||
num_samples[i] = 0;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < 1; i++) {
|
||||
if (_hot_soaked[i] && !_tempcal_complete[i]) {
|
||||
double res[4];
|
||||
P[i][0].fit(res);
|
||||
PX4_WARN("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
|
||||
(double)res[3]);
|
||||
P[i][1].fit(res);
|
||||
PX4_WARN("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
|
||||
(double)res[3]);
|
||||
P[i][2].fit(res);
|
||||
PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
|
||||
(double)res[3]);
|
||||
_tempcal_complete[i] = true;
|
||||
}
|
||||
}
|
||||
|
||||
usleep(100);
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < num_gyro; i++) {
|
||||
orb_unsubscribe(gyro_sub[i]);
|
||||
}
|
||||
|
||||
delete tempcal::instance;
|
||||
tempcal::instance = nullptr;
|
||||
PX4_WARN("Tempcal process stopped");
|
||||
}
|
||||
|
||||
void Tempcal::do_temperature_calibration(int argc, char *argv[])
|
||||
{
|
||||
tempcal::instance->task_main();
|
||||
}
|
||||
|
||||
int Tempcal::start()
|
||||
{
|
||||
|
||||
ASSERT(_control_task == -1);
|
||||
_control_task = px4_task_spawn_cmd("temperature_calib",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
5800,
|
||||
(px4_main_t)&Tempcal::do_temperature_calibration,
|
||||
nullptr);
|
||||
|
||||
if (_control_task < 0) {
|
||||
delete tempcal::instance;
|
||||
tempcal::instance = nullptr;
|
||||
PX4_WARN("start failed");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void do_temperature_calibration(int argc, char *argv[])
|
||||
int run_temperature_calibration()
|
||||
{
|
||||
//TODO ...
|
||||
PX4_INFO("Starting Temperature calibration task");
|
||||
tempcal::instance = new Tempcal();
|
||||
|
||||
if (tempcal::instance == nullptr) {
|
||||
PX4_WARN("alloc failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return tempcal::instance->start();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user