diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index 1050916d5a..c56c550c7b 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -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 diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 397ddb2c2c..1c408881f8 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -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; diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/events/temperature_calibration/accel.cpp index 58b23abb27..ea31572f24 100644 --- a/src/modules/events/temperature_calibration/accel.cpp +++ b/src/modules/events/temperature_calibration/accel.cpp @@ -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 */ -#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 "accel.h" #include -#include "polyfit.hpp" -#include "temperature_calibration.h" +#include -#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), ¶m); - - 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), ¶m); - - 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), ¶m); - - 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), ¶m); - - 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), ¶m); + + 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; } diff --git a/src/modules/events/temperature_calibration/accel.h b/src/modules/events/temperature_calibration/accel.h new file mode 100644 index 0000000000..3a1d8bf276 --- /dev/null +++ b/src/modules/events/temperature_calibration/accel.h @@ -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]; +}; diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/events/temperature_calibration/baro.cpp index cc4b28cb77..bb5cfeb9ac 100644 --- a/src/modules/events/temperature_calibration/baro.cpp +++ b/src/modules/events/temperature_calibration/baro.cpp @@ -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 */ -#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 "baro.h" #include -#include "polyfit.hpp" -#include "temperature_calibration.h" +#include -#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 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), ¶m_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), ¶m_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), ¶m_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), ¶m_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), ¶m); + + 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; } diff --git a/src/modules/events/temperature_calibration/baro.h b/src/modules/events/temperature_calibration/baro.h new file mode 100644 index 0000000000..1a84e4b2e7 --- /dev/null +++ b/src/modules/events/temperature_calibration/baro.h @@ -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]; +}; diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/events/temperature_calibration/common.h new file mode 100644 index 0000000000..59672c1a2e --- /dev/null +++ b/src/modules/events/temperature_calibration/common.h @@ -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 + +#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; +} + diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/events/temperature_calibration/gyro.cpp index 36a9d1d7a5..390e84f77b 100644 --- a/src/modules/events/temperature_calibration/gyro.cpp +++ b/src/modules/events/temperature_calibration/gyro.cpp @@ -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 */ -#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" +#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), ¶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("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), ¶m); + + 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; } diff --git a/src/modules/events/temperature_calibration/gyro.h b/src/modules/events/temperature_calibration/gyro.h new file mode 100644 index 0000000000..1ac16b7d46 --- /dev/null +++ b/src/modules/events/temperature_calibration/gyro.h @@ -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; +}; diff --git a/src/modules/events/temperature_calibration/task.cpp b/src/modules/events/temperature_calibration/task.cpp new file mode 100644 index 0000000000..e1a22f497e --- /dev/null +++ b/src/modules/events/temperature_calibration/task.cpp @@ -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 + */ + +#include +#include +#include +#include +#include +#include + +#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(); +} diff --git a/src/modules/events/temperature_calibration/temperature_calibration.h b/src/modules/events/temperature_calibration/temperature_calibration.h index 273125cdd6..4d6dac61e7 100644 --- a/src/modules/events/temperature_calibration/temperature_calibration.h +++ b/src/modules/events/temperature_calibration/temperature_calibration.h @@ -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();