diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index 32be35ab53..d1922f22ef 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -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 diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 2e5811d60d..59b9aeb64e 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -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"); diff --git a/src/modules/events/temperature_calibration.cpp b/src/modules/events/temperature_calibration.cpp deleted file mode 100644 index 8f1c8e2789..0000000000 --- a/src/modules/events/temperature_calibration.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include "polyfit.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), ¶m); - - 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), ¶m); - - 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), ¶m); - - 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), ¶m); - - 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(); -} diff --git a/src/modules/events/temperature_calibration.h b/src/modules/events/temperature_calibration.h index 837687ebeb..f5fa873f52 100644 --- a/src/modules/events/temperature_calibration.h +++ b/src/modules/events/temperature_calibration.h @@ -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(); diff --git a/src/modules/events/temperature_gyro_calibration.cpp b/src/modules/events/temperature_gyro_calibration.cpp new file mode 100644 index 0000000000..507eeb3809 --- /dev/null +++ b/src/modules/events/temperature_gyro_calibration.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "polyfit.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), ¶m); + + 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), ¶m); + + 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), ¶m); + + 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), ¶m); + + 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(); +}