yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

temperature_calibration: refactor to separate code & reduce code duplication

Browse Source
This commit is contained in:
Beat Küng
2017-02-02 20:11:28 +01:00
committed by Lorenz Meier
parent b6f3cf9425
commit c4a8aa9c68
11 changed files with 1122 additions and 1013 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/events/CMakeLists.txt
View File

@@ -38,9 +38,10 @@ px4_add_module(
COMPILE_FLAGS
SRCS
send_event.cpp
temperature_calibration/gyro.cpp
temperature_calibration/accel.cpp
temperature_calibration/baro.cpp
temperature_calibration/gyro.cpp
temperature_calibration/task.cpp
DEPENDS
platforms__common
modules__uORB

15
src/modules/events/send_event.cpp
View File

@@ -120,27 +120,32 @@ void SendEvent::process_commands()
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
bool got_temperature_calibration_command = false;
bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;
switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
if ((int)(cmd.param1) == 2) { //TODO: this (and the others) needs to be specified in mavlink...
run_temperature_gyro_calibration();
gyro = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param5) == 2) {
run_temperature_accel_calibration();
accel = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param7) == 2) {
run_temperature_baro_calibration();
baro = true;
got_temperature_calibration_command = true;
}
if (got_temperature_calibration_command) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
if (run_temperature_calibration(accel, baro, gyro) == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
}
break;

516
src/modules/events/temperature_calibration/accel.cpp
View File

@@ -32,381 +32,197 @@
****************************************************************************/
/**
* @file temperature_accel_calibration.cpp
* Implementation of the Temperature Calibration for onboard accelerometer sensors.
* @file accel.cpp
* Implementation of the Accel Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Paul Riseborough
* @author Beat Küng <beat-kueng@gmx.net>
*/
#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 <drivers/drv_accel.h>
#include <controllib/uorb/blocks.hpp>
#include "accel.h"
#include <uORB/topics/sensor_accel.h>
#include "polyfit.hpp"
#include "temperature_calibration.h"
#include <mathlib/mathlib.h>
#define TC_PRINT_DEBUG 0
#if TC_PRINT_DEBUG
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define TC_DEBUG(fmt, ...)
#endif
#define SENSOR_COUNT_MAX 3
extern "C" __EXPORT int tempcal_main(int argc, char *argv[]);
class Tempcalaccel;
namespace tempcalaccel
{
Tempcalaccel *instance = nullptr;
}
class Tempcalaccel : public control::SuperBlock
{
public:
/**
* Constructor
*/
Tempcalaccel();
/**
* Destructor, also kills task.
*/
~Tempcalaccel();
/**
* Start task.
*
* @return OK on success.
*/
int start();
static void do_temperature_accel_calibration(int argc, char *argv[]);
void task_main();
void print_status();
void exit() { _force_task_exit = true; }
private:
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
};
Tempcalaccel::Tempcalaccel():
SuperBlock(NULL, "Tempcalaccel")
{
}
Tempcalaccel::~Tempcalaccel()
TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise)
: TemperatureCalibrationBase(min_temperature_rise)
{
}
//init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_accel));
void Tempcalaccel::task_main()
{
// subscribe to relevant topics
int accel_sub[SENSOR_COUNT_MAX];
float accel_sample_filt[SENSOR_COUNT_MAX][4];
polyfitter<4> P[SENSOR_COUNT_MAX][3];
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
unsigned hot_soak_sat[SENSOR_COUNT_MAX] = {};
unsigned num_accel = orb_group_count(ORB_ID(sensor_accel));
unsigned num_samples[SENSOR_COUNT_MAX] = {0};
uint32_t device_ids[SENSOR_COUNT_MAX] = {};
int param_set_result = PX4_OK;
char param_str[30];
int num_completed = 0; // number of completed sensors
if (num_accel > SENSOR_COUNT_MAX) {
num_accel = SENSOR_COUNT_MAX;
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
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_accel; i++) {
accel_sub[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
fds[i].fd = accel_sub[i];
fds[i].events = POLLIN;
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
}
}
// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_accel_s accel_data = {};
void TemperatureCalibrationAccel::reset_calibration()
{
/* reset all driver level calibrations */
float offset = 0.0f;
float scale = 1.0f;
for (unsigned s = 0; s < num_accel; s++) {
(void)sprintf(param_str, "CAL_ACC%u_XOFF", s);
param_set_result = param_set_no_notification(param_find(param_str), &offset);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_ACC%u_YOFF", s);
param_set_result = param_set_no_notification(param_find(param_str), &offset);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_ACC%u_ZOFF", s);
param_set_result = param_set_no_notification(param_find(param_str), &offset);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_ACC%u_XSCALE", s);
param_set_result = param_set_no_notification(param_find(param_str), &scale);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_ACC%u_YSCALE", s);
param_set_result = param_set_no_notification(param_find(param_str), &scale);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_ACC%u_ZSCALE", s);
param_set_result = param_set_no_notification(param_find(param_str), &scale);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_ACC%u_XOFF", s, &offset);
set_parameter("CAL_ACC%u_YOFF", s, &offset);
set_parameter("CAL_ACC%u_ZOFF", s, &offset);
set_parameter("CAL_ACC%u_XSCALE", s, &scale);
set_parameter("CAL_ACC%u_YSCALE", s, &scale);
set_parameter("CAL_ACC%u_ZSCALE", s, &scale);
}
// get required temperature swing
int32_t min_temp_rise = 24;
param_get(param_find("SYS_CAL_TEMP"), &min_temp_rise);
while (!_force_task_exit) {
int ret = px4_poll(fds, num_accel, 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 (unsigned uorb_index = 0; uorb_index < num_accel; uorb_index++) {
if (hot_soaked[uorb_index]) {
continue;
}
if (fds[uorb_index].revents & POLLIN) {
orb_copy(ORB_ID(sensor_accel), accel_sub[uorb_index], &accel_data);
device_ids[uorb_index] = accel_data.device_id;
accel_sample_filt[uorb_index][0] = accel_data.x;
accel_sample_filt[uorb_index][1] = accel_data.y;
accel_sample_filt[uorb_index][2] = accel_data.z;
accel_sample_filt[uorb_index][3] = accel_data.temperature;
if (!cold_soaked[uorb_index]) {
cold_soaked[uorb_index] = true;
low_temp[uorb_index] = accel_sample_filt[uorb_index][3]; //Record the low temperature
ref_temp[uorb_index] = accel_sample_filt[uorb_index][3] + 0.5f * (float)min_temp_rise;
}
num_samples[uorb_index]++;
}
}
for (unsigned sensor_index = 0; sensor_index < num_accel; sensor_index++) {
if (hot_soaked[sensor_index]) {
continue;
}
if (accel_sample_filt[sensor_index][3] > high_temp[sensor_index]) {
high_temp[sensor_index] = accel_sample_filt[sensor_index][3];
hot_soak_sat[sensor_index] = 0;
} else {
continue;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (hot_soak_sat[sensor_index] == 10 || (high_temp[sensor_index] - low_temp[sensor_index]) > float(min_temp_rise)) {
hot_soaked[sensor_index] = true;
}
if (sensor_index == 0) {
TC_DEBUG("\n%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)accel_sample_filt[sensor_index][0],
(double)accel_sample_filt[sensor_index][1],
(double)accel_sample_filt[sensor_index][2], (double)accel_sample_filt[sensor_index][3], (double)low_temp[sensor_index], (double)high_temp[sensor_index],
(double)(high_temp[sensor_index] - low_temp[sensor_index]));
}
//update linear fit matrices
accel_sample_filt[sensor_index][3] -= ref_temp[sensor_index];
P[sensor_index][0].update((double)accel_sample_filt[sensor_index][3], (double)accel_sample_filt[sensor_index][0]);
P[sensor_index][1].update((double)accel_sample_filt[sensor_index][3], (double)accel_sample_filt[sensor_index][1]);
P[sensor_index][2].update((double)accel_sample_filt[sensor_index][3], (double)accel_sample_filt[sensor_index][2]);
num_samples[sensor_index] = 0;
}
for (unsigned sensor_index = 0; sensor_index < num_accel; sensor_index++) {
if (hot_soaked[sensor_index] && !tempcal_complete[sensor_index]) {
double res[3][4] = {0.0f};
P[sensor_index][0].fit(res[0]);
res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_WARN("Result Accel %u Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1], (double)res[0][2],
(double)res[0][3]);
P[sensor_index][1].fit(res[1]);
res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_WARN("Result Accel %u Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1], (double)res[1][2],
(double)res[1][3]);
P[sensor_index][2].fit(res[2]);
res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_WARN("Result Accel %u Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1], (double)res[2][2],
(double)res[2][3]);
tempcal_complete[sensor_index] = true;
++num_completed;
float param = 0.0f;
sprintf(param_str, "TC_A%d_ID", sensor_index);
param_set_result = param_set_no_notification(param_find(param_str), &device_ids[sensor_index]);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(param_str, "TC_A%d_X%d_%d", sensor_index, (3-coef_index), axis_index);
param = (float)res[axis_index][coef_index];
param_set_result = param_set_no_notification(param_find(param_str), &param);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
}
sprintf(param_str, "TC_A%d_TMAX", sensor_index);
param = high_temp[sensor_index];
param_set_result = param_set_no_notification(param_find(param_str), &param);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
sprintf(param_str, "TC_A%d_TMIN", sensor_index);
param = low_temp[sensor_index];
param_set_result = param_set_no_notification(param_find(param_str), &param);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
sprintf(param_str, "TC_A%d_TREF", sensor_index);
param = ref_temp[sensor_index];
param_set_result = param_set_no_notification(param_find(param_str), &param);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
}
}
}
// Check if completed and enable use of the thermal compensation
if (num_completed >= num_accel) {
sprintf(param_str, "TC_A_ENABLE");
int32_t enabled = 1;
param_set_result = param_set(param_find(param_str), &enabled);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
// exit the while loop
break;
}
}
for (unsigned i = 0; i < num_accel; i++) {
orb_unsubscribe(accel_sub[i]);
}
delete tempcalaccel::instance;
tempcalaccel::instance = nullptr;
PX4_INFO("Exiting accel temperature calibration task");
}
void Tempcalaccel::do_temperature_accel_calibration(int argc, char *argv[])
int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
tempcalaccel::instance->task_main();
}
int Tempcalaccel::start()
{
ASSERT(_control_task == -1);
_control_task = px4_task_spawn_cmd("accel_temp_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
5800,
(px4_main_t)&Tempcalaccel::do_temperature_accel_calibration,
nullptr);
if (_control_task < 0) {
delete tempcalaccel::instance;
tempcalaccel::instance = nullptr;
PX4_ERR("start failed");
return -errno;
if (data.hot_soaked) {
// already done
return 0;
}
return 0;
}
bool updated;
orb_check(sensor_sub, &updated);
int run_temperature_accel_calibration()
{
PX4_INFO("Starting accel temperature calibration task");
tempcalaccel::instance = new Tempcalaccel();
if (tempcalaccel::instance == nullptr) {
PX4_ERR("alloc failed");
if (!updated) {
return 1;
}
return tempcalaccel::instance->start();
sensor_accel_s accel_data;
orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data);
data.device_id = accel_data.device_id;
data.sensor_sample_filt[0] = accel_data.x;
data.sensor_sample_filt[1] = accel_data.y;
data.sensor_sample_filt[2] = accel_data.z;
data.sensor_sample_filt[3] = accel_data.temperature;
if (!data.cold_soaked) {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; //Record the low temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
data.sensor_sample_filt[3] -= data.ref_temp;
data.P[0].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[0]);
data.P[1].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[1]);
data.P[2].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[2]);
return 1;
}
int TemperatureCalibrationAccel::update()
{
int num_not_complete = 0;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
num_not_complete += update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
}
if (num_not_complete > 0) {
// calculate progress
float min_diff = _min_temperature_rise;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
if (cur_diff < min_diff) {
min_diff = cur_diff;
}
}
return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
}
return 110;
}
int TemperatureCalibrationAccel::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[3][4] = {0.0f};
data.P[0].fit(res[0]);
res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
(double)res[0][2],
(double)res[0][3]);
data.P[1].fit(res[1]);
res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
(double)res[1][2],
(double)res[1][3]);
data.P[2].fit(res[2]);
res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
(double)res[2][2],
(double)res[2][3]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_A%d_ID", sensor_index, &data.device_id);
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
return 0;
}

87
src/modules/events/temperature_calibration/accel.h Normal file
View File

@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
#pragma once
#include "common.h"
#include "polyfit.hpp"
class TemperatureCalibrationAccel : public TemperatureCalibrationBase
{
public:
TemperatureCalibrationAccel(float min_temperature_rise);
virtual ~TemperatureCalibrationAccel() {}
/**
* @see TemperatureCalibrationBase::update()
*/
int update();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
struct PerSensorData {
float sensor_sample_filt[4];
polyfitter<4> P[3];
unsigned hot_soak_sat = 0;
uint32_t device_id = 0;
bool cold_soaked = false;
bool hot_soaked = false;
bool tempcal_complete = false;
float low_temp = 0.f;
float high_temp = 0.f;
float ref_temp = 0.f;
};
PerSensorData _data[SENSOR_COUNT_MAX];
/**
* update a single sensor instance
* @return 0 when done, 1 not finished yet
*/
inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
int _num_sensor_instances;
int _sensor_subs[SENSOR_COUNT_MAX];
};

440
src/modules/events/temperature_calibration/baro.cpp
View File

@@ -32,329 +32,169 @@
****************************************************************************/
/**
* @file temperature_baro_calibration.cpp
* Implementation of the Temperature Calibration for onboard baroerometer sensors.
* @file baro.cpp
* Implementation of the Baro Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Paul Riseborough
* @author Beat Küng <beat-kueng@gmx.net>
*/
#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 <drivers/drv_baro.h>
#include <controllib/uorb/blocks.hpp>
#include "baro.h"
#include <uORB/topics/sensor_baro.h>
#include "polyfit.hpp"
#include "temperature_calibration.h"
#include <mathlib/mathlib.h>
#define TC_PRINT_DEBUG 0
#if TC_PRINT_DEBUG
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define TC_DEBUG(fmt, ...)
#endif
#define SENSOR_COUNT_MAX 3
#define POLYFIT_ORDER 5
extern "C" __EXPORT int tempcal_main(int argc, char *argv[]);
class Tempcalbaro;
namespace tempcalbaro
{
Tempcalbaro *instance = nullptr;
}
class Tempcalbaro : public control::SuperBlock
{
public:
/**
* Constructor
*/
Tempcalbaro();
/**
* Destructor, also kills task.
*/
~Tempcalbaro();
/**
* Start task.
*
* @return OK on success.
*/
int start();
static void do_temperature_baro_calibration(int argc, char *argv[]);
void task_main();
void print_status();
void exit() { _force_task_exit = true; }
private:
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
};
Tempcalbaro::Tempcalbaro():
SuperBlock(NULL, "Tempcalbaro")
{
}
Tempcalbaro::~Tempcalbaro()
TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise)
: TemperatureCalibrationBase(min_temperature_rise)
{
}
//init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
void Tempcalbaro::task_main()
{
// subscribe to relevant topics
int baro_sub[SENSOR_COUNT_MAX];
float baro_sample_filt[SENSOR_COUNT_MAX][2];
polyfitter<POLYFIT_ORDER+1> P[SENSOR_COUNT_MAX];
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
unsigned hot_soak_sat[SENSOR_COUNT_MAX] = {};
unsigned num_baro = orb_group_count(ORB_ID(sensor_baro));
unsigned num_samples[SENSOR_COUNT_MAX] = {0};
uint32_t device_ids[SENSOR_COUNT_MAX] = {};
if (num_baro > SENSOR_COUNT_MAX) {
num_baro = SENSOR_COUNT_MAX;
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
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_baro; i++) {
baro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
fds[i].fd = baro_sub[i];
fds[i].events = POLLIN;
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
}
// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_baro_s baro_data = {};
int param_set_result;
char param_str[30];
int num_completed = 0; // number of completed sensors
// get required temperature swing
int32_t min_temp_rise = 24;
param_get(param_find("SYS_CAL_TEMP"), &min_temp_rise);
while (!_force_task_exit) {
int ret = px4_poll(fds, num_baro, 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 (unsigned uorb_index = 0; uorb_index < num_baro; uorb_index++) {
if (hot_soaked[uorb_index]) {
continue;
}
if (fds[uorb_index].revents & POLLIN) {
orb_copy(ORB_ID(sensor_baro), baro_sub[uorb_index], &baro_data);
device_ids[uorb_index] = baro_data.device_id;
baro_sample_filt[uorb_index][0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
baro_sample_filt[uorb_index][1] = baro_data.temperature;
if (!cold_soaked[uorb_index]) {
cold_soaked[uorb_index] = true;
low_temp[uorb_index] = baro_sample_filt[uorb_index][1]; //Record the low temperature
ref_temp[uorb_index] = baro_sample_filt[uorb_index][1] + 0.5f * (float)min_temp_rise;
}
num_samples[uorb_index]++;
}
}
for (unsigned sensor_index = 0; sensor_index < num_baro; sensor_index++) {
if (hot_soaked[sensor_index]) {
continue;
}
if (baro_sample_filt[sensor_index][1] > high_temp[sensor_index]) {
high_temp[sensor_index] = baro_sample_filt[sensor_index][1];
hot_soak_sat[sensor_index] = 0;
} else {
continue;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (hot_soak_sat[sensor_index] == 10 || (high_temp[sensor_index] - low_temp[sensor_index]) > (float)min_temp_rise) {
hot_soaked[sensor_index] = true;
}
if (sensor_index == 0) {
TC_DEBUG("\n%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)baro_sample_filt[sensor_index][0],
(double)baro_sample_filt[sensor_index][1], (double)low_temp[sensor_index], (double)high_temp[sensor_index],
(double)(high_temp[sensor_index] - low_temp[sensor_index]));
}
//update linear fit matrices
baro_sample_filt[sensor_index][1] -= ref_temp[sensor_index];
P[sensor_index].update((double)baro_sample_filt[sensor_index][1], (double)baro_sample_filt[sensor_index][0]);
num_samples[sensor_index] = 0;
}
for (unsigned sensor_index = 0; sensor_index < num_baro; sensor_index++) {
if (hot_soaked[sensor_index] && !tempcal_complete[sensor_index]) {
double res[POLYFIT_ORDER+1] = {0.0f};
P[sensor_index].fit(res);
res[POLYFIT_ORDER] = 0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero
PX4_WARN("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0], (double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
tempcal_complete[sensor_index] = true;
++num_completed;
float param_val = 0.0f;
sprintf(param_str, "TC_B%d_ID", sensor_index);
param_set_result = param_set_no_notification(param_find(param_str), &device_ids[sensor_index]);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
sprintf(param_str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
param_val = (float)res[coef_index];
param_set_result = param_set_no_notification(param_find(param_str), &param_val);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
}
sprintf(param_str, "TC_B%d_TMAX", sensor_index);
param_val = high_temp[sensor_index];
param_set_result = param_set_no_notification(param_find(param_str), &param_val);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
sprintf(param_str, "TC_B%d_TMIN", sensor_index);
param_val = low_temp[sensor_index];
param_set_result = param_set_no_notification(param_find(param_str), &param_val);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
sprintf(param_str, "TC_B%d_TREF", sensor_index);
param_val = ref_temp[sensor_index];
param_set_result = param_set_no_notification(param_find(param_str), &param_val);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
}
}
// Check if completed and enable use of the thermal compensation
if (num_completed >= num_baro) {
sprintf(param_str, "TC_B_ENABLE");
int32_t enabled = 1;
param_set_result = param_set(param_find(param_str), &enabled);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
break;
}
}
for (unsigned i = 0; i < num_baro; i++) {
orb_unsubscribe(baro_sub[i]);
}
delete tempcalbaro::instance;
tempcalbaro::instance = nullptr;
PX4_INFO("Exiting baro temperature calibration task");
}
void Tempcalbaro::do_temperature_baro_calibration(int argc, char *argv[])
void TemperatureCalibrationBaro::reset_calibration()
{
tempcalbaro::instance->task_main();
//nothing to do
}
int Tempcalbaro::start()
int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
ASSERT(_control_task == -1);
_control_task = px4_task_spawn_cmd("baro_temp_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
5800,
(px4_main_t)&Tempcalbaro::do_temperature_baro_calibration,
nullptr);
if (_control_task < 0) {
delete tempcalbaro::instance;
tempcalbaro::instance = nullptr;
PX4_ERR("start failed");
return -errno;
if (data.hot_soaked) {
// already done
return 0;
}
return 0;
}
bool updated;
orb_check(sensor_sub, &updated);
int run_temperature_baro_calibration()
{
PX4_INFO("Starting baro temperature calibration task");
tempcalbaro::instance = new Tempcalbaro();
if (tempcalbaro::instance == nullptr) {
PX4_ERR("alloc failed");
if (!updated) {
return 1;
}
return tempcalbaro::instance->start();
sensor_baro_s baro_data;
orb_copy(ORB_ID(sensor_baro), sensor_sub, &baro_data);
data.device_id = baro_data.device_id;
data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
data.sensor_sample_filt[1] = baro_data.temperature;
if (!data.cold_soaked) {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[1]; //Record the low temperature
data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise;
}
// check if temperature increased
if (data.sensor_sample_filt[1] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[1];
data.hot_soak_sat = 0;
} else {
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nBaro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
data.sensor_sample_filt[1] -= data.ref_temp;
data.P.update((double)data.sensor_sample_filt[1], (double)data.sensor_sample_filt[0]);
return 1;
}
int TemperatureCalibrationBaro::update()
{
int num_not_complete = 0;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
num_not_complete += update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
}
if (num_not_complete > 0) {
// calculate progress
float min_diff = _min_temperature_rise;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
if (cur_diff < min_diff) {
min_diff = cur_diff;
}
}
return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
}
return 110;
}
int TemperatureCalibrationBaro::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_B_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_B_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[POLYFIT_ORDER + 1] = {};
data.P.fit(res);
res[POLYFIT_ORDER] =
0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero
PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0],
(double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_B%d_ID", sensor_index, &data.device_id);
for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
param = (float)res[coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
return 0;
}

90
src/modules/events/temperature_calibration/baro.h Normal file
View File

@@ -0,0 +1,90 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
#pragma once
#include "common.h"
#include "polyfit.hpp"
#define POLYFIT_ORDER 5
class TemperatureCalibrationBaro : public TemperatureCalibrationBase
{
public:
TemperatureCalibrationBaro(float min_temperature_rise);
virtual ~TemperatureCalibrationBaro() {}
/**
* @see TemperatureCalibrationBase::update()
*/
int update();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
struct PerSensorData {
float sensor_sample_filt[2];
polyfitter < POLYFIT_ORDER + 1 > P;
unsigned hot_soak_sat = 0;
uint32_t device_id = 0;
bool cold_soaked = false;
bool hot_soaked = false;
bool tempcal_complete = false;
float low_temp = 0.f;
float high_temp = 0.f;
float ref_temp = 0.f;
};
PerSensorData _data[SENSOR_COUNT_MAX];
/**
* update a single sensor instance
* @return 0 when done, 1 not finished yet
*/
inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
int _num_sensor_instances;
int _sensor_subs[SENSOR_COUNT_MAX];
};

101
src/modules/events/temperature_calibration/common.h Normal file
View File

@@ -0,0 +1,101 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
#pragma once
#define TC_PRINT_DEBUG 0
#if TC_PRINT_DEBUG
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define TC_DEBUG(fmt, ...)
#endif
#include <px4_log.h>
#define SENSOR_COUNT_MAX 3
/**
* Base class for temperature calibration types (for all different sensor types)
*/
class TemperatureCalibrationBase
{
public:
TemperatureCalibrationBase(float min_temperature_rise)
: _min_temperature_rise(min_temperature_rise) {}
virtual ~TemperatureCalibrationBase() {}
/**
* check & update new sensor data.
* @return progress in range [0, 100], 110 when finished, <0 on error
*/
virtual int update() = 0;
/**
* do final fitting & write the parameters. Call this exactly once after update() returned 110
* @return 0 on success, <0 otherwise
*/
virtual int finish() = 0;
/** reset all driver-level calibration parameters */
virtual void reset_calibration() = 0;
protected:
/**
* set a system parameter (without system notification) and print an error if it fails
* @param format_str for example "CAL_GYRO%u_XOFF"
* @param index which index (will replace %u in format_str)
* @param value
* @return 0 on success
*/
inline int set_parameter(const char *format_str, unsigned index, const void *value);
float _min_temperature_rise; ///< minimum difference in temperature before the process finishes
};
int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value)
{
char param_str[30];
(void)sprintf(param_str, format_str, index);
int result = param_set_no_notification(param_find(param_str), value);
if (result != 0) {
PX4_ERR("unable to reset %s (%i)", param_str, result);
}
return result;
}

502
src/modules/events/temperature_calibration/gyro.cpp
View File

@@ -32,377 +32,183 @@
****************************************************************************/
/**
* @file temperature_gyro_calibration.cpp
* Implementation of the Temperature Calibration for onboard sensors.
* @file gyro.cpp
* Implementation of the Gyro Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Beat Küng <beat-kueng@gmx.net>
*/
#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 <drivers/drv_gyro.h>
#include <controllib/uorb/blocks.hpp>
#include <uORB/topics/sensor_gyro.h>
#include "polyfit.hpp"
#include "temperature_calibration.h"
#include "gyro.h"
#define TC_PRINT_DEBUG 0
#if TC_PRINT_DEBUG
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define TC_DEBUG(fmt, ...)
#endif
#define SENSOR_COUNT_MAX 3
extern "C" __EXPORT int tempcalgyro_main(int argc, char *argv[]);
class Tempcalgyro;
namespace tempcalgyro
{
Tempcalgyro *instance = nullptr;
}
class Tempcalgyro : public control::SuperBlock
{
public:
/**
* Constructor
*/
Tempcalgyro();
/**
* Destructor, also kills task.
*/
~Tempcalgyro();
/**
* 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() { _force_task_exit = true; }
private:
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
};
Tempcalgyro::Tempcalgyro():
SuperBlock(NULL, "Tempcalgyro")
{
}
Tempcalgyro::~Tempcalgyro()
TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, int gyro_subs[], int num_gyros)
: TemperatureCalibrationBase(min_temperature_rise), _num_sensor_instances(num_gyros), _sensor_subs(gyro_subs)
{
}
void Tempcalgyro::task_main()
void TemperatureCalibrationGyro::reset_calibration()
{
// 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] = {};
unsigned hot_soak_sat[SENSOR_COUNT_MAX] = {};
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
unsigned num_samples[SENSOR_COUNT_MAX] = {0};
uint32_t device_ids[SENSOR_COUNT_MAX] = {};
int param_set_result;
char param_str[30];
int num_completed = 0; // number of completed gyros
if (num_gyro > SENSOR_COUNT_MAX) {
num_gyro = SENSOR_COUNT_MAX;
}
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++) {
gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_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 = {};
/* reset all driver level calibrations */
float offset = 0.0f;
float scale = 1.0f;
for (unsigned s = 0; s < num_gyro; s++) {
(void)sprintf(param_str, "CAL_GYRO%u_XOFF", s);
param_set_result = param_set_no_notification(param_find(param_str), &offset);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_GYRO%u_YOFF", s);
param_set_result = param_set_no_notification(param_find(param_str), &offset);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_GYRO%u_ZOFF", s);
param_set_result = param_set_no_notification(param_find(param_str), &offset);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_GYRO%u_XSCALE", s);
param_set_result = param_set_no_notification(param_find(param_str), &scale);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_GYRO%u_YSCALE", s);
param_set_result = param_set_no_notification(param_find(param_str), &scale);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
(void)sprintf(param_str, "CAL_GYRO%u_ZSCALE", s);
param_set_result = param_set_no_notification(param_find(param_str), &scale);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_GYRO%u_XOFF", s, &offset);
set_parameter("CAL_GYRO%u_YOFF", s, &offset);
set_parameter("CAL_GYRO%u_ZOFF", s, &offset);
set_parameter("CAL_GYRO%u_XSCALE", s, &scale);
set_parameter("CAL_GYRO%u_YSCALE", s, &scale);
set_parameter("CAL_GYRO%u_ZSCALE", s, &scale);
}
int32_t min_temp_rise = 24;
param_get(param_find("SYS_CAL_TEMP"), &min_temp_rise);
while (!_force_task_exit) {
int ret = px4_poll(fds, num_gyro, 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 (unsigned uorb_index = 0; uorb_index < num_gyro; uorb_index++) {
if (hot_soaked[uorb_index]) {
continue;
}
if (fds[uorb_index].revents & POLLIN) {
orb_copy(ORB_ID(sensor_gyro), gyro_sub[uorb_index], &gyro_data);
device_ids[uorb_index] = gyro_data.device_id;
gyro_sample_filt[uorb_index][0] = gyro_data.x;
gyro_sample_filt[uorb_index][1] = gyro_data.y;
gyro_sample_filt[uorb_index][2] = gyro_data.z;
gyro_sample_filt[uorb_index][3] = gyro_data.temperature;
if (!cold_soaked[uorb_index]) {
cold_soaked[uorb_index] = true;
low_temp[uorb_index] = gyro_sample_filt[uorb_index][3]; //Record the low temperature
ref_temp[uorb_index] = gyro_sample_filt[uorb_index][3] + 0.5f * (float)min_temp_rise;
}
num_samples[uorb_index]++;
}
}
for (unsigned sensor_index = 0; sensor_index < num_gyro; sensor_index++) {
if (hot_soaked[sensor_index]) {
continue;
}
if (gyro_sample_filt[sensor_index][3] > high_temp[sensor_index]) {
high_temp[sensor_index] = gyro_sample_filt[sensor_index][3];
hot_soak_sat[sensor_index] = 0;
} else {
continue;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (hot_soak_sat[sensor_index] == 10 || (high_temp[sensor_index] - low_temp[sensor_index]) > (float)min_temp_rise) {
hot_soaked[sensor_index] = true;
}
if (sensor_index == 0) {
TC_DEBUG("\n%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)gyro_sample_filt[sensor_index][0],
(double)gyro_sample_filt[sensor_index][1],
(double)gyro_sample_filt[sensor_index][2], (double)gyro_sample_filt[sensor_index][3], (double)low_temp[sensor_index], (double)high_temp[sensor_index],
(double)(high_temp[sensor_index] - low_temp[sensor_index]));
}
//update linear fit matrices
gyro_sample_filt[sensor_index][3] -= ref_temp[sensor_index];
P[sensor_index][0].update((double)gyro_sample_filt[sensor_index][3], (double)gyro_sample_filt[sensor_index][0]);
P[sensor_index][1].update((double)gyro_sample_filt[sensor_index][3], (double)gyro_sample_filt[sensor_index][1]);
P[sensor_index][2].update((double)gyro_sample_filt[sensor_index][3], (double)gyro_sample_filt[sensor_index][2]);
num_samples[sensor_index] = 0;
}
for (unsigned sensor_index = 0; sensor_index < num_gyro; sensor_index++) {
if (hot_soaked[sensor_index] && !tempcal_complete[sensor_index]) {
double res[3][4] = {0.0f};
P[sensor_index][0].fit(res[0]);
PX4_WARN("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1], (double)res[0][2],
(double)res[0][3]);
P[sensor_index][1].fit(res[1]);
PX4_WARN("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1], (double)res[1][2],
(double)res[1][3]);
P[sensor_index][2].fit(res[2]);
PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1], (double)res[2][2],
(double)res[2][3]);
tempcal_complete[sensor_index] = true;
++num_completed;
char str[30];
float param = 0.0f;
int result = PX4_OK;
sprintf(str, "TC_G%d_ID", sensor_index);
result = param_set(param_find(str), &device_ids[sensor_index]);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_G%d_X%d_%d", sensor_index, (3-coef_index), axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
sprintf(str, "TC_G%d_TMAX", sensor_index);
param = high_temp[sensor_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
sprintf(str, "TC_G%d_TMIN", sensor_index);
param = low_temp[sensor_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
sprintf(str, "TC_G%d_TREF", sensor_index);
param = ref_temp[sensor_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
}
// Check if completed and enable use of the thermal compensation
if (num_completed >= num_gyro) {
sprintf(param_str, "TC_G_ENABLE");
int32_t enabled = 1;
param_set_result = param_set(param_find(param_str), &enabled);
if (param_set_result != PX4_OK) {
PX4_ERR("unable to reset %s", param_str);
}
break;
}
}
for (unsigned i = 0; i < num_gyro; i++) {
orb_unsubscribe(gyro_sub[i]);
}
delete tempcalgyro::instance;
tempcalgyro::instance = nullptr;
PX4_INFO("Exiting gyro temperature calibration task");
}
void Tempcalgyro::do_temperature_calibration(int argc, char *argv[])
int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
tempcalgyro::instance->task_main();
}
int Tempcalgyro::start()
{
ASSERT(_control_task == -1);
_control_task = px4_task_spawn_cmd("temperature_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
5800,
(px4_main_t)&Tempcalgyro::do_temperature_calibration,
nullptr);
if (_control_task < 0) {
delete tempcalgyro::instance;
tempcalgyro::instance = nullptr;
PX4_ERR("start failed");
return -errno;
if (data.hot_soaked) {
// already done
return 0;
}
return 0;
}
bool updated;
orb_check(sensor_sub, &updated);
int run_temperature_gyro_calibration()
{
PX4_INFO("Starting gyro temperature calibration task");
tempcalgyro::instance = new Tempcalgyro();
if (tempcalgyro::instance == nullptr) {
PX4_ERR("alloc failed");
if (!updated) {
return 1;
}
return tempcalgyro::instance->start();
sensor_gyro_s gyro_data;
orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data);
data.device_id = gyro_data.device_id;
data.sensor_sample_filt[0] = gyro_data.x;
data.sensor_sample_filt[1] = gyro_data.y;
data.sensor_sample_filt[2] = gyro_data.z;
data.sensor_sample_filt[3] = gyro_data.temperature;
if (!data.cold_soaked) {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; //Record the low temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nGyro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
data.sensor_sample_filt[3] -= data.ref_temp;
data.P[0].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[0]);
data.P[1].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[1]);
data.P[2].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[2]);
return 1;
}
int TemperatureCalibrationGyro::update()
{
int num_not_complete = 0;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
num_not_complete += update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
}
if (num_not_complete > 0) {
// calculate progress
float min_diff = _min_temperature_rise;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
if (cur_diff < min_diff) {
min_diff = cur_diff;
}
}
return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
}
return 110;
}
int TemperatureCalibrationGyro::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_G_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_G_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[3][4] = {0.0f};
data.P[0].fit(res[0]);
PX4_INFO("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
(double)res[0][2],
(double)res[0][3]);
data.P[1].fit(res[1]);
PX4_INFO("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
(double)res[1][2],
(double)res[1][3]);
data.P[2].fit(res[2]);
PX4_INFO("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
(double)res[2][2],
(double)res[2][3]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_G%d_ID", sensor_index, &data.device_id);
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
set_parameter("TC_G%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp);
return 0;
}

87
src/modules/events/temperature_calibration/gyro.h Normal file
View File

@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
#pragma once
#include "common.h"
#include "polyfit.hpp"
class TemperatureCalibrationGyro : public TemperatureCalibrationBase
{
public:
TemperatureCalibrationGyro(float min_temperature_rise, int gyro_subs[], int num_gyros);
virtual ~TemperatureCalibrationGyro() {}
/**
* @see TemperatureCalibrationBase::update()
*/
int update();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
struct PerSensorData {
float sensor_sample_filt[4];
polyfitter<4> P[3];
unsigned hot_soak_sat = 0;
uint32_t device_id = 0;
bool cold_soaked = false;
bool hot_soaked = false;
bool tempcal_complete = false;
float low_temp = 0.f;
float high_temp = 0.f;
float ref_temp = 0.f;
};
PerSensorData _data[SENSOR_COUNT_MAX];
/**
* update a single sensor instance
* @return 0 when done, 1 not finished yet
*/
inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
int _num_sensor_instances;
int *_sensor_subs;
};

283
src/modules/events/temperature_calibration/task.cpp Normal file
View File

@@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (c) 2017 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 task.cpp
*
* Main task handling the temperature calibration process
*
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <uORB/topics/sensor_gyro.h>
#include <mathlib/mathlib.h>
#include <px4_log.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <drivers/drv_hrt.h>
#include "common.h"
#include "temperature_calibration.h"
#include "accel.h"
#include "baro.h"
#include "gyro.h"
class TemperatureCalibration;
namespace temperature_calibration
{
TemperatureCalibration *instance = nullptr;
}
class TemperatureCalibration
{
public:
/**
* Constructor
*/
TemperatureCalibration(bool accel, bool baro, bool gyro);
/**
* Destructor
*/
~TemperatureCalibration();
/**
* Start task.
*
* @return OK on success.
*/
int start();
static void do_temperature_calibration(int argc, char *argv[]);
void task_main();
void exit() { _force_task_exit = true; }
private:
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
bool _accel; ///< enable accel calibration?
bool _baro; ///< enable baro calibration?
bool _gyro; ///< enable gyro calibration?
};
TemperatureCalibration::TemperatureCalibration(bool accel, bool baro, bool gyro)
: _accel(accel), _baro(baro), _gyro(gyro)
{
}
TemperatureCalibration::~TemperatureCalibration()
{
}
void TemperatureCalibration::task_main()
{
// subscribe to all gyro instances
int gyro_sub[SENSOR_COUNT_MAX];
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
if (num_gyro > SENSOR_COUNT_MAX) {
num_gyro = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < num_gyro; i++) {
gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
fds[i].fd = gyro_sub[i];
fds[i].events = POLLIN;
}
int32_t min_temp_rise = 24;
param_get(param_find("SYS_CAL_TEMP"), &min_temp_rise);
PX4_INFO("Waiting for %i degrees difference in sensor temperature", min_temp_rise);
//init calibrators
TemperatureCalibrationBase *calibrators[3];
int num_calibrators = 0;
if (_accel) {
calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
if (_baro) {
calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
if (_gyro) {
calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, gyro_sub, num_gyro);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
// reset params
for (int i = 0; i < num_calibrators; ++i) {
calibrators[i]->reset_calibration();
}
hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
while (!_force_task_exit) {
/* we poll on the gyro(s), since this is the sensor with the highest update rate.
* Each individual sensor will then check on its own if there's new data.
*/
int ret = px4_poll(fds, num_gyro, 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;
}
//if gyro is not enabled: we must do an orb_copy here, so that poll() does not immediately return again
if (!_gyro) {
sensor_gyro_s gyro_data;
for (int i = 0; i < num_gyro; ++i) {
orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
}
}
int min_progress = 110;
for (int i = 0; i < num_calibrators; ++i) {
ret = calibrators[i]->update();
if (ret < 0) {
PX4_ERR("Calibration update step failed (%i)", ret);
} else if (ret < min_progress) {
min_progress = ret;
}
}
if (min_progress == 110) {
break; // we are done
}
//print progress each second
hrt_abstime now = hrt_absolute_time();
if (now > next_progress_output) {
PX4_INFO("Calibration progress: %i%%", min_progress);
next_progress_output = now + 1e6;
}
}
// do final calculations & parameter storage
for (int i = 0; i < num_calibrators; ++i) {
int ret = calibrators[i]->finish();
if (ret < 0) {
PX4_ERR("Failed to finish calibration process (%i)", ret);
}
}
for (int i = 0; i < num_calibrators; ++i) {
delete calibrators[i];
}
for (unsigned i = 0; i < num_gyro; i++) {
orb_unsubscribe(gyro_sub[i]);
}
delete temperature_calibration::instance;
temperature_calibration::instance = nullptr;
PX4_INFO("Exiting temperature calibration task");
}
void TemperatureCalibration::do_temperature_calibration(int argc, char *argv[])
{
temperature_calibration::instance->task_main();
}
int TemperatureCalibration::start()
{
ASSERT(_control_task == -1);
_control_task = px4_task_spawn_cmd("temperature_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
5800,
(px4_main_t)&TemperatureCalibration::do_temperature_calibration,
nullptr);
if (_control_task < 0) {
delete temperature_calibration::instance;
temperature_calibration::instance = nullptr;
PX4_ERR("start failed");
return -errno;
}
return 0;
}
int run_temperature_calibration(bool accel, bool baro, bool gyro)
{
PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
temperature_calibration::instance = new TemperatureCalibration(accel, baro, gyro);
if (temperature_calibration::instance == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
return temperature_calibration::instance->start();
}

11
src/modules/events/temperature_calibration/temperature_calibration.h
View File

@@ -34,14 +34,7 @@
#pragma once
/** start gyro temperature calibration in a new task
/** start temperature calibration in a new task for one or multiple sensors
* @return 0 on success, <0 error otherwise */
int run_temperature_gyro_calibration();
int run_temperature_calibration(bool accel, bool baro, bool gyro);
/** start accel temperature calibration in a new task
* @return 0 on success, <0 error otherwise */
int run_temperature_accel_calibration();
/** start baro temperature calibration in a new task
* @return 0 on success, <0 error otherwise */
int run_temperature_baro_calibration();