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

events: give gyro cal unique names in preparation for other sensor types

Browse Source
This commit is contained in:
Paul Riseborough 2017-02-02 08:52:39 +11:00 committed by Lorenz Meier
parent 9e80a6c9d6
commit f0c456dd54
5 changed files with 427 additions and 373 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,7 +38,7 @@ px4_add_module(
COMPILE_FLAGS
SRCS
send_event.cpp
temperature_calibration.cpp
temperature_gyro_calibration.cpp
DEPENDS
platforms__common
modules__uORB

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

@ -123,7 +123,23 @@ void SendEvent::process_commands()
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
if ((int)(cmd.param1) == 2) { //TODO: this needs to be specified in mavlink (and adjust commander accordingly)...
if (run_temperature_calibration() == 0) {
if (run_temperature_gyro_calibration() == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
} else if ((int)(cmd.param1) == 3) { //TODO: this needs to be specified in mavlink (and adjust commander accordingly)...
if (run_temperature_accel_calibration() == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
} else if ((int)(cmd.param1) == 4) { //TODO: this needs to be specified in mavlink (and adjust commander accordingly)...
if (run_temperature_baro_calibration() == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
@ -170,7 +186,7 @@ static void print_usage(const char *reason = nullptr)
PX4_INFO("usage: send_event {start_listening|stop_listening|status|temperature_calibration}\n"
"\tstart_listening: start background task to listen to events\n"
"\ttemperature_calibration: start temperature calibration task\n"
"\tstart_temp_gyro_cal: start gyro temperature calibration task\n"
);
}
@ -216,7 +232,7 @@ int send_event_main(int argc, char *argv[])
PX4_INFO("not running");
}
} else if (!strcmp(argv[1], "temperature_calibration")) {
} else if (!strcmp(argv[1], "start_temp_gyro_cal")) {
if (!send_event_obj) {
PX4_ERR("background task not running");

367
src/modules/events/temperature_calibration.cpp
View File

@ -1,367 +0,0 @@
/****************************************************************************
*
* 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 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 <drivers/drv_gyro.h>
#include <controllib/uorb/blocks.hpp>
#include <uORB/topics/sensor_gyro.h>
#include "polyfit.hpp"
#include "temperature_calibration.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 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 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
};
Tempcal::Tempcal():
SuperBlock(NULL, "Tempcal")
{
}
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] = {};
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 result;
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] = {};
bool hot_soaked[SENSOR_COUNT_MAX] = {};
bool tempcal_complete[SENSOR_COUNT_MAX] = {};
float low_temp[SENSOR_COUNT_MAX];
float high_temp[SENSOR_COUNT_MAX] = {};
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 = {};
while (!_task_should_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 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);
device_ids[i] = gyro_data.device_id;
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 (unsigned i = 0; i < num_gyro; i++) {
if (hot_soaked[i]) {
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("\n%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\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 (unsigned i = 0; i < num_gyro; i++) {
if (hot_soaked[i] && !tempcal_complete[i]) {
double res[3][4] = {0.0f};
P[i][0].fit(res[0]);
PX4_WARN("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", i, (double)res[0][0], (double)res[0][1], (double)res[0][2],
(double)res[0][3]);
P[i][1].fit(res[1]);
PX4_WARN("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", i, (double)res[1][0], (double)res[1][1], (double)res[1][2],
(double)res[1][3]);
P[i][2].fit(res[2]);
PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", i, (double)res[2][0], (double)res[2][1], (double)res[2][2],
(double)res[2][3]);
tempcal_complete[i] = true;
++num_completed;
char str[30];
float param = 0.0f;
sprintf(str, "TC_G%d_ID", i);
result = param_set_no_notification(param_find(str), &device_ids[i]);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
for (unsigned j = 0; j < 3; j++) {
for (unsigned m = 0; m <= 3; m++) {
sprintf(str, "TC_G%d_X%d_%d", i, 3 - m, j);
param = (float)res[j][m];
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", i);
param = high_temp[i];
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", i);
param = low_temp[i];
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", i);
param = ref_temp[i];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
if (num_completed == num_gyro) {
/* all gyros are done */
int32_t enabled = 1;
result = param_set(param_find("TC_G_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_G_ENABLE");
}
break;
}
}
for (unsigned i = 0; i < num_gyro; i++) {
orb_unsubscribe(gyro_sub[i]);
}
delete tempcal::instance;
tempcal::instance = nullptr;
PX4_INFO("Tempcal process exited");
}
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_ERR("start failed");
return -errno;
}
return 0;
}
int run_temperature_calibration()
{
PX4_INFO("Starting Temperature calibration task");
tempcal::instance = new Tempcal();
if (tempcal::instance == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
return tempcal::instance->start();
}

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

@ -34,7 +34,7 @@
#pragma once
/** start temperature calibration in a new task
/** start gyro temperature calibration in a new task
* @return 0 on success, <0 error otherwise */
int run_temperature_calibration();
int run_temperature_gyro_calibration();

405
src/modules/events/temperature_gyro_calibration.cpp Normal file
View File

@ -0,0 +1,405 @@
/****************************************************************************
*
* 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 temperature_gyro_calibration.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 <drivers/drv_gyro.h>
#include <controllib/uorb/blocks.hpp>
#include <uORB/topics/sensor_gyro.h>
#include "polyfit.hpp"
#include "temperature_calibration.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()
{
}
void Tempcalgyro::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] = {};
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);
}
}
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] + 12.0f;
}
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: Hot Soak Saturation
if (hot_soak_sat[sensor_index] == 10 || (high_temp[sensor_index] - low_temp[sensor_index]) > 24.0f) {
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("Tempcalgyro process exited");
}
void Tempcalgyro::do_temperature_calibration(int argc, char *argv[])
{
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;
}
return 0;
}
int run_temperature_gyro_calibration()
{
PX4_INFO("Starting Temperature calibration task");
tempcalgyro::instance = new Tempcalgyro();
if (tempcalgyro::instance == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
return tempcalgyro::instance->start();
}