diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index e51475a039..58eb69126d 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -59,6 +59,7 @@ add_subdirectory(output_limit) add_subdirectory(perf) add_subdirectory(pid) add_subdirectory(rc) +add_subdirectory(sensor_calibration) add_subdirectory(systemlib) add_subdirectory(tecs) add_subdirectory(terrain_estimation) diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp new file mode 100644 index 0000000000..e3073075df --- /dev/null +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 "Accelerometer.hpp" + +#include "Utilities.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +namespace calibration +{ + +Accelerometer::Accelerometer() +{ + Reset(); +} + +Accelerometer::Accelerometer(uint32_t device_id) +{ + Reset(); + set_device_id(device_id); +} + +void Accelerometer::set_device_id(uint32_t device_id) +{ + if (_device_id != device_id) { + _device_id = device_id; + ParametersUpdate(); + SensorCorrectionsUpdate(true); + } +} + +void Accelerometer::SensorCorrectionsUpdate(bool force) +{ + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + + // valid device id required + if (_device_id == 0) { + return; + } + + sensor_correction_s corrections; + + if (_sensor_correction_sub.copy(&corrections)) { + // find sensor_corrections index + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (corrections.accel_device_ids[i] == _device_id) { + switch (i) { + case 0: + _thermal_offset = Vector3f{corrections.accel_offset_0}; + return; + case 1: + _thermal_offset = Vector3f{corrections.accel_offset_1}; + return; + case 2: + _thermal_offset = Vector3f{corrections.accel_offset_2}; + return; + } + } + } + } + + // zero thermal offset if not found + _thermal_offset.zero(); + } +} + +void Accelerometer::ParametersUpdate() +{ + if (_device_id == 0) { + Reset(); + return; + } + + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + + if (_calibration_index >= 0) { + + if (!_external) { + _rotation = GetBoardRotation(); + + } else { + // TODO: per sensor external rotation + _rotation.setIdentity(); + } + + // CAL_ACCx_EN + int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index); + _enabled = (enabled == 1); + + // CAL_ACCx_OFF{X,Y,Z} + _offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); + + // CAL_ACCx_SCALE{X,Y,Z} + _scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index); + + } else { + Reset(); + } +} + +void Accelerometer::Reset() +{ + _rotation.setIdentity(); + _offset.zero(); + _scale = Vector3f{1.f, 1.f, 1.f}; + _thermal_offset.zero(); + + _enabled = true; + + _calibration_index = -1; +} + +bool Accelerometer::ParametersSave() +{ + if (_calibration_index >= 0) { + // save calibration + SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); + SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0); + SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); + SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale); + + return true; + } + + return false; +} + +void Accelerometer::PrintStatus() +{ + PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), device_id(), enabled(), + (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2)); + + if (_thermal_offset.norm() > 0.f) { + PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id, + (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); + } +} + +} // namespace calibration diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp new file mode 100644 index 0000000000..b6d85bdec5 --- /dev/null +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include + +namespace calibration +{ +class Accelerometer +{ +public: + static constexpr int MAX_SENSOR_COUNT = 3; + + static constexpr const char *SensorString() { return "ACC"; } + + Accelerometer(); + explicit Accelerometer(uint32_t device_id); + + ~Accelerometer() = default; + + void PrintStatus(); + + void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } + void set_device_id(uint32_t device_id); + void set_external(bool external = true) { _external = external; } + void set_offset(const matrix::Vector3f &offset) { _offset = offset; } + void set_scale(const matrix::Vector3f &scale) { _scale = scale; } + + uint32_t device_id() const { return _device_id; } + bool enabled() const { return _enabled; } + bool external() const { return _external; } + const matrix::Dcmf &rotation() const { return _rotation; } + + // apply offsets and scale + // rotate corrected measurements from sensor to body frame + inline matrix::Vector3f Correct(const matrix::Vector3f &data) const + { + return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)}; + } + + bool ParametersSave(); + void ParametersUpdate(); + + void Reset(); + + void SensorCorrectionsUpdate(bool force = false); + +private: + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + + matrix::Dcmf _rotation; + matrix::Vector3f _offset; + matrix::Vector3f _scale; + matrix::Vector3f _thermal_offset; + + int8_t _calibration_index{-1}; + uint32_t _device_id{0}; + + bool _enabled{true}; + bool _external{false}; +}; +} // namespace calibration diff --git a/src/modules/sensors/sensor_calibration/CMakeLists.txt b/src/lib/sensor_calibration/CMakeLists.txt similarity index 89% rename from src/modules/sensors/sensor_calibration/CMakeLists.txt rename to src/lib/sensor_calibration/CMakeLists.txt index 711bdddccc..d82e016f61 100644 --- a/src/modules/sensors/sensor_calibration/CMakeLists.txt +++ b/src/lib/sensor_calibration/CMakeLists.txt @@ -32,8 +32,10 @@ ############################################################################ px4_add_library(sensor_calibration - SensorCalibration.cpp - SensorCalibration.hpp + Accelerometer.cpp + Accelerometer.hpp + Gyroscope.cpp + Gyroscope.hpp + Utilities.cpp + Utilities.hpp ) -target_compile_options(sensor_calibration PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_include_directories(sensor_calibration PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp new file mode 100644 index 0000000000..c12683b53e --- /dev/null +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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 "Gyroscope.hpp" + +#include "Utilities.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +namespace calibration +{ + +Gyroscope::Gyroscope() +{ + Reset(); +} + +Gyroscope::Gyroscope(uint32_t device_id) +{ + Reset(); + set_device_id(device_id); +} + +void Gyroscope::set_device_id(uint32_t device_id) +{ + if (_device_id != device_id) { + _device_id = device_id; + ParametersUpdate(); + SensorCorrectionsUpdate(true); + } +} + +void Gyroscope::SensorCorrectionsUpdate(bool force) +{ + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + + // valid device id required + if (_device_id == 0) { + return; + } + + sensor_correction_s corrections; + + if (_sensor_correction_sub.copy(&corrections)) { + // find sensor_corrections index + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (corrections.gyro_device_ids[i] == _device_id) { + switch (i) { + case 0: + _thermal_offset = Vector3f{corrections.gyro_offset_0}; + return; + case 1: + _thermal_offset = Vector3f{corrections.gyro_offset_1}; + return; + case 2: + _thermal_offset = Vector3f{corrections.gyro_offset_2}; + return; + } + } + } + } + + // zero thermal offset if not found + _thermal_offset.zero(); + } +} + +void Gyroscope::ParametersUpdate() +{ + if (_device_id == 0) { + Reset(); + return; + } + + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + + if (_calibration_index >= 0) { + + if (!_external) { + _rotation = GetBoardRotation(); + + } else { + // TODO: per sensor external rotation + _rotation.setIdentity(); + } + + // CAL_GYROx_EN + int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index); + _enabled = (enabled == 1); + + // CAL_GYROx_OFF{X,Y,Z} + _offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); + + } else { + Reset(); + } +} + +void Gyroscope::Reset() +{ + _rotation.setIdentity(); + _offset.zero(); + _thermal_offset.zero(); + + _enabled = true; + + _calibration_index = -1; +} + +bool Gyroscope::ParametersSave() +{ + if (_calibration_index >= 0) { + // save calibration + SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); + SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0); + SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); + + return true; + } + + return false; +} + +void Gyroscope::PrintStatus() +{ + PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), device_id(), enabled(), + (double)_offset(0), (double)_offset(1), (double)_offset(2)); + + if (_thermal_offset.norm() > 0.f) { + PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id, + (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); + } +} + +} // namespace calibration diff --git a/src/modules/sensors/sensor_calibration/SensorCalibration.hpp b/src/lib/sensor_calibration/Gyroscope.hpp similarity index 76% rename from src/modules/sensors/sensor_calibration/SensorCalibration.hpp rename to src/lib/sensor_calibration/Gyroscope.hpp index b4a7f95cd0..209aa84173 100644 --- a/src/modules/sensors/sensor_calibration/SensorCalibration.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -40,67 +40,57 @@ #include #include -namespace sensors +namespace calibration { - -class SensorCalibration +class Gyroscope { public: + static constexpr int MAX_SENSOR_COUNT = 3; - enum class SensorType : uint8_t { - Accelerometer, - Gyroscope, - }; + static constexpr const char *SensorString() { return "GYRO"; } - SensorCalibration() = delete; - explicit SensorCalibration(SensorType type) : _type(type) {} - ~SensorCalibration() = default; + Gyroscope(); + explicit Gyroscope(uint32_t device_id); + + ~Gyroscope() = default; void PrintStatus(); + void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } void set_device_id(uint32_t device_id); void set_external(bool external = true) { _external = external; } + void set_offset(const matrix::Vector3f &offset) { _offset = offset; } uint32_t device_id() const { return _device_id; } bool enabled() const { return _enabled; } bool external() const { return _external; } + const matrix::Dcmf &rotation() const { return _rotation; } // apply offsets and scale // rotate corrected measurements from sensor to body frame - matrix::Vector3f Correct(const matrix::Vector3f &data); + inline matrix::Vector3f Correct(const matrix::Vector3f &data) const + { + return _rotation * matrix::Vector3f{data - _thermal_offset - _offset}; + } + bool ParametersSave(); void ParametersUpdate(); + + void Reset(); + void SensorCorrectionsUpdate(bool force = false); - const matrix::Dcmf &getBoardRotation() const { return _rotation; } - private: - - static constexpr int MAX_SENSOR_COUNT = 3; - - int FindCalibrationIndex(uint32_t device_id) const; - - bool CalibrationEnabled(uint8_t calibration_index) const; - matrix::Vector3f CalibrationOffset(uint8_t calibration_index) const; - matrix::Vector3f CalibrationScale(uint8_t calibration_index) const; - - const char *SensorString() const; - uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; matrix::Dcmf _rotation; + matrix::Vector3f _offset; + matrix::Vector3f _thermal_offset; - matrix::Vector3f _offset{0.f, 0.f, 0.f}; - matrix::Vector3f _scale{1.f, 1.f, 1.f}; - - matrix::Vector3f _thermal_offset{0.f, 0.f, 0.f}; - + int8_t _calibration_index{-1}; uint32_t _device_id{0}; - const SensorType _type; - bool _enabled{true}; bool _external{false}; }; - -} // namespace sensors +} // namespace calibration diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp new file mode 100644 index 0000000000..22f9368cca --- /dev/null +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * 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 + +using math::radians; +using matrix::Eulerf; +using matrix::Dcmf; +using matrix::Vector3f; + +namespace calibration +{ + +int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id) +{ + if (device_id == 0) { + return -1; + } + + for (unsigned i = 0; i < 4; ++i) { + char str[20] {}; + sprintf(str, "CAL_%s%u_ID", sensor_type, i); + + int32_t device_id_val = 0; + + param_t param_handle = param_find_no_notification(str); + + if (param_handle == PARAM_INVALID) { + continue; + } + + // find again and get value, but this time mark it active + if (param_get(param_find(str), &device_id_val) != OK) { + continue; + } + + if ((uint32_t)device_id_val == device_id) { + return i; + } + } + + return -1; +} + +int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance) +{ + // eg CAL_MAGn_ID/CAL_MAGn_ROT + char str[20] {}; + sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type); + + int32_t value = 0; + + if (param_get(param_find(str), &value) != 0) { + PX4_ERR("failed to get %s", str); + } + + return value; +} + +int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value) +{ + char str[20] {}; + + // eg CAL_MAGn_ID/CAL_MAGn_ROT + sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type); + + int ret = param_set_no_notification(param_find(str), &value); + + if (ret != PX4_OK) { + PX4_ERR("failed to set %s = %d", str, value); + } + + return ret; +} + +Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance) +{ + Vector3f values{0.f, 0.f, 0.f}; + + char str[20] {}; + + for (int axis = 0; axis < 3; axis++) { + char axis_char = 'X' + axis; + + // eg CAL_MAGn_{X,Y,Z}OFF + sprintf(str, "CAL_%s%u_%c%s", sensor_type, instance, axis_char, cal_type); + + if (param_get(param_find(str), &values(axis)) != 0) { + PX4_ERR("failed to get %s", str); + } + } + + return values; +} + +int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values) +{ + int ret = PX4_OK; + char str[20] {}; + + for (int axis = 0; axis < 3; axis++) { + char axis_char = 'X' + axis; + + // eg CAL_MAGn_{X,Y,Z}OFF + sprintf(str, "CAL_%s%u_%c%s", sensor_type, instance, axis_char, cal_type); + + if (param_set_no_notification(param_find(str), &values(axis)) != 0) { + PX4_ERR("failed to set %s = %.4f", str, (double)values(axis)); + ret = PX4_ERROR; + } + } + + return ret; +} + +Dcmf GetBoardRotation() +{ + float x_offset = 0.f; + float y_offset = 0.f; + float z_offset = 0.f; + param_get(param_find("SENS_BOARD_X_OFF"), &x_offset); + param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset); + param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset); + + const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset))); + + // get transformation matrix from sensor/board to body frame + int32_t board_rot = 0; + param_get(param_find("SENS_BOARD_ROT"), &board_rot); + + return board_rotation_offset * get_rot_matrix((enum Rotation)board_rot); +} + +} // namespace calibration diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp new file mode 100644 index 0000000000..b7c0960aeb --- /dev/null +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 + +#include + +namespace calibration +{ + +int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id); + +int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance); +int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value); + +matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance); +int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, + matrix::Vector3f values); + +matrix::Dcmf GetBoardRotation(); + + +} // namespace calibration diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt index 46b9c36729..0bf2c119f6 100644 --- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt +++ b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt @@ -50,4 +50,4 @@ px4_add_library(PreFlightCheck checks/cpuResourceCheck.cpp ) target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags) +target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 6e6ba21c36..1c4884bc45 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -301,38 +301,3 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* Report status */ return !failed; } - -bool PreFlightCheck::check_calibration(const char *param_template, const int32_t device_id) -{ - bool calibration_found = false; - - char s[20]; - int instance = 0; - - /* old style transition: check param values */ - while (!calibration_found) { - sprintf(s, param_template, instance); - const param_t parm = param_find_no_notification(s); - - /* if the calibration param is not present, abort */ - if (parm == PARAM_INVALID) { - break; - } - - /* if param get succeeds */ - int32_t calibration_devid = -1; - - if (param_get(parm, &calibration_devid) == PX4_OK) { - - /* if the devid matches, exit early */ - if (device_id == calibration_devid) { - calibration_found = true; - break; - } - } - - instance++; - } - - return calibration_found; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index e84da80b02..1e17228886 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -112,7 +112,7 @@ private: const bool report_fail, const bool enforce_gps_required); static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm); - static bool check_calibration(const char *param_template, const int32_t device_id); + static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail); static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status); static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index 28e7631d91..dd2cd3d34b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -37,7 +37,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -65,7 +66,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s device_id = accel.get().device_id; - calibration_valid = check_calibration("CAL_ACC%u_ID", device_id); + calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0); if (!calibration_valid) { if (report_fail) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index 50496e686a..6c69b93f2b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -36,7 +36,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -64,7 +65,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & device_id = gyro.get().device_id; - calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id); + calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0); if (!calibration_valid) { if (report_fail) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index 9dfcad5610..cfaadfdccb 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -36,7 +36,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -64,7 +65,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st device_id = magnetometer.get().device_id; - calibration_valid = check_calibration("CAL_MAG%u_ID", device_id); + calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0); if (!calibration_valid) { if (report_fail) { diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index ad128743b5..8fa82b6299 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -46,6 +46,7 @@ px4_add_module( commander_helper.cpp esc_calibration.cpp gyro_calibration.cpp + level_calibration.cpp mag_calibration.cpp rc_calibration.cpp state_machine_helper.cpp @@ -58,6 +59,7 @@ px4_add_module( PreFlightCheck ArmAuthorization HealthFlags + sensor_calibration ) if(PX4_TESTING) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4120aeb6d9..04b2e83d03 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -54,6 +54,7 @@ #include "commander_helper.h" #include "esc_calibration.h" #include "gyro_calibration.h" +#include "level_calibration.h" #include "mag_calibration.h" #include "px4_custom_mode.h" #include "rc_calibration.h" diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfe2cb7fc9..68a3619408 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -130,38 +130,25 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include +#include +#include #include -#include -#include #include #include -using namespace time_literals; using namespace matrix; -using math::radians; +using namespace time_literals; static constexpr char sensor_name[] {"accel"}; - static constexpr unsigned MAX_ACCEL_SENS = 3; -static calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, - Vector3f(&accel_offs)[MAX_ACCEL_SENS], - Matrix3f(&accel_T)[MAX_ACCEL_SENS], unsigned active_sensors); - -static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3], - unsigned orient, unsigned samples_num); - -static calibrate_return calculate_calibration_values(unsigned sensor, - float (&accel_ref)[MAX_ACCEL_SENS][detect_orientation_side_count][3], Matrix3f(&accel_T)[MAX_ACCEL_SENS], - Vector3f(&accel_offs)[MAX_ACCEL_SENS]); - /// Data passed to calibration worker routine typedef struct { orb_advert_t *mavlink_log_pub{nullptr}; @@ -169,233 +156,10 @@ typedef struct { float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {}; } accel_worker_data_t; -int do_accel_calibration(orb_advert_t *mavlink_log_pub) -{ - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - - int res = PX4_OK; - - int32_t device_id[MAX_ACCEL_SENS] {}; - int device_prio_max = 0; - int32_t device_id_primary = 0; - unsigned active_sensors = 0; - - // We should not try to subscribe if the topic doesn't actually exist and can be counted. - const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel)); - - // Warn that we will not calibrate more than max_accels accelerometers - if (orb_accel_count > MAX_ACCEL_SENS) { - calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, - MAX_ACCEL_SENS); - } - - for (uint8_t cur_accel = 0; cur_accel < orb_accel_count && cur_accel < MAX_ACCEL_SENS; cur_accel++) { - uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel), cur_accel}; - device_id[cur_accel] = accel_sub.get().device_id; - - if (device_id[cur_accel] != 0) { - // Get priority - int32_t prio = accel_sub.get_priority(); - - if (prio > device_prio_max) { - device_prio_max = prio; - device_id_primary = device_id[cur_accel]; - } - - active_sensors++; - - } else { - calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel); - return PX4_ERROR; - } - } - - /* measure and calculate offsets & scales */ - Vector3f accel_offs[MAX_ACCEL_SENS] {}; - Matrix3f accel_T[MAX_ACCEL_SENS] {}; - calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, active_sensors); - - if (cal_return != calibrate_return_ok) { - // Cancel message already displayed, nothing left to do - return PX4_ERROR; - } - - if (active_sensors == 0) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); - return PX4_ERROR; - } - - /* measurements completed successfully, rotate calibration values */ - int32_t board_rotation_int = 0; - param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int); - const Dcmf board_rotation = get_rot_matrix((enum Rotation)board_rotation_int); - const Dcmf board_rotation_t = board_rotation.transpose(); - - param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary); - - for (unsigned uorb_index = 0; uorb_index < MAX_ACCEL_SENS; uorb_index++) { - - if (uorb_index < active_sensors) { - /* handle individual sensors, one by one */ - const Vector3f accel_offs_rotated = board_rotation_t *accel_offs[uorb_index]; - const Matrix3f accel_T_rotated = board_rotation_t *accel_T[uorb_index] * board_rotation; - - PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, - (double)accel_offs_rotated(0), (double)accel_offs_rotated(1), (double)accel_offs_rotated(2)); - - PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, - (double)accel_T_rotated(0, 0), (double)accel_T_rotated(1, 1), (double)accel_T_rotated(2, 2)); - - char str[30] {}; - - // calibration offsets - float x_offset = accel_offs_rotated(0); - sprintf(str, "CAL_ACC%u_XOFF", uorb_index); - param_set_no_notification(param_find(str), &x_offset); - - float y_offset = accel_offs_rotated(1); - sprintf(str, "CAL_ACC%u_YOFF", uorb_index); - param_set_no_notification(param_find(str), &y_offset); - - float z_offset = accel_offs_rotated(2); - sprintf(str, "CAL_ACC%u_ZOFF", uorb_index); - param_set_no_notification(param_find(str), &z_offset); - - - // calibration scale - float x_scale = accel_T_rotated(0, 0); - sprintf(str, "CAL_ACC%u_XSCALE", uorb_index); - param_set_no_notification(param_find(str), &x_scale); - - float y_scale = accel_T_rotated(1, 1); - sprintf(str, "CAL_ACC%u_YSCALE", uorb_index); - param_set_no_notification(param_find(str), &y_scale); - - float z_scale = accel_T_rotated(2, 2); - sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index); - param_set_no_notification(param_find(str), &z_scale); - - // calibration device ID - sprintf(str, "CAL_ACC%u_ID", uorb_index); - param_set_no_notification(param_find(str), &device_id[uorb_index]); - - } else { - char str[30] {}; - - // reset calibration offsets - sprintf(str, "CAL_ACC%u_XOFF", uorb_index); - param_reset(param_find(str)); - sprintf(str, "CAL_ACC%u_YOFF", uorb_index); - param_reset(param_find(str)); - sprintf(str, "CAL_ACC%u_ZOFF", uorb_index); - param_reset(param_find(str)); - - // reset calibration scale - sprintf(str, "CAL_ACC%u_XSCALE", uorb_index); - param_reset(param_find(str)); - sprintf(str, "CAL_ACC%u_YSCALE", uorb_index); - param_reset(param_find(str)); - sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index); - param_reset(param_find(str)); - - // reset calibration device ID - sprintf(str, "CAL_ACC%u_ID", uorb_index); - param_reset(param_find(str)); - } - } - - param_notify_changes(); - - if (res == PX4_OK) { - /* if there is a any preflight-check system response, let the barrage of messages through */ - px4_usleep(200000); - - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); - - } else { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); - } - - /* give this message enough time to propagate */ - px4_usleep(600000); - - return res; -} - -static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data) -{ - const unsigned samples_num = 750; - accel_worker_data_t *worker_data = (accel_worker_data_t *)(data); - - calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", - detect_orientation_str(orientation)); - - read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num); - - calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", - detect_orientation_str(orientation), - (double)worker_data->accel_ref[0][orientation][0], - (double)worker_data->accel_ref[0][orientation][1], - (double)worker_data->accel_ref[0][orientation][2]); - - worker_data->done_count++; - calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); - - return calibrate_return_ok; -} - -static calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, - Vector3f(&accel_offs)[MAX_ACCEL_SENS], Matrix3f(&accel_T)[MAX_ACCEL_SENS], unsigned active_sensors) -{ - calibrate_return result = calibrate_return_ok; - - accel_worker_data_t worker_data{}; - worker_data.mavlink_log_pub = mavlink_log_pub; - - bool data_collected[detect_orientation_side_count] {}; - - if (result == calibrate_return_ok) { - int cancel_sub = calibrate_cancel_subscribe(); - result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, - false); - - calibrate_cancel_unsubscribe(cancel_sub); - } - - if (result == calibrate_return_ok) { - /* calculate offsets and transform matrix */ - for (unsigned i = 0; i < active_sensors; i++) { - result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs); - - if (result != calibrate_return_ok) { - calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error"); - break; - } - } - } - - return result; -} - -/* - * Read specified number of accelerometer samples, calculate average and dispersion. - */ +// Read specified number of accelerometer samples, calculate average and dispersion. static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { - /* get total sensor board rotation matrix */ - float board_offset[3] {}; - param_get(param_find("SENS_BOARD_X_OFF"), &board_offset[0]); - param_get(param_find("SENS_BOARD_Y_OFF"), &board_offset[1]); - param_get(param_find("SENS_BOARD_Z_OFF"), &board_offset[2]); - - const Dcmf board_rotation_offset{Eulerf{math::radians(board_offset[0]), math::radians(board_offset[1]), math::radians(board_offset[2])}}; - - int32_t board_rotation_int = 0; - param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int); - - const Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); - Vector3f accel_sum[MAX_ACCEL_SENS] {}; unsigned counts[MAX_ACCEL_SENS] {}; @@ -457,6 +221,8 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS } // rotate sensor measurements from sensor to body frame using board rotation matrix + const Dcmf board_rotation = calibration::GetBoardRotation(); + for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { accel_sum[s] = board_rotation * accel_sum[s]; } @@ -469,151 +235,145 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS return calibrate_return_ok; } -static calibrate_return calculate_calibration_values(unsigned sensor, - float (&accel_ref)[MAX_ACCEL_SENS][detect_orientation_side_count][3], Matrix3f(&accel_T)[MAX_ACCEL_SENS], - Vector3f(&accel_offs)[MAX_ACCEL_SENS]) +static calibrate_return accel_calibration_worker(detect_orientation_return orientation, void *data) { - /* calculate offsets */ - for (unsigned i = 0; i < 3; i++) { - accel_offs[sensor](i) = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2; - } + static constexpr unsigned samples_num = 750; + accel_worker_data_t *worker_data = (accel_worker_data_t *)(data); - /* fill matrix A for linear equations system*/ - Matrix3f mat_A; + calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", + detect_orientation_str(orientation)); - for (unsigned i = 0; i < 3; i++) { - for (unsigned j = 0; j < 3; j++) { - mat_A(i, j) = accel_ref[sensor][i * 2][j] - accel_offs[sensor](j); - } - } + read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num); - /* calculate inverse matrix for A */ - const Matrix3f mat_A_inv = mat_A.I(); + calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", + detect_orientation_str(orientation), + (double)worker_data->accel_ref[0][orientation][0], + (double)worker_data->accel_ref[0][orientation][1], + (double)worker_data->accel_ref[0][orientation][2]); - /* copy results to accel_T */ - for (unsigned i = 0; i < 3; i++) { - for (unsigned j = 0; j < 3; j++) { - /* simplify matrices mult because b has only one non-zero element == g at index i */ - accel_T[sensor](j, i) = mat_A_inv(j, i) * CONSTANTS_ONE_G; - } - } + worker_data->done_count++; + calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); return calibrate_return_ok; } -int do_level_calibration(orb_advert_t *mavlink_log_pub) +int do_accel_calibration(orb_advert_t *mavlink_log_pub) { - bool success = false; + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level"); + calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; + ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED; + int32_t device_id_primary = 0; + unsigned active_sensors = 0; - param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF"); - param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF"); + for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) { + uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel), cur_accel}; - // get old values - float roll_offset_current = 0.f; - float pitch_offset_current = 0.f; - param_get(roll_offset_handle, &roll_offset_current); - param_get(pitch_offset_handle, &pitch_offset_current); + if (accel_sub.advertised() && (accel_sub.get().device_id != 0) && (accel_sub.get().timestamp > 0)) { + calibrations[cur_accel].set_device_id(accel_sub.get().device_id); + active_sensors++; - int32_t board_rot_current = 0; - param_get(param_find("SENS_BOARD_ROT"), &board_rot_current); + // Get priority + const ORB_PRIO prio = accel_sub.get_priority(); - const Dcmf board_rotation_offset{Eulerf{radians(roll_offset_current), radians(pitch_offset_current), 0.f}}; - - float roll_mean = 0.f; - float pitch_mean = 0.f; - unsigned counter = 0; - bool had_motion = true; - int num_retries = 0; - - uORB::SubscriptionBlocking att_sub{ORB_ID(vehicle_attitude)}; - - while (had_motion && num_retries++ < 50) { - Vector2f min_angles{100.f, 100.f}; - Vector2f max_angles{-100.f, -100.f}; - roll_mean = 0.0f; - pitch_mean = 0.0f; - counter = 0; - int last_progress_report = -100; - const hrt_abstime calibration_duration = 500_ms; - const hrt_abstime start = hrt_absolute_time(); - - while (hrt_elapsed_time(&start) < calibration_duration) { - - vehicle_attitude_s att{}; - - if (!att_sub.updateBlocking(att, 100000)) { - // attitude estimator is not running - calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot"); - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); - goto out; + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = accel_sub.get().device_id; } - int progress = 100 * hrt_elapsed_time(&start) / calibration_duration; - - if (progress >= last_progress_report + 20) { - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress); - last_progress_report = progress; - } - - Eulerf att_euler{Quatf{att.q}}; - - // keep min + max angles - for (int i = 0; i < 2; ++i) { - if (att_euler(i) < min_angles(i)) { min_angles(i) = att_euler(i); } - - if (att_euler(i) > max_angles(i)) { max_angles(i) = att_euler(i); } - } - - att_euler(2) = 0.f; // ignore yaw - - att_euler = Eulerf{board_rotation_offset *Dcmf{att_euler}}; // subtract existing board rotation - roll_mean += att_euler.phi(); - pitch_mean += att_euler.theta(); - ++counter; + } else { + calibrations[cur_accel].Reset(); } - // motion detection: check that (max-min angle) is within a threshold. - // The difference is typically <0.1 deg while at rest - if (max_angles(0) - min_angles(0) < math::radians(0.5f) && - max_angles(1) - min_angles(1) < math::radians(0.5f)) { - - had_motion = false; - } + // reset calibration index to match uORB numbering + calibrations[cur_accel].set_calibration_index(cur_accel); } - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); + if (active_sensors == 0) { + calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); + return PX4_ERROR; - roll_mean /= counter; - pitch_mean /= counter; + } else if (active_sensors > MAX_ACCEL_SENS) { + calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", active_sensors, + MAX_ACCEL_SENS); + } - if (had_motion) { - calibration_log_critical(mavlink_log_pub, "motion during calibration"); + /* measure and calculate offsets & scales */ + accel_worker_data_t worker_data{}; + worker_data.mavlink_log_pub = mavlink_log_pub; + bool data_collected[detect_orientation_side_count] {}; - } else if (fabsf(roll_mean) > 0.8f) { - calibration_log_critical(mavlink_log_pub, "excess roll angle"); + if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data, + false) == calibrate_return_ok) { - } else if (fabsf(pitch_mean) > 0.8f) { - calibration_log_critical(mavlink_log_pub, "excess pitch angle"); + const Dcmf board_rotation = calibration::GetBoardRotation(); + const Dcmf board_rotation_t = board_rotation.transpose(); - } else { - float roll_mean_degrees = math::degrees(roll_mean); - float pitch_mean_degrees = math::degrees(pitch_mean); - param_set_no_notification(roll_offset_handle, &roll_mean_degrees); - param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees); + for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) { + if (i < active_sensors) { + // calculate offsets + Vector3f offset{}; + + // X offset: average X from TAIL_DOWN + NOSE_DOWN + const Vector3f accel_tail_down{worker_data.accel_ref[i][ORIENTATION_TAIL_DOWN]}; + const Vector3f accel_nose_down{worker_data.accel_ref[i][ORIENTATION_NOSE_DOWN]}; + offset(0) = (accel_tail_down(0) + accel_nose_down(0)) * 0.5f; + + // Y offset: average Y from LEFT + RIGHT + const Vector3f accel_left{worker_data.accel_ref[i][ORIENTATION_LEFT]}; + const Vector3f accel_right{worker_data.accel_ref[i][ORIENTATION_RIGHT]}; + offset(1) = (accel_left(1) + accel_right(1)) * 0.5f; + + // Z offset: average Z from UPSIDE_DOWN + RIGHTSIDE_UP + const Vector3f accel_upside_down{worker_data.accel_ref[i][ORIENTATION_UPSIDE_DOWN]}; + const Vector3f accel_rightside_up{worker_data.accel_ref[i][ORIENTATION_RIGHTSIDE_UP]}; + offset(2) = (accel_upside_down(2) + accel_rightside_up(2)) * 0.5f; + + // transform matrix + Matrix3f mat_A; + mat_A.row(0) = accel_tail_down - offset; + mat_A.row(1) = accel_left - offset; + mat_A.row(2) = accel_upside_down - offset; + + // calculate inverse matrix for A: simplify matrices mult because b has only one non-zero element == g at index i + const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G; + + // update calibration + const Vector3f accel_offs_rotated{board_rotation_t *offset}; + calibrations[i].set_offset(accel_offs_rotated); + + const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation}; + calibrations[i].set_scale(accel_T_rotated.diag()); + +#if defined(DEBUD_BUILD) + PX4_INFO("accel %d: offset", i); + offset.print(); + PX4_INFO("accel %d: bT * offset", i); + accel_offs_rotated.print(); + + PX4_INFO("accel %d: mat_A", i); + mat_A.print(); + PX4_INFO("accel %d: accel_T", i); + accel_T.print(); + PX4_INFO("accel %d: bT * accel_T * b", i); + accel_T_rotated.print(); +#endif // DEBUD_BUILD + } + + // save all calibrations including empty slots + calibrations[i].ParametersSave(); + } + + param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary); param_notify_changes(); - success = true; + + /* if there is a any preflight-check system response, let the barrage of messages through */ + px4_usleep(200000); + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + px4_usleep(600000); /* give this message enough time to propagate */ + return PX4_OK; } -out: - - if (success) { - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level"); - return 0; - - } else { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); - return 1; - } + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + return PX4_ERROR; } diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 2000a2cdc5..fe3e03aedc 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -46,6 +46,5 @@ #include int do_accel_calibration(orb_advert_t *mavlink_log_pub); -int do_level_calibration(orb_advert_t *mavlink_log_pub); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0c0518933f..24266698f1 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -65,6 +65,8 @@ static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub) int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) { + const hrt_abstime calibration_started = hrt_absolute_time(); + int result = PX4_OK; unsigned calibration_counter = 0; const unsigned maxcount = 2400; @@ -99,8 +101,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) px4_close(fd); } - int cancel_sub = calibrate_cancel_subscribe(); - if (!paramreset_successful) { /* only warn if analog scaling is zero */ @@ -124,7 +124,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) while (calibration_counter < calibration_count) { - if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { goto error_return; } @@ -206,7 +206,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { goto error_return; } @@ -279,7 +279,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) px4_usleep(2e6); normal_return: - calibrate_cancel_unsubscribe(cancel_sub); px4_close(diff_pres_sub); // This give a chance for the log messages to go out of the queue before someone else stomps on then diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 054e1bdada..22136293bd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -50,9 +50,11 @@ #include #include +#include #include #include #include +#include #include "calibration_routines.h" #include "calibration_messages.h" @@ -616,49 +618,49 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, if (poll_errcount > 1000) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); - return DETECT_ORIENTATION_ERROR; + return ORIENTATION_ERROR; } } if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { - return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ] + return ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ] } if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { - return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ] + return ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { - return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ] + return ORIENTATION_LEFT; // [ 0, g, 0 ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[2]) < accel_err_thr) { - return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ] + return ORIENTATION_RIGHT; // [ 0, -g, 0 ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { - return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ] + return ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ] } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { - return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] + return ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] } calibration_log_critical(mavlink_log_pub, "ERROR: invalid orientation"); - return DETECT_ORIENTATION_ERROR; // Can't detect orientation + return ORIENTATION_ERROR; // Can't detect orientation } const char *detect_orientation_str(enum detect_orientation_return orientation) @@ -677,19 +679,17 @@ const char *detect_orientation_str(enum detect_orientation_return orientation) } calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, - int cancel_sub, - bool side_data_collected[detect_orientation_side_count], - calibration_from_orientation_worker_t calibration_worker, - void *worker_data, - bool lenient_still_position) + bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, + void *worker_data, bool lenient_still_position) { + const hrt_abstime calibration_started = hrt_absolute_time(); calibrate_return result = calibrate_return_ok; unsigned orientation_failures = 0; // Rotate through all requested orientation while (true) { - if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { result = calibrate_return_cancelled; break; } @@ -731,7 +731,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, px4_usleep(20000); enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position); - if (orient == DETECT_ORIENTATION_ERROR) { + if (orient == ORIENTATION_ERROR) { orientation_failures++; calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still..."); px4_usleep(20000); @@ -754,7 +754,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, orientation_failures = 0; // Call worker routine - result = calibration_worker(orient, cancel_sub, worker_data); + result = calibration_worker(orient, worker_data); if (result != calibrate_return_ok) { break; @@ -779,74 +779,51 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, return result; } -int calibrate_cancel_subscribe() +bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started) { - int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + bool ret = false; - if (vehicle_command_sub >= 0) { - // make sure we won't read any old messages - struct vehicle_command_s cmd; - bool update; + uORB::Subscription vehicle_command_sub{ORB_ID(vehicle_command)}; + vehicle_command_s cmd; - while (orb_check(vehicle_command_sub, &update) == 0 && update) { - orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd); - } - } + while (vehicle_command_sub.update(&cmd)) { + if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) { + // only handle commands sent after calibration started from external sources + if ((cmd.timestamp > calibration_started) && cmd.from_external) { - return vehicle_command_sub; -} + vehicle_command_ack_s command_ack{}; -void calibrate_cancel_unsubscribe(int cmd_sub) -{ - orb_unsubscribe(cmd_sub); -} + if ((int)cmd.param1 == 0 && + (int)cmd.param2 == 0 && + (int)cmd.param3 == 0 && + (int)cmd.param4 == 0 && + (int)cmd.param5 == 0 && + (int)cmd.param6 == 0) { -static void calibrate_answer_command(orb_advert_t *mavlink_log_pub, struct vehicle_command_s &cmd, unsigned result) -{ - switch (result) { - case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); - break; + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); + tune_positive(true); + ret = true; - case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command); - tune_negative(true); - break; + } else { + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command); + tune_negative(true); + ret = false; + } - default: - break; - } -} + command_ack.command = cmd.command; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + command_ack.timestamp = hrt_absolute_time(); -bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub) -{ - px4_pollfd_struct_t fds[1]; - fds[0].fd = cancel_sub; - fds[0].events = POLLIN; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); - if (px4_poll(&fds[0], 1, 0) > 0) { - struct vehicle_command_s cmd; - - orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); - - // ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount - if (cmd.from_external) { - if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION && - (int)cmd.param1 == 0 && - (int)cmd.param2 == 0 && - (int)cmd.param3 == 0 && - (int)cmd.param4 == 0 && - (int)cmd.param5 == 0 && - (int)cmd.param6 == 0) { - calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); - return true; - - } else { - calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + return ret; } } } - return false; + return ret; } diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 3fc38ad95c..66eca657a2 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -36,6 +36,8 @@ #pragma once +#include + /** * Least-squares fit of a sphere to a set of points. * @@ -74,22 +76,21 @@ bool mat_inverse(float *A, float *inv, uint8_t n); // The order of these cannot change since the calibration calculations depend on them in this order enum detect_orientation_return { - DETECT_ORIENTATION_TAIL_DOWN, - DETECT_ORIENTATION_NOSE_DOWN, - DETECT_ORIENTATION_LEFT, - DETECT_ORIENTATION_RIGHT, - DETECT_ORIENTATION_UPSIDE_DOWN, - DETECT_ORIENTATION_RIGHTSIDE_UP, - DETECT_ORIENTATION_ERROR + ORIENTATION_TAIL_DOWN, + ORIENTATION_NOSE_DOWN, + ORIENTATION_LEFT, + ORIENTATION_RIGHT, + ORIENTATION_UPSIDE_DOWN, + ORIENTATION_RIGHTSIDE_UP, + ORIENTATION_ERROR }; -static const unsigned detect_orientation_side_count = 6; +static constexpr unsigned detect_orientation_side_count = 6; /// Wait for vehicle to become still and detect it's orientation /// @return Returns detect_orientation_return according to orientation when vehicle /// and ready for measurements enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to - int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe - bool lenient_still_detection); ///< true: Use more lenient still position detection + bool lenient_still_detection); ///< true: Use more lenient still position detection /// Returns the human readable string representation of the orientation /// @param orientation Orientation to return string for, "error" if buffer is too small @@ -103,29 +104,18 @@ enum calibrate_return { typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return orientation, ///< Orientation which was detected - int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe - void *worker_data); ///< Opaque worker data + void *worker_data); ///< Opaque worker data /// Perform calibration sequence which require a rest orientation detection prior to calibration. /// @return OK: Calibration succeeded, ERROR: Calibration failed -calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to - int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe - bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration - calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration - void *worker_data, ///< Opaque data passed to worker routine - bool lenient_still_detection); ///< true: Use more lenient still position detection - -/// Called at the beginning of calibration in order to subscribe to the cancel command -/// @return Handle to vehicle_command subscription -int calibrate_cancel_subscribe(void); - -/// Called to cancel the subscription to the cancel command -/// @param cancel_sub Cancel subcription from calibration_cancel_subscribe -void calibrate_cancel_unsubscribe(int cancel_sub); +calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to + bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration + calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration + void *worker_data, ///< Opaque data passed to worker routine + bool lenient_still_detection); ///< true: Use more lenient still position detection /// Used to periodically check for a cancel command -bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to - int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe +bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started); // TODO FIXME: below are workarounds for QGC. The issue is that sometimes diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8565ba1ef6..bb14f9c020 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -48,6 +48,8 @@ #include #include +#include +#include #include #include #include @@ -65,7 +67,9 @@ using matrix::Vector3f; /// Data passed to calibration worker routine struct gyro_worker_data_t { orb_advert_t *mavlink_log_pub{nullptr}; - int32_t device_id[MAX_GYROS] {}; + + calibration::Gyroscope calibrations[MAX_GYROS] {}; + Vector3f offset[MAX_GYROS] {}; static constexpr int last_num_samples = 9; ///< number of samples for the motion detection median filter @@ -84,14 +88,15 @@ static int float_cmp(const void *elem1, const void *elem2) return *(const float *)elem1 > *(const float *)elem2; } -static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data_t &worker_data) +static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data) { + const hrt_abstime calibration_started = hrt_absolute_time(); unsigned calibration_counter[MAX_GYROS] {}; static constexpr unsigned CALIBRATION_COUNT = 250; unsigned poll_errcount = 0; uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; - sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ + sensor_correction_s sensor_correction{}; uORB::SubscriptionBlocking gyro_sub[MAX_GYROS] { {ORB_ID(sensor_gyro), 0, 0}, @@ -108,7 +113,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data unsigned slow_count = 0; while (slow_count < CALIBRATION_COUNT) { - if (calibrate_cancel_check(worker_data.mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(worker_data.mavlink_log_pub, calibration_started)) { return calibrate_return_cancelled; } @@ -116,52 +121,55 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data unsigned update_count = CALIBRATION_COUNT; for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) { - if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) { - // Skip if instance has enough samples - continue; - } + if (worker_data.calibrations[gyro_index].device_id() != 0) { - sensor_gyro_s gyro_report; + if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) { + // Skip if instance has enough samples + continue; + } - if (gyro_sub[gyro_index].update(&gyro_report)) { + sensor_gyro_s gyro_report; - // fetch optional thermal offset corrections in sensor/board frame - Vector3f offset{0, 0, 0}; - sensor_correction_sub.update(&sensor_correction); + if (gyro_sub[gyro_index].update(&gyro_report)) { - if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) { - for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) { - if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) { - switch (correction_index) { - case 0: - offset = Vector3f{sensor_correction.gyro_offset_0}; - break; - case 1: - offset = Vector3f{sensor_correction.gyro_offset_1}; - break; - case 2: - offset = Vector3f{sensor_correction.gyro_offset_2}; - break; + // fetch optional thermal offset corrections in sensor/board frame + Vector3f offset{0, 0, 0}; + sensor_correction_sub.update(&sensor_correction); + + if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) { + for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) { + if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) { + switch (correction_index) { + case 0: + offset = Vector3f{sensor_correction.gyro_offset_0}; + break; + case 1: + offset = Vector3f{sensor_correction.gyro_offset_1}; + break; + case 2: + offset = Vector3f{sensor_correction.gyro_offset_2}; + break; + } } } } + + worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset; + calibration_counter[gyro_index]++; + + if (gyro_index == 0) { + worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0); + worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1); + worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2); + worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples; + } } - worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset; - calibration_counter[gyro_index]++; - - if (gyro_index == 0) { - worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0); - worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1); - worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2); - worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples; + // Maintain the sample count of the slowest sensor + if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) { + update_count = calibration_counter[gyro_index]; } } - - // Maintain the sample count of the slowest sensor - if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) { - update_count = calibration_counter[gyro_index]; - } } if (update_count % (CALIBRATION_COUNT / 20) == 0) { @@ -184,7 +192,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data } for (unsigned s = 0; s < MAX_GYROS; s++) { - if ((worker_data.device_id[s] != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) { + if ((worker_data.calibrations[s].device_id() != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) { calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", s) return calibrate_return_error; } @@ -204,7 +212,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) gyro_worker_data_t worker_data{}; worker_data.mavlink_log_pub = mavlink_log_pub; - enum ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED; + ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED; int32_t device_id_primary = 0; // We should not try to subscribe if the topic doesn't actually exist and can be counted. @@ -213,31 +221,30 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) // Warn that we will not calibrate more than MAX_GYROS gyroscopes if (orb_gyro_count > MAX_GYROS) { calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, MAX_GYROS); + + } else if (orb_gyro_count < 1) { + calibration_log_critical(mavlink_log_pub, "No gyros found"); + return PX4_ERROR; } - for (uint8_t cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < MAX_GYROS; cur_gyro++) { + for (uint8_t cur_gyro = 0; cur_gyro < MAX_GYROS; cur_gyro++) { + uORB::SubscriptionData gyro_sub{ORB_ID(sensor_gyro), cur_gyro}; - uORB::Subscription gyro_sensor_sub{ORB_ID(sensor_gyro), cur_gyro}; - sensor_gyro_s report{}; - gyro_sensor_sub.copy(&report); + if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) { + worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id); - worker_data.device_id[cur_gyro] = report.device_id; - - if (worker_data.device_id[cur_gyro] != 0) { // Get priority - enum ORB_PRIO prio = gyro_sensor_sub.get_priority(); + const ORB_PRIO prio = gyro_sub.get_priority(); if (prio > device_prio_max) { device_prio_max = prio; - device_id_primary = worker_data.device_id[cur_gyro]; + device_id_primary = gyro_sub.get().device_id; } - - } else { - calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro); } - } - int cancel_sub = calibrate_cancel_subscribe(); + // reset calibration index to match uORB numbering + worker_data.calibrations[cur_gyro].set_calibration_index(cur_gyro); + } unsigned try_count = 0; unsigned max_tries = 20; @@ -245,7 +252,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) do { // Calibrate gyro and ensure user didn't move - calibrate_return cal_return = gyro_calibration_worker(cancel_sub, worker_data); + calibrate_return cal_return = gyro_calibration_worker(worker_data); if (cal_return == calibrate_return_cancelled) { // Cancel message already sent, we are done here @@ -266,7 +273,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](2); /* maximum allowable calibration error */ - const float maxoff = math::radians(0.6f); + static constexpr float maxoff = math::radians(0.6f); if (!PX4_ISFINITE(worker_data.offset[0](0)) || !PX4_ISFINITE(worker_data.offset[0](1)) || @@ -290,49 +297,28 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) res = PX4_ERROR; } - calibrate_cancel_unsubscribe(cancel_sub); - if (res == PX4_OK) { /* set offset parameters to new values */ - bool failed = (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary)); + bool failed = false; for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) { - char str[30] {}; + auto &calibration = worker_data.calibrations[uorb_index]; - if (uorb_index < orb_gyro_count) { - float x_offset = worker_data.offset[uorb_index](0); - sprintf(str, "CAL_GYRO%u_XOFF", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &x_offset)); - - float y_offset = worker_data.offset[uorb_index](1); - sprintf(str, "CAL_GYRO%u_YOFF", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &y_offset)); - - float z_offset = worker_data.offset[uorb_index](2); - sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &z_offset)); - - int32_t device_id = worker_data.device_id[uorb_index]; - sprintf(str, "CAL_GYRO%u_ID", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &device_id)); + if (calibration.device_id() != 0) { + calibration.set_offset(worker_data.offset[uorb_index]); } else { - // reset unused calibration offsets - sprintf(str, "CAL_GYRO%u_XOFF", uorb_index); - param_reset(param_find(str)); - sprintf(str, "CAL_GYRO%u_YOFF", uorb_index); - param_reset(param_find(str)); - sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index); - param_reset(param_find(str)); - - // reset unused calibration device ID - sprintf(str, "CAL_GYRO%u_ID", uorb_index); - param_reset(param_find(str)); + calibration.Reset(); } + + calibration.set_calibration_index(uorb_index); + calibration.ParametersSave(); } + param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary); + if (failed) { calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params"); res = PX4_ERROR; diff --git a/src/modules/commander/level_calibration.cpp b/src/modules/commander/level_calibration.cpp new file mode 100644 index 0000000000..d4d1901080 --- /dev/null +++ b/src/modules/commander/level_calibration.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "level_calibration.h" +#include "calibration_messages.h" +#include "calibration_routines.h" +#include "commander_helper.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; +using namespace matrix; +using math::radians; + +int do_level_calibration(orb_advert_t *mavlink_log_pub) +{ + bool success = false; + + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level"); + + param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF"); + param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF"); + + // get old values + float roll_offset_current = 0.f; + float pitch_offset_current = 0.f; + param_get(roll_offset_handle, &roll_offset_current); + param_get(pitch_offset_handle, &pitch_offset_current); + + int32_t board_rot_current = 0; + param_get(param_find("SENS_BOARD_ROT"), &board_rot_current); + + const Dcmf board_rotation_offset{Eulerf{radians(roll_offset_current), radians(pitch_offset_current), 0.f}}; + + float roll_mean = 0.f; + float pitch_mean = 0.f; + unsigned counter = 0; + bool had_motion = true; + int num_retries = 0; + + uORB::SubscriptionBlocking att_sub{ORB_ID(vehicle_attitude)}; + + while (had_motion && num_retries++ < 50) { + Vector2f min_angles{100.f, 100.f}; + Vector2f max_angles{-100.f, -100.f}; + roll_mean = 0.0f; + pitch_mean = 0.0f; + counter = 0; + int last_progress_report = -100; + const hrt_abstime calibration_duration = 500_ms; + const hrt_abstime start = hrt_absolute_time(); + + while (hrt_elapsed_time(&start) < calibration_duration) { + + vehicle_attitude_s att{}; + + if (!att_sub.updateBlocking(att, 100000)) { + // attitude estimator is not running + calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot"); + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); + goto out; + } + + int progress = 100 * hrt_elapsed_time(&start) / calibration_duration; + + if (progress >= last_progress_report + 20) { + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress); + last_progress_report = progress; + } + + Eulerf att_euler{Quatf{att.q}}; + + // keep min + max angles + for (int i = 0; i < 2; ++i) { + if (att_euler(i) < min_angles(i)) { min_angles(i) = att_euler(i); } + + if (att_euler(i) > max_angles(i)) { max_angles(i) = att_euler(i); } + } + + att_euler(2) = 0.f; // ignore yaw + + att_euler = Eulerf{board_rotation_offset *Dcmf{att_euler}}; // subtract existing board rotation + roll_mean += att_euler.phi(); + pitch_mean += att_euler.theta(); + ++counter; + } + + // motion detection: check that (max-min angle) is within a threshold. + // The difference is typically <0.1 deg while at rest + if (max_angles(0) - min_angles(0) < math::radians(0.5f) && + max_angles(1) - min_angles(1) < math::radians(0.5f)) { + + had_motion = false; + } + } + + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); + + roll_mean /= counter; + pitch_mean /= counter; + + if (had_motion) { + calibration_log_critical(mavlink_log_pub, "motion during calibration"); + + } else if (fabsf(roll_mean) > 0.8f) { + calibration_log_critical(mavlink_log_pub, "excess roll angle"); + + } else if (fabsf(pitch_mean) > 0.8f) { + calibration_log_critical(mavlink_log_pub, "excess pitch angle"); + + } else { + float roll_mean_degrees = math::degrees(roll_mean); + float pitch_mean_degrees = math::degrees(pitch_mean); + param_set_no_notification(roll_offset_handle, &roll_mean_degrees); + param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees); + param_notify_changes(); + success = true; + } + +out: + + if (success) { + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level"); + return 0; + + } else { + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); + return 1; + } +} diff --git a/src/modules/commander/level_calibration.h b/src/modules/commander/level_calibration.h new file mode 100644 index 0000000000..454a538b8a --- /dev/null +++ b/src/modules/commander/level_calibration.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + +#include +#include + +int do_level_calibration(orb_advert_t *mavlink_log_pub); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 857d044d42..1b97f4af41 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -342,8 +342,9 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, return calibrate_return_ok; } -static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data) +static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data) { + const hrt_abstime calibration_started = hrt_absolute_time(); calibrate_return result = calibrate_return_ok; unsigned int calibration_counter_side; @@ -380,7 +381,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta fabsf(gyro_z_integral) < gyro_int_thresh_rad) { /* abort on request */ - if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { result = calibrate_return_cancelled; px4_close(sub_gyro); return result; @@ -430,7 +431,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { - if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { result = calibrate_return_cancelled; break; } @@ -669,15 +670,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } if (result == calibrate_return_ok) { - int cancel_sub = calibrate_cancel_subscribe(); - result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output - cancel_sub, // Subscription to vehicle_command for cancel support worker_data.side_data_collected, // Sides to calibrate mag_calibration_worker, // Calibration worker &worker_data, // Opaque data for calibration worked true); // true: lenient still detection - calibrate_cancel_unsubscribe(cancel_sub); } // Close subscriptions diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 71f67fb571..50e020a165 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -33,7 +33,6 @@ add_subdirectory(data_validator) -add_subdirectory(sensor_calibration) # used by vehicle_{acceleration, angular_velocity, imu} include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) @@ -54,6 +53,7 @@ px4_add_module( drivers__device git_ecl mathlib + sensor_calibration vehicle_acceleration vehicle_angular_velocity vehicle_air_data diff --git a/src/modules/sensors/sensor_calibration/SensorCalibration.cpp b/src/modules/sensors/sensor_calibration/SensorCalibration.cpp deleted file mode 100644 index d2f24ba8e2..0000000000 --- a/src/modules/sensors/sensor_calibration/SensorCalibration.cpp +++ /dev/null @@ -1,252 +0,0 @@ -/**************************************************************************** - * - * 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 "SensorCalibration.hpp" - -#include - -using namespace matrix; -using namespace time_literals; -using math::radians; - -namespace sensors -{ - -void SensorCalibration::set_device_id(uint32_t device_id) -{ - if (_device_id != device_id) { - _device_id = device_id; - SensorCorrectionsUpdate(true); - ParametersUpdate(); - } -} - -matrix::Vector3f SensorCalibration::Correct(const matrix::Vector3f &data) -{ - SensorCorrectionsUpdate(); - return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)}; -} - -const char *SensorCalibration::SensorString() const -{ - switch (_type) { - case SensorType::Accelerometer: - return "ACC"; - - case SensorType::Gyroscope: - return "GYRO"; - } - - return nullptr; -} - -int SensorCalibration::FindCalibrationIndex(uint32_t device_id) const -{ - if (device_id == 0) { - return -1; - } - - for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { - char str[16] {}; - sprintf(str, "CAL_%s%u_ID", SensorString(), i); - - int32_t device_id_val = 0; - - if (param_get(param_find(str), &device_id_val) != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id_val == device_id) { - return i; - } - } - - return -1; -} - -bool SensorCalibration::CalibrationEnabled(uint8_t calibration_index) const -{ - char str[20] {}; - sprintf(str, "CAL_%s%u_EN", SensorString(), calibration_index); - int32_t enabled_val = 1; - param_get(param_find(str), &enabled_val); - - return (enabled_val != 0); -} - -Vector3f SensorCalibration::CalibrationOffset(uint8_t calibration_index) const -{ - // offsets (x, y, z) - Vector3f offset{0.f, 0.f, 0.f}; - char str[20] {}; - - sprintf(str, "CAL_%s%u_XOFF", SensorString(), calibration_index); - param_get(param_find(str), &offset(0)); - - sprintf(str, "CAL_%s%u_YOFF", SensorString(), calibration_index); - param_get(param_find(str), &offset(1)); - - sprintf(str, "CAL_%s%u_ZOFF", SensorString(), calibration_index); - param_get(param_find(str), &offset(2)); - - return offset; -} - -Vector3f SensorCalibration::CalibrationScale(uint8_t calibration_index) const -{ - // scale factors (x, y, z) - Vector3f scale{1.f, 1.f, 1.f}; - - // gyroscope doesn't have a scale factor calibration - if (_type != SensorType::Gyroscope) { - char str[20] {}; - - sprintf(str, "CAL_%s%u_XSCALE", SensorString(), calibration_index); - param_get(param_find(str), &scale(0)); - - sprintf(str, "CAL_%s%u_YSCALE", SensorString(), calibration_index); - param_get(param_find(str), &scale(1)); - - sprintf(str, "CAL_%s%u_ZSCALE", SensorString(), calibration_index); - param_get(param_find(str), &scale(2)); - } - - return scale; -} - -void SensorCalibration::SensorCorrectionsUpdate(bool force) -{ - // check if the selected sensor has updated - if (_sensor_correction_sub.updated() || force) { - - // valid device id required - if (_device_id == 0) { - return; - } - - sensor_correction_s corrections; - - if (_sensor_correction_sub.copy(&corrections)) { - // find sensor_corrections index - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if ((_type == SensorType::Accelerometer) && (corrections.accel_device_ids[i] == _device_id)) { - switch (i) { - case 0: - _thermal_offset = Vector3f{corrections.accel_offset_0}; - return; - case 1: - _thermal_offset = Vector3f{corrections.accel_offset_1}; - return; - case 2: - _thermal_offset = Vector3f{corrections.accel_offset_2}; - return; - } - - } else if ((_type == SensorType::Gyroscope) && (corrections.gyro_device_ids[i] == _device_id)) { - switch (i) { - case 0: - _thermal_offset = Vector3f{corrections.gyro_offset_0}; - return; - case 1: - _thermal_offset = Vector3f{corrections.gyro_offset_1}; - return; - case 2: - _thermal_offset = Vector3f{corrections.gyro_offset_2}; - return; - } - } - } - } - - // zero thermal offset if not found - _thermal_offset.zero(); - } -} - -void SensorCalibration::ParametersUpdate() -{ - if (!_external) { - // fine tune the rotation - float x_offset = 0.f; - float y_offset = 0.f; - float z_offset = 0.f; - param_get(param_find("SENS_BOARD_X_OFF"), &x_offset); - param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset); - param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset); - - const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset))); - - // get transformation matrix from sensor/board to body frame - int32_t board_rot = 0; - param_get(param_find("SENS_BOARD_ROT"), &board_rot); - _rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rot); - - } else { - // TODO: per sensor external rotation - _rotation.setIdentity(); - } - - - int calibration_index = FindCalibrationIndex(_device_id); - - if (calibration_index >= 0) { - _enabled = CalibrationEnabled(calibration_index); - _offset = CalibrationOffset(calibration_index); - _scale = CalibrationScale(calibration_index); - - } else { - _enabled = true; - _offset.zero(); - _scale = Vector3f{1.f, 1.f, 1.f}; - } -} - -void SensorCalibration::PrintStatus() -{ - if (_type != SensorType::Gyroscope) { - PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled, - (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2)); - - } else { - PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled, - (double)_offset(0), (double)_offset(1), (double)_offset(2)); - } - - if (_thermal_offset.norm() > 0.f) { - PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id, - (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); - } -} - -} // namespace sensors diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index 4cc5eb9e22..f79cd6a9e9 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -47,12 +47,12 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); * recommended. * * Bits: - * DETECT_ORIENTATION_TAIL_DOWN = 1 - * DETECT_ORIENTATION_NOSE_DOWN = 2 - * DETECT_ORIENTATION_LEFT = 4 - * DETECT_ORIENTATION_RIGHT = 8 - * DETECT_ORIENTATION_UPSIDE_DOWN = 16 - * DETECT_ORIENTATION_RIGHTSIDE_UP = 32 + * ORIENTATION_TAIL_DOWN = 1 + * ORIENTATION_NOSE_DOWN = 2 + * ORIENTATION_LEFT = 4 + * ORIENTATION_RIGHT = 8 + * ORIENTATION_UPSIDE_DOWN = 16 + * ORIENTATION_RIGHTSIDE_UP = 32 * * @min 34 * @max 63 diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt index 7accc1b9a6..37f3558992 100644 --- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -41,6 +41,6 @@ target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_link_libraries(vehicle_acceleration PRIVATE mathlib - sensor_calibration px4_work_queue + sensor_calibration ) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 4c154b4339..73003534e0 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -33,8 +33,7 @@ #pragma once -#include - +#include #include #include #include @@ -88,7 +87,7 @@ private: {this, ORB_ID(sensor_accel), 2} }; - SensorCalibration _calibration{SensorCalibration::SensorType::Accelerometer}; + calibration::Accelerometer _calibration{}; matrix::Vector3f _bias{0.f, 0.f, 0.f}; diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt index 14116e9d44..e38916fc8f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -41,6 +41,6 @@ target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_link_libraries(vehicle_angular_velocity PRIVATE mathlib - sensor_calibration px4_work_queue + sensor_calibration ) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index d44af5962d..3596ae773f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -33,8 +33,7 @@ #pragma once -#include - +#include #include #include #include @@ -91,7 +90,7 @@ private: {this, ORB_ID(sensor_gyro), 2} }; - SensorCalibration _calibration{SensorCalibration::SensorType::Gyroscope}; + calibration::Gyroscope _calibration{}; matrix::Vector3f _bias{0.f, 0.f, 0.f}; diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 85458b01b1..2fc8b30243 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -38,4 +38,4 @@ px4_add_library(vehicle_imu VehicleIMU.hpp ) target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_link_libraries(vehicle_imu PRIVATE sensor_calibration px4_work_queue) +target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 3ad32242d2..7e91388e84 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -191,6 +191,11 @@ void VehicleIMU::Run() ParametersUpdate(); + if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { + return; + } + + bool sensor_data_gap = false; bool update_integrator_config = false; bool publish_status = false; @@ -269,7 +274,7 @@ void VehicleIMU::Run() if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { // rotate sensor clip counts into vehicle body frame - const Vector3f clipping{_accel_calibration.getBoardRotation() * + const Vector3f clipping{_accel_calibration.rotation() * Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; // round to get reasonble clip counts per axis (after board rotation) @@ -332,42 +337,45 @@ void VehicleIMU::Run() if (_accel_integrator.reset(delta_velocity, accel_integral_dt) && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) { - // delta angle: apply offsets, scale, and board rotation - _gyro_calibration.SensorCorrectionsUpdate(); - const float gyro_dt_inv = 1.e6f / gyro_integral_dt; - const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; + if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { - // delta velocity: apply offsets, scale, and board rotation - _accel_calibration.SensorCorrectionsUpdate(); - const float accel_dt_inv = 1.e6f / accel_integral_dt; - Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; + // delta angle: apply offsets, scale, and board rotation + _gyro_calibration.SensorCorrectionsUpdate(); + const float gyro_dt_inv = 1.e6f / gyro_integral_dt; + const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; - UpdateAccelVibrationMetrics(delta_velocity_corrected); - UpdateGyroVibrationMetrics(delta_angle_corrected); + // delta velocity: apply offsets, scale, and board rotation + _accel_calibration.SensorCorrectionsUpdate(); + const float accel_dt_inv = 1.e6f / accel_integral_dt; + Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; - // vehicle_imu_status - // publish before vehicle_imu so that error counts are available synchronously if needed - if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) { - _status.accel_device_id = _accel_calibration.device_id(); - _status.gyro_device_id = _gyro_calibration.device_id(); - _status.timestamp = hrt_absolute_time(); - _vehicle_imu_status_pub.publish(_status); + UpdateAccelVibrationMetrics(delta_velocity_corrected); + UpdateGyroVibrationMetrics(delta_angle_corrected); + + // vehicle_imu_status + // publish before vehicle_imu so that error counts are available synchronously if needed + if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) { + _status.accel_device_id = _accel_calibration.device_id(); + _status.gyro_device_id = _gyro_calibration.device_id(); + _status.timestamp = hrt_absolute_time(); + _vehicle_imu_status_pub.publish(_status); + } + + + // publish vehicle_imu + vehicle_imu_s imu; + imu.timestamp_sample = _last_timestamp_sample_gyro; + imu.accel_device_id = _accel_calibration.device_id(); + imu.gyro_device_id = _gyro_calibration.device_id(); + delta_angle_corrected.copyTo(imu.delta_angle); + delta_velocity_corrected.copyTo(imu.delta_velocity); + imu.delta_angle_dt = gyro_integral_dt; + imu.delta_velocity_dt = accel_integral_dt; + imu.delta_velocity_clipping = _delta_velocity_clipping; + imu.timestamp = hrt_absolute_time(); + _vehicle_imu_pub.publish(imu); } - - // publish vehicle_imu - vehicle_imu_s imu; - imu.timestamp_sample = _last_timestamp_sample_gyro; - imu.accel_device_id = _accel_calibration.device_id(); - imu.gyro_device_id = _gyro_calibration.device_id(); - delta_angle_corrected.copyTo(imu.delta_angle); - delta_velocity_corrected.copyTo(imu.delta_velocity); - imu.delta_angle_dt = gyro_integral_dt; - imu.delta_velocity_dt = accel_integral_dt; - imu.delta_velocity_clipping = _delta_velocity_clipping; - imu.timestamp = hrt_absolute_time(); - _vehicle_imu_pub.publish(imu); - // reset clip counts _delta_velocity_clipping = 0; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index b2ebab1147..46f7705364 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -35,11 +35,11 @@ #include "Integrator.hpp" -#include - #include #include #include +#include +#include #include #include #include @@ -91,8 +91,8 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_accel_sub; uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; - SensorCalibration _accel_calibration{SensorCalibration::SensorType::Accelerometer}; - SensorCalibration _gyro_calibration{SensorCalibration::SensorType::Gyroscope}; + calibration::Accelerometer _accel_calibration{}; + calibration::Gyroscope _gyro_calibration{}; Integrator _accel_integrator{}; // 200 Hz default Integrator _gyro_integrator{true}; // 200 Hz default, coning compensation enabled