diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index 0777ab919d..aef93c5f91 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -234,6 +234,19 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1); */ PARAM_DEFINE_INT32(SYS_HAS_BARO, 1); +/** + * Enable factory calibration mode + * + * If enabled, future sensor calibrations will be stored to /fs/mtd_caldata. + * + * Note: this is only supported on boards with a separate calibration storage + * /fs/mtd_caldata. + * + * @boolean + * @group System + */ +PARAM_DEFINE_INT32(SYS_FAC_CAL_MODE, 0); + /** * Bootloader update * diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 8fa82b6299..8a9314eb66 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -45,6 +45,7 @@ px4_add_module( Commander.cpp commander_helper.cpp esc_calibration.cpp + factory_calibration_storage.cpp gyro_calibration.cpp level_calibration.cpp mag_calibration.cpp diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index e6a14f9015..9cd0d09c6d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -124,6 +124,7 @@ #include "calibration_messages.h" #include "calibration_routines.h" #include "commander_helper.h" +#include "factory_calibration_storage.h" #include #include @@ -346,6 +347,13 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } + FactoryCalibrationStorage factory_storage; + + if (factory_storage.open() != PX4_OK) { + calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage"); + return PX4_ERROR; + } + /* measure and calculate offsets & scales */ accel_worker_data_t worker_data{}; worker_data.mavlink_log_pub = mavlink_log_pub; @@ -424,6 +432,10 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } } + if (!failed && factory_storage.store() != PX4_OK) { + failed = true; + } + if (param_save) { param_notify_changes(); } @@ -446,6 +458,13 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) bool param_save = false; bool failed = true; + FactoryCalibrationStorage factory_storage; + + if (factory_storage.open() != PX4_OK) { + calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage"); + return PX4_ERROR; + } + // sensor thermal corrections (optional) uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; sensor_correction_s sensor_correction{}; @@ -566,6 +585,10 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) } } + if (!failed && factory_storage.store() != PX4_OK) { + failed = true; + } + if (param_save) { param_notify_changes(); } diff --git a/src/modules/commander/factory_calibration_storage.cpp b/src/modules/commander/factory_calibration_storage.cpp new file mode 100644 index 0000000000..075b9b8ec5 --- /dev/null +++ b/src/modules/commander/factory_calibration_storage.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include +#include +#include + +#include +#include + +#include "factory_calibration_storage.h" + + +static const char *CALIBRATION_STORAGE = "/fs/mtd_caldata"; + +static bool filter_calibration_params(param_t handle) +{ + const char *name = param_name(handle); + // filter all non-calibration params + return strncmp(name, "CAL_", 4) == 0 || strncmp(name, "TC_", 3) == 0; +} + +FactoryCalibrationStorage::FactoryCalibrationStorage() +{ + int32_t param = 0; + param_get(param_find("SYS_FAC_CAL_MODE"), ¶m); + _enabled = param == 1; +} + +int FactoryCalibrationStorage::open() +{ + if (_fd >= 0) { + cleanup(); + } + + if (!_enabled) { + return 0; + } + + _fd = ::open(CALIBRATION_STORAGE, O_RDWR); + + if (_fd == -1) { + return -errno; + } + + PX4_INFO("Storing parameters to factory storage %s", CALIBRATION_STORAGE); + param_control_autosave(false); + return 0; +} + +int FactoryCalibrationStorage::store() +{ + if (!_enabled) { + return 0; + } + + int ret = param_export(_fd, false, filter_calibration_params); + + if (ret != 0) { + PX4_ERR("param export failed (%i)", ret); + } + + return ret; +} + +void FactoryCalibrationStorage::cleanup() +{ + if (_enabled) { + param_control_autosave(true); + } + + if (_fd >= 0) { + close(_fd); + _fd = -1; + } +} + diff --git a/src/modules/commander/factory_calibration_storage.h b/src/modules/commander/factory_calibration_storage.h new file mode 100644 index 0000000000..bfb75966d7 --- /dev/null +++ b/src/modules/commander/factory_calibration_storage.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + +/** + * @class FactoryCalibrationStorage + * Stores calibration parameters to a separate storage, if enabled by parameter + */ +class FactoryCalibrationStorage +{ +public: + FactoryCalibrationStorage(); + ~FactoryCalibrationStorage() { cleanup(); } + + /** + * open the storage & disable param autosaving + * @return 0 on success, <0 error otherwise + */ + int open(); + + /** + * store the calibration parameters + * Note: this method requires a lot of stack + * @return 0 on success, <0 error otherwise + */ + int store(); + +private: + void cleanup(); + + bool _enabled{false}; + int _fd{-1}; +}; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 3e7bcd0c7f..8362d5f5f6 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -38,6 +38,7 @@ */ #include +#include "factory_calibration_storage.h" #include "gyro_calibration.h" #include "calibration_messages.h" #include "calibration_routines.h" @@ -289,6 +290,13 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) res = PX4_ERROR; } + FactoryCalibrationStorage factory_storage; + + if (factory_storage.open() != PX4_OK) { + calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage"); + res = PX4_ERROR; + } + if (res == PX4_OK) { // set offset parameters to new values bool param_save = false; @@ -320,6 +328,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } } + if (!failed && factory_storage.store() != PX4_OK) { + failed = true; + } + if (param_save) { param_notify_changes(); } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index d5cd517d4b..4c5ea622d4 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -41,6 +41,7 @@ #include "commander_helper.h" #include "calibration_routines.h" #include "calibration_messages.h" +#include "factory_calibration_storage.h" #include #include @@ -852,6 +853,13 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma free(worker_data.z[cur_mag]); } + FactoryCalibrationStorage factory_storage; + + if (result == calibrate_return_ok && factory_storage.open() != PX4_OK) { + calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage"); + result = calibrate_return_error; + } + if (result == calibrate_return_ok) { bool param_save = false; bool failed = true; @@ -893,6 +901,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } } + if (!failed && factory_storage.store() != PX4_OK) { + failed = true; + } + if (param_save) { // reset the learned EKF mag in-flight bias offsets which have been learned for the previous // sensor calibration and will be invalidated by a new sensor calibration @@ -960,6 +972,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian return PX4_ERROR; } + FactoryCalibrationStorage factory_storage; + + if (factory_storage.open() != PX4_OK) { + calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage"); + return PX4_ERROR; + } + calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees", (double)math::radians(heading_radians)); @@ -1005,6 +1024,10 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian param_notify_changes(); } + if (!failed && factory_storage.store() != PX4_OK) { + failed = true; + } + if (!failed) { calibration_log_info(mavlink_log_pub, "Mag quick calibration finished"); return PX4_OK; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index dffda46932..318e3a5e37 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -245,6 +245,7 @@ Sensors::Sensors(bool hil_enabled) : param_find("SENS_BOARD_Y_OFF"); param_find("SENS_BOARD_Z_OFF"); + param_find("SYS_FAC_CAL_MODE"); param_find("SYS_PARAM_VER"); param_find("SYS_AUTOSTART"); param_find("SYS_AUTOCONFIG");