diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 4421c3c389..45f5153bc4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -143,9 +143,12 @@ then param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION param set CAL_MAG0_ID 197388 - param set CAL_MAG0_PRIO 50 + param set SENS_MAG0_ID 197388 + param set SENS_MAG0_PRIO 50 + param set CAL_MAG1_ID 197644 - param set CAL_MAG1_PRIO 50 + param set SENS_MAG1_ID 197644 + param set SENS_MAG1_PRIO 50 param set SENS_BOARD_X_OFF 0.000001 param set SENS_DPRES_OFF 0.001 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 594a891482..1f49ef7eef 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -31,7 +31,7 @@ param set-default BAT1_R_INTERNAL 0.0025 param set-default CBRK_AIRSPD_CHK 162128 param set-default CBRK_IO_SAFETY 22027 -param set-default EKF2_GPS_POS_X -0.12 +param set-default SENS_GNSS0_XPOS -0.12 param set-default EKF2_IMU_POS_X -0.12 param set-default FW_ARSP_MODE 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d6fd77945b..4e4110b39d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -18,8 +18,8 @@ param set-default COM_DISARM_LAND 0.5 # EKF2 parameters param set-default EKF2_DRAG_CTRL 1 param set-default EKF2_IMU_POS_X 0.02 -param set-default EKF2_GPS_POS_X 0.055 -param set-default EKF2_GPS_POS_Z -0.15 +param set-default SENS_GNSS0_XPOS 0.055 +param set-default SENS_GNSS0_ZPOS -0.15 param set-default EKF2_MIN_RNG 0.03 param set-default EKF2_OF_CTRL 1 param set-default EKF2_OF_POS_X 0.055 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index bcfcd10fda..4b2af6b5e7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -46,7 +46,9 @@ param set-default EKF2_BCOEF_Y 25.5 param set-default EKF2_DRAG_CTRL 1 param set-default EKF2_GPS_DELAY 100 -param set-default EKF2_GPS_POS_X 0.06 +param set-default SENS_GNSS0_XPOS 0.06 +param set-default SENS_GNSS0_YPOS 0.0 +param set-default SENS_GNSS0_ZPOS 0.0 param set-default EKF2_GPS_V_NOISE 0.5 param set-default EKF2_IMU_POS_X 0.06 diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index db18899cb1..c398fac748 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -62,4 +62,6 @@ uint8 RTCM_MSG_USED_NOT_USED = 1 uint8 RTCM_MSG_USED_USED = 2 uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver +float32[3] position_offset # position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) + # TOPICS sensor_gps vehicle_gps_position diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1d88b3b022..22011e2a32 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -67,7 +67,7 @@ add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) -add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) +add_subdirectory(sensor EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) add_subdirectory(system_identification EXCLUDE_FROM_ALL) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index b438de23a2..3cdb9e14cb 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -44,5 +44,24 @@ bool param_modify_on_import(bson_node_t node) { + // TODO: + // ACC, BARO, GYRO, MAG + // prio, rot + + // 2023-11-30: translate sensor calibration to configuration + { + if (strcmp("CAL_ACC0_PRIO", node->name) == 0) { + strcpy(node->name, "SENS_ACC0_PRIO"); + PX4_INFO("copying %s -> %s", "CAL_ACC0_PRIO", "SENS_ACC0_PRIO"); + + // TODO: set SENS_ACC0_ID=CAL_ACC0_ID + + + return true; + } + } + + + return false; } diff --git a/src/lib/sensor/CMakeLists.txt b/src/lib/sensor/CMakeLists.txt new file mode 100644 index 0000000000..45c284eb38 --- /dev/null +++ b/src/lib/sensor/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +px4_add_library(sensor_utilities + Utilities.cpp + Utilities.hpp +) + +target_link_libraries(sensor_utilities PRIVATE conversion) + +add_subdirectory(configuration) +add_subdirectory(calibration) diff --git a/src/lib/sensor/Utilities.cpp b/src/lib/sensor/Utilities.cpp new file mode 100644 index 0000000000..987418f290 --- /dev/null +++ b/src/lib/sensor/Utilities.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#if defined(CONFIG_I2C) +# include +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) +# include +#endif // CONFIG_SPI + +using math::radians; +using matrix::Eulerf; +using matrix::Dcmf; +using matrix::Vector3f; + +namespace sensor +{ +namespace utilities +{ + +Eulerf GetSensorLevelAdjustment() +{ + 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); + + return Eulerf{radians(x_offset), radians(y_offset), radians(z_offset)}; +} + +enum Rotation GetBoardRotation() +{ + // get transformation matrix from sensor/board to body frame + int32_t board_rot = -1; + param_get(param_find("SENS_BOARD_ROT"), &board_rot); + + if (board_rot >= 0 && board_rot <= Rotation::ROTATION_MAX) { + return static_cast(board_rot); + + } else { + PX4_ERR("invalid SENS_BOARD_ROT: %" PRId32, board_rot); + } + + return Rotation::ROTATION_NONE; +} + +Dcmf GetBoardRotationMatrix() +{ + return get_rot_matrix(GetBoardRotation()); +} + +bool DeviceExternal(uint32_t device_id) +{ + bool external = true; + + // decode device id to determine if external + union device::Device::DeviceId id {}; + id.devid = device_id; + + const device::Device::DeviceBusType bus_type = id.devid_s.bus_type; + + switch (bus_type) { + case device::Device::DeviceBusType_I2C: +#if defined(CONFIG_I2C) + external = px4_i2c_bus_external(id.devid_s.bus); +#endif // CONFIG_I2C + break; + + case device::Device::DeviceBusType_SPI: +#if defined(CONFIG_SPI) + external = px4_spi_bus_external(id.devid_s.bus); +#endif // CONFIG_SPI + break; + + case device::Device::DeviceBusType_UAVCAN: + external = true; + break; + + case device::Device::DeviceBusType_SIMULATION: + external = false; + break; + + case device::Device::DeviceBusType_SERIAL: + external = true; + break; + + case device::Device::DeviceBusType_MAVLINK: + external = true; + break; + + case device::Device::DeviceBusType_UNKNOWN: + external = true; + break; + } + + return external; +} + +} // namespace utilities +} // namespace sensor diff --git a/src/lib/sensor/Utilities.hpp b/src/lib/sensor/Utilities.hpp new file mode 100644 index 0000000000..ababf12cdc --- /dev/null +++ b/src/lib/sensor/Utilities.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 sensor +{ +namespace utilities +{ + +/** + * @brief Get the board sensor level adjustment (SENS_BOARD_X_OFF, SENS_BOARD_Y_OFF, SENS_BOARD_Z_OFF). + * + * @return matrix::Eulerf + */ +matrix::Eulerf GetSensorLevelAdjustment(); + +/** + * @brief Get the board rotation. + * + * @return enum Rotation + */ +Rotation GetBoardRotation(); + +/** + * @brief Get the board rotation Dcm. + * + * @return matrix::Dcmf + */ +matrix::Dcmf GetBoardRotationMatrix(); + +/** + * @brief Determine if device is on an external bus + * + * @return true if device is on an external bus + */ +bool DeviceExternal(uint32_t device_id); + +} // namespace utilities +} // namespace sensor diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor/calibration/Accelerometer.cpp similarity index 65% rename from src/lib/sensor_calibration/Accelerometer.cpp rename to src/lib/sensor/calibration/Accelerometer.cpp index ac7c74d1dc..c2ff97c86e 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor/calibration/Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -36,10 +36,14 @@ #include "Utilities.hpp" #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -48,7 +52,8 @@ Accelerometer::Accelerometer() Reset(); } -Accelerometer::Accelerometer(uint32_t device_id) +Accelerometer::Accelerometer(uint32_t device_id) : + _configuration(device_id) { set_device_id(device_id); } @@ -57,10 +62,9 @@ void Accelerometer::set_device_id(uint32_t device_id) { bool external = DeviceExternal(device_id); - if (_device_id != device_id || _external != external) { + if (_configuration.device_id() != device_id || _configuration.external() != external) { - _device_id = device_id; - _external = external; + _configuration.set_device_id(device_id); Reset(); @@ -75,7 +79,7 @@ void Accelerometer::SensorCorrectionsUpdate(bool force) if (_sensor_correction_sub.updated() || force) { // valid device id required - if (_device_id == 0) { + if (device_id() == 0) { return; } @@ -84,17 +88,20 @@ void Accelerometer::SensorCorrectionsUpdate(bool force) 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) { + 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; + case 3: _thermal_offset = Vector3f{corrections.accel_offset_3}; return; @@ -110,12 +117,15 @@ void Accelerometer::SensorCorrectionsUpdate(bool force) bool Accelerometer::set_offset(const Vector3f &offset) { - if (Vector3f(_offset - offset).longerThan(0.01f)) { - if (offset.isAllFinite()) { - _offset = offset; - _calibration_count++; - return true; - } + static constexpr float MIN_OFFSET_CHANGE = 0.01f; + + if (offset.isAllFinite() + && (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0)) + ) { + _offset = offset; + + _calibration_count++; + return true; } return false; @@ -123,25 +133,20 @@ bool Accelerometer::set_offset(const Vector3f &offset) bool Accelerometer::set_scale(const Vector3f &scale) { - if (Vector3f(_scale - scale).longerThan(0.01f)) { - if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) { - _scale = scale; - _calibration_count++; - return true; - } + static constexpr float MIN_SCALE_CHANGE = 0.01f; + + if (scale.isAllFinite() && scale.longerThan(0.f) + && (Vector3f(_scale - scale).longerThan(MIN_SCALE_CHANGE) || (_calibration_count == 0)) + ) { + _scale = scale; + + _calibration_count++; + return true; } return false; } -void Accelerometer::set_rotation(Rotation rotation) -{ - _rotation_enum = rotation; - - // always apply board level adjustments - _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); -} - bool Accelerometer::set_calibration_index(int calibration_index) { if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { @@ -154,11 +159,11 @@ bool Accelerometer::set_calibration_index(int calibration_index) void Accelerometer::ParametersUpdate() { - if (_device_id == 0) { + if (device_id() == 0) { return; } - _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); + _calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id()); if (_calibration_index == -1) { // no saved calibration available @@ -166,52 +171,32 @@ void Accelerometer::ParametersUpdate() } else { ParametersLoad(); + + if (calibrated() && !_configuration.configured()) { + _configuration.ParametersSave(calibration_index()); + } } } bool Accelerometer::ParametersLoad() { if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { - // CAL_ACCx_ROT - int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); - if (_external) { - if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { - // invalid rotation, resetting - rotation_value = ROTATION_NONE; - } + // CAL_{}n_{X,Y,Z}OFF + Vector3f offset{ + GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index) + }; - set_rotation(static_cast(rotation_value)); + // CAL_{}n_{X,Y,Z}SCALE + Vector3f scale{ + GetCalibrationParamFloat(SensorString(), "XSCALE", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YSCALE", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZSCALE", _calibration_index) + }; - } else { - // internal sensors follow board rotation - set_rotation(GetBoardRotation()); - } - - // CAL_ACCx_PRIO - _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); - - if ((_priority < 0) || (_priority > 100)) { - // reset to default, -1 is the uninitialized parameter value - static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1; - - if (_priority != CAL_PRIO_UNINITIALIZED) { - PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, - _calibration_index, _priority); - - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED); - } - - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - } - - // CAL_ACCx_OFF{X,Y,Z} - set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); - - // CAL_ACCx_SCALE{X,Y,Z} - set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index)); - - return true; + return set_offset(offset) && set_scale(scale); } return false; @@ -219,21 +204,11 @@ bool Accelerometer::ParametersLoad() void Accelerometer::Reset() { - if (_external) { - set_rotation(ROTATION_NONE); - - } else { - // internal sensors follow board rotation - set_rotation(GetBoardRotation()); - } - _offset.zero(); _scale = Vector3f{1.f, 1.f, 1.f}; _thermal_offset.zero(); - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - _calibration_index = -1; _calibration_count = 0; @@ -241,6 +216,8 @@ void Accelerometer::Reset() bool Accelerometer::ParametersSave(int desired_calibration_index, bool force) { + _configuration.ParametersSave(); + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { _calibration_index = desired_calibration_index; @@ -249,10 +226,10 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force) // ensure we have a valid calibration slot (matching existing or first available slot) int8_t calibration_index_prev = _calibration_index; - _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + _calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index); if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { - PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(), calibration_index_prev, _calibration_index); } } @@ -260,17 +237,18 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force) if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { // save calibration bool success = true; - success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); - success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); - success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale); - if (_external) { - success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id()); - } else { - success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal - } + // CAL_{}n_{X,Y,Z}OFF + success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0)); + success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1)); + success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2)); + + // CAL_{}n_{X,Y,Z}SCALE + success &= SetCalibrationParam(SensorString(), "XSCALE", _calibration_index, _scale(0)); + success &= SetCalibrationParam(SensorString(), "YSCALE", _calibration_index, _scale(1)); + success &= SetCalibrationParam(SensorString(), "ZSCALE", _calibration_index, _scale(2)); return success; } @@ -297,9 +275,10 @@ void Accelerometer::PrintStatus() } if (_thermal_offset.norm() > 0.f) { - PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), _device_id, + PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(), (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); } } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor/calibration/Accelerometer.hpp similarity index 77% rename from src/lib/sensor_calibration/Accelerometer.hpp rename to src/lib/sensor/calibration/Accelerometer.hpp index ef631a00f1..5f7a8b9fa7 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor/calibration/Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -33,13 +33,16 @@ #pragma once -#include -#include #include -#include + +#include #include #include +#include + +namespace sensor +{ namespace calibration { class Accelerometer @@ -47,9 +50,6 @@ class Accelerometer public: static constexpr int MAX_SENSOR_COUNT = 4; - static constexpr uint8_t DEFAULT_PRIORITY = 50; - static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; - static constexpr const char *SensorString() { return "ACC"; } Accelerometer(); @@ -61,34 +61,39 @@ public: bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); + bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); - void set_rotation(Rotation rotation); - bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } + void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); } + + bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } - uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } - bool external() const { return _external; } + const matrix::Vector3f &offset() const { return _offset; } - const int32_t &priority() const { return _priority; } - const matrix::Dcmf &rotation() const { return _rotation; } - const Rotation &rotation_enum() const { return _rotation_enum; } const matrix::Vector3f &scale() const { return _scale; } + uint32_t device_id() const { return _configuration.device_id(); } + bool enabled() const { return _configuration.enabled(); } + bool external() const { return _configuration.external(); } + + const int32_t &priority() const { return _configuration.priority(); } + const matrix::Dcmf &rotation() const { return _configuration.rotation(); } + const Rotation &rotation_enum() const { return _configuration.rotation_enum(); } + // 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)}; + return _configuration.rotation() * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)}; } // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { // updated calibration offset = existing offset + bias rotated to sensor frame and unscaled - return _offset + (_rotation.I() * bias).edivide(_scale); + return _offset + (_configuration.rotation().I() * bias).edivide(_scale); } bool ParametersLoad(); @@ -100,21 +105,17 @@ public: void SensorCorrectionsUpdate(bool force = false); private: + sensor::configuration::Accelerometer _configuration{}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; - Rotation _rotation_enum{ROTATION_NONE}; - - matrix::Dcmf _rotation; matrix::Vector3f _offset; matrix::Vector3f _scale; matrix::Vector3f _thermal_offset; int8_t _calibration_index{-1}; - uint32_t _device_id{0}; - int32_t _priority{-1}; - - bool _external{false}; - uint8_t _calibration_count{0}; }; + } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Barometer.cpp b/src/lib/sensor/calibration/Barometer.cpp similarity index 77% rename from src/lib/sensor_calibration/Barometer.cpp rename to src/lib/sensor/calibration/Barometer.cpp index 430dfc1af3..cbc88dade8 100644 --- a/src/lib/sensor_calibration/Barometer.cpp +++ b/src/lib/sensor/calibration/Barometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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 @@ -36,10 +36,14 @@ #include "Utilities.hpp" #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -48,7 +52,8 @@ Barometer::Barometer() Reset(); } -Barometer::Barometer(uint32_t device_id) +Barometer::Barometer(uint32_t device_id) : + _configuration(device_id) { set_device_id(device_id); } @@ -57,10 +62,9 @@ void Barometer::set_device_id(uint32_t device_id) { bool external = DeviceExternal(device_id); - if (_device_id != device_id || _external != external) { + if (_configuration.device_id() != device_id || _configuration.external() != external) { - _device_id = device_id; - _external = external; + _configuration.set_device_id(device_id); Reset(); @@ -75,7 +79,7 @@ void Barometer::SensorCorrectionsUpdate(bool force) if (_sensor_correction_sub.updated() || force) { // valid device id required - if (_device_id == 0) { + if (device_id() == 0) { return; } @@ -84,7 +88,7 @@ void Barometer::SensorCorrectionsUpdate(bool force) if (_sensor_correction_sub.copy(&corrections)) { // find sensor_corrections index for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if (corrections.baro_device_ids[i] == _device_id) { + if (corrections.baro_device_ids[i] == device_id()) { switch (i) { case 0: _thermal_offset = corrections.baro_offset_0; @@ -113,12 +117,15 @@ void Barometer::SensorCorrectionsUpdate(bool force) bool Barometer::set_offset(const float &offset) { - if (fabsf(_offset - offset) > 0.01f) { - if (PX4_ISFINITE(offset)) { - _offset = offset; - _calibration_count++; - return true; - } + static constexpr float MIN_OFFSET_CHANGE = 0.01f; + + if (PX4_ISFINITE(offset) + && ((fabsf(_offset - offset) > MIN_OFFSET_CHANGE) || (_calibration_count == 0)) + ) { + _offset = offset; + + _calibration_count++; + return true; } return false; @@ -136,11 +143,11 @@ bool Barometer::set_calibration_index(int calibration_index) void Barometer::ParametersUpdate() { - if (_device_id == 0) { + if (device_id() == 0) { return; } - _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); + _calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id()); if (_calibration_index == -1) { // no saved calibration available @@ -148,33 +155,21 @@ void Barometer::ParametersUpdate() } else { ParametersLoad(); + + if (calibrated() && !_configuration.configured()) { + _configuration.ParametersSave(calibration_index()); + } } } bool Barometer::ParametersLoad() { if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { - // CAL_BAROx_PRIO - _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); - if ((_priority < 0) || (_priority > 100)) { - // reset to default, -1 is the uninitialized parameter value - static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1; + // CAL_{}n_OFF + float offset = GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index); - if (_priority != CAL_PRIO_UNINITIALIZED) { - PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, - _calibration_index, _priority); - - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED); - } - - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - } - - // CAL_BAROx_OFF - set_offset(GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index)); - - return true; + return set_offset(offset); } return false; @@ -186,8 +181,6 @@ void Barometer::Reset() _thermal_offset = 0; - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - _calibration_index = -1; _calibration_count = 0; @@ -195,6 +188,8 @@ void Barometer::Reset() bool Barometer::ParametersSave(int desired_calibration_index, bool force) { + _configuration.ParametersSave(); + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { _calibration_index = desired_calibration_index; @@ -203,10 +198,10 @@ bool Barometer::ParametersSave(int desired_calibration_index, bool force) // ensure we have a valid calibration slot (matching existing or first available slot) int8_t calibration_index_prev = _calibration_index; - _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + _calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index); if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { - PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(), calibration_index_prev, _calibration_index); } } @@ -214,8 +209,10 @@ bool Barometer::ParametersSave(int desired_calibration_index, bool force) if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { // save calibration bool success = true; - success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); + + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id()); + + // CAL_{}n_OFF success &= SetCalibrationParam(SensorString(), "OFF", _calibration_index, _offset); return success; @@ -239,8 +236,10 @@ void Barometer::PrintStatus() } if (fabsf(_thermal_offset) > 0.f) { - PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), _device_id, (double)_thermal_offset); + PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), device_id(), + (double)_thermal_offset); } } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Barometer.hpp b/src/lib/sensor/calibration/Barometer.hpp similarity index 82% rename from src/lib/sensor_calibration/Barometer.hpp rename to src/lib/sensor/calibration/Barometer.hpp index dedd45d54e..41bde9a723 100644 --- a/src/lib/sensor_calibration/Barometer.hpp +++ b/src/lib/sensor/calibration/Barometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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 @@ -34,10 +34,14 @@ #pragma once #include -#include + #include #include +#include + +namespace sensor +{ namespace calibration { class Barometer @@ -45,9 +49,6 @@ class Barometer public: static constexpr int MAX_SENSOR_COUNT = 4; - static constexpr uint8_t DEFAULT_PRIORITY = 50; - static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; - static constexpr const char *SensorString() { return "BARO"; } Barometer(); @@ -59,18 +60,21 @@ public: bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); + bool set_offset(const float &offset); - bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } + bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } - uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } - bool external() const { return _external; } + const float &offset() const { return _offset; } - const int32_t &priority() const { return _priority; } const float &thermal_offset() const { return _thermal_offset; } + uint32_t device_id() const { return _configuration.device_id(); } + bool enabled() const { return _configuration.enabled(); } + bool external() const { return _configuration.external(); } + const int32_t &priority() const { return _configuration.priority(); } + // apply offsets inline float Correct(const float &data) const { @@ -85,7 +89,8 @@ public: // Compute sensor offset from bias (board frame) float BiasCorrectedSensorOffset(const float &bias) const { - return bias + _thermal_offset + _offset; + // updated calibration offset = existing offset + bias + return _offset + bias; } bool ParametersLoad(); @@ -97,17 +102,16 @@ public: void SensorCorrectionsUpdate(bool force = false); private: + sensor::configuration::Barometer _configuration{}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; float _offset{0}; float _thermal_offset{0}; int8_t _calibration_index{-1}; - uint32_t _device_id{0}; - int32_t _priority{-1}; - - bool _external{false}; - uint8_t _calibration_count{0}; }; + } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/CMakeLists.txt b/src/lib/sensor/calibration/CMakeLists.txt similarity index 91% rename from src/lib/sensor_calibration/CMakeLists.txt rename to src/lib/sensor/calibration/CMakeLists.txt index c85bde638e..a6bbb19ff1 100644 --- a/src/lib/sensor_calibration/CMakeLists.txt +++ b/src/lib/sensor/calibration/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-2022 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 @@ -44,4 +44,9 @@ px4_add_library(sensor_calibration Utilities.hpp ) -target_link_libraries(sensor_calibration PRIVATE conversion) +target_link_libraries(sensor_calibration + PRIVATE + conversion + sensor_configuration + sensor_utilities +) diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor/calibration/Gyroscope.cpp similarity index 66% rename from src/lib/sensor_calibration/Gyroscope.cpp rename to src/lib/sensor/calibration/Gyroscope.cpp index e81c1903c9..67473ff3ea 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor/calibration/Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -36,10 +36,14 @@ #include "Utilities.hpp" #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -48,7 +52,8 @@ Gyroscope::Gyroscope() Reset(); } -Gyroscope::Gyroscope(uint32_t device_id) +Gyroscope::Gyroscope(uint32_t device_id) : + _configuration(device_id) { set_device_id(device_id); } @@ -57,10 +62,9 @@ void Gyroscope::set_device_id(uint32_t device_id) { bool external = DeviceExternal(device_id); - if (_device_id != device_id || _external != external) { + if (_configuration.device_id() != device_id || _configuration.external() != external) { - _device_id = device_id; - _external = external; + _configuration.set_device_id(device_id); Reset(); @@ -75,7 +79,7 @@ void Gyroscope::SensorCorrectionsUpdate(bool force) if (_sensor_correction_sub.updated() || force) { // valid device id required - if (_device_id == 0) { + if (device_id() == 0) { return; } @@ -84,17 +88,20 @@ void Gyroscope::SensorCorrectionsUpdate(bool force) 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) { + 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; + case 3: _thermal_offset = Vector3f{corrections.gyro_offset_3}; return; @@ -110,25 +117,20 @@ void Gyroscope::SensorCorrectionsUpdate(bool force) bool Gyroscope::set_offset(const Vector3f &offset) { - if (Vector3f(_offset - offset).longerThan(0.01f) || (_calibration_count == 0)) { - if (offset.isAllFinite()) { - _offset = offset; - _calibration_count++; - return true; - } + static constexpr float MIN_OFFSET_CHANGE = 0.01f; + + if (offset.isAllFinite() + && (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0)) + ) { + _offset = offset; + + _calibration_count++; + return true; } return false; } -void Gyroscope::set_rotation(Rotation rotation) -{ - _rotation_enum = rotation; - - // always apply board level adjustments - _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); -} - bool Gyroscope::set_calibration_index(int calibration_index) { if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { @@ -141,11 +143,11 @@ bool Gyroscope::set_calibration_index(int calibration_index) void Gyroscope::ParametersUpdate() { - if (_device_id == 0) { + if (device_id() == 0) { return; } - _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); + _calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id()); if (_calibration_index == -1) { // no saved calibration available @@ -153,49 +155,25 @@ void Gyroscope::ParametersUpdate() } else { ParametersLoad(); + + if (calibrated() && !_configuration.configured()) { + _configuration.ParametersSave(calibration_index()); + } } } bool Gyroscope::ParametersLoad() { if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { - // CAL_GYROx_ROT - int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); - if (_external) { - if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { - // invalid rotation, resetting - rotation_value = ROTATION_NONE; - } + // CAL_{}n_{X,Y,Z}OFF + Vector3f offset{ + GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index) + }; - set_rotation(static_cast(rotation_value)); - - } else { - // internal sensors follow board rotation - set_rotation(GetBoardRotation()); - } - - // CAL_GYROx_PRIO - _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); - - if ((_priority < 0) || (_priority > 100)) { - // reset to default, -1 is the uninitialized parameter value - static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1; - - if (_priority != CAL_PRIO_UNINITIALIZED) { - PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, - _calibration_index, _priority); - - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED); - } - - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - } - - // CAL_GYROx_OFF{X,Y,Z} - set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); - - return true; + return set_offset(offset); } return false; @@ -203,20 +181,10 @@ bool Gyroscope::ParametersLoad() void Gyroscope::Reset() { - if (_external) { - set_rotation(ROTATION_NONE); - - } else { - // internal sensors follow board rotation - set_rotation(GetBoardRotation()); - } - _offset.zero(); _thermal_offset.zero(); - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - _calibration_index = -1; _calibration_count = 0; @@ -224,6 +192,8 @@ void Gyroscope::Reset() bool Gyroscope::ParametersSave(int desired_calibration_index, bool force) { + _configuration.ParametersSave(); + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { _calibration_index = desired_calibration_index; @@ -232,10 +202,10 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force) // ensure we have a valid calibration slot (matching existing or first available slot) int8_t calibration_index_prev = _calibration_index; - _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + _calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index); if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { - PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(), calibration_index_prev, _calibration_index); } } @@ -243,16 +213,13 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force) if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { // save calibration bool success = true; - success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); - success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); - if (_external) { - success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id()); - } else { - success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal - } + // CAL_{}n_{X,Y,Z}OFF + success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0)); + success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1)); + success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2)); return success; } @@ -270,15 +237,17 @@ void Gyroscope::PrintStatus() rotation_enum()); } else { - PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n", + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n", SensorString(), device_id(), enabled(), (double)_offset(0), (double)_offset(1), (double)_offset(2)); } if (_thermal_offset.norm() > 0.f) { - PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), _device_id, + PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(), (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); } } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor/calibration/Gyroscope.hpp similarity index 76% rename from src/lib/sensor_calibration/Gyroscope.hpp rename to src/lib/sensor/calibration/Gyroscope.hpp index 6d160a1217..6395433055 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor/calibration/Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -33,13 +33,16 @@ #pragma once -#include -#include #include -#include + +#include #include #include +#include + +namespace sensor +{ namespace calibration { class Gyroscope @@ -47,9 +50,6 @@ class Gyroscope public: static constexpr int MAX_SENSOR_COUNT = 4; - static constexpr uint8_t DEFAULT_PRIORITY = 50; - static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; - static constexpr const char *SensorString() { return "GYRO"; } Gyroscope(); @@ -61,38 +61,43 @@ public: bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); - bool set_offset(const matrix::Vector3f &offset); - void set_rotation(Rotation rotation); - bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } + bool set_offset(const matrix::Vector3f &offset); + + void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); } + + bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } - uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } - bool external() const { return _external; } + const matrix::Vector3f &offset() const { return _offset; } - const int32_t &priority() const { return _priority; } - const matrix::Dcmf &rotation() const { return _rotation; } - const Rotation &rotation_enum() const { return _rotation_enum; } const matrix::Vector3f &thermal_offset() const { return _thermal_offset; } + uint32_t device_id() const { return _configuration.device_id(); } + bool enabled() const { return _configuration.enabled(); } + bool external() const { return _configuration.external(); } + + const int32_t &priority() const { return _configuration.priority(); } + const matrix::Dcmf &rotation() const { return _configuration.rotation(); } + const Rotation &rotation_enum() const { return _configuration.rotation_enum(); } + // 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}; + return _configuration.rotation() * matrix::Vector3f{data - _thermal_offset - _offset}; } inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const { - return (_rotation.I() * corrected_data) + _thermal_offset + _offset; + return (_configuration.rotation().I() * corrected_data) + _thermal_offset + _offset; } // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { // updated calibration offset = existing offset + bias rotated to sensor frame - return _offset + (_rotation.I() * bias); + return _offset + (_configuration.rotation().I() * bias); } bool ParametersLoad(); @@ -104,20 +109,16 @@ public: void SensorCorrectionsUpdate(bool force = false); private: + sensor::configuration::Gyroscope _configuration{}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; - Rotation _rotation_enum{ROTATION_NONE}; - - matrix::Dcmf _rotation; matrix::Vector3f _offset; matrix::Vector3f _thermal_offset; int8_t _calibration_index{-1}; - uint32_t _device_id{0}; - int32_t _priority{-1}; - - bool _external{false}; - uint8_t _calibration_count{0}; }; + } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor/calibration/Magnetometer.cpp b/src/lib/sensor/calibration/Magnetometer.cpp new file mode 100644 index 0000000000..bb33823752 --- /dev/null +++ b/src/lib/sensor/calibration/Magnetometer.cpp @@ -0,0 +1,337 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 "Magnetometer.hpp" + +#include "Utilities.hpp" + +#include +#include + +using namespace matrix; +using namespace time_literals; +using namespace sensor::utilities; + +namespace sensor +{ +namespace calibration +{ + +Magnetometer::Magnetometer() +{ + Reset(); +} + +Magnetometer::Magnetometer(uint32_t device_id) : + _configuration(device_id) +{ + set_device_id(device_id); +} + +void Magnetometer::set_device_id(uint32_t device_id) +{ + bool external = DeviceExternal(device_id); + + if (_configuration.device_id() != device_id || _configuration.external() != external) { + + _configuration.set_device_id(device_id); + + Reset(); + + ParametersUpdate(); + SensorCorrectionsUpdate(true); + } +} + +void Magnetometer::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.mag_device_ids[i] == device_id()) { + switch (i) { + case 0: + _thermal_offset = Vector3f{corrections.mag_offset_0}; + return; + + case 1: + _thermal_offset = Vector3f{corrections.mag_offset_1}; + return; + + case 2: + _thermal_offset = Vector3f{corrections.mag_offset_2}; + return; + + case 3: + _thermal_offset = Vector3f{corrections.mag_offset_3}; + return; + } + } + } + } + + // zero thermal offset if not found + _thermal_offset.zero(); + } +} + +bool Magnetometer::set_offset(const Vector3f &offset) +{ + static constexpr float MIN_OFFSET_CHANGE = 0.005f; + + if (offset.isAllFinite() + && (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0)) + ) { + _offset = offset; + + _calibration_count++; + return true; + } + + return false; +} + +bool Magnetometer::set_scale(const Vector3f &scale) +{ + static constexpr float MIN_SCALE_CHANGE = 0.01f; + + if (scale.isAllFinite() && scale.longerThan(0.f) + && (Vector3f(_scale.diag() - scale).longerThan(MIN_SCALE_CHANGE) || (_calibration_count == 0)) + ) { + _scale(0, 0) = scale(0); + _scale(1, 1) = scale(1); + _scale(2, 2) = scale(2); + + _calibration_count++; + return true; + } + + return false; +} + +bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal) +{ + if (offdiagonal.isAllFinite() + && (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f) + || (_calibration_count == 0)) + ) { + + _scale(0, 1) = offdiagonal(0); + _scale(1, 0) = offdiagonal(0); + + _scale(0, 2) = offdiagonal(1); + _scale(2, 0) = offdiagonal(1); + + _scale(1, 2) = offdiagonal(2); + _scale(2, 1) = offdiagonal(2); + + _calibration_count++; + return true; + } + + return false; +} + +bool Magnetometer::set_calibration_index(int calibration_index) +{ + if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { + _calibration_index = calibration_index; + return true; + } + + return false; +} + +void Magnetometer::ParametersUpdate() +{ + if (device_id() == 0) { + return; + } + + _calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id()); + + if (_calibration_index == -1) { + // no saved calibration available + Reset(); + + } else { + ParametersLoad(); + + if (calibrated() && !_configuration.configured()) { + _configuration.ParametersSave(calibration_index()); + } + } +} + +bool Magnetometer::ParametersLoad() +{ + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { + + // CAL_{}n_{X,Y,Z}OFF + Vector3f offset{ + GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index) + }; + + // CAL_{}n_{X,Y,Z}SCALE + Vector3f scale{ + GetCalibrationParamFloat(SensorString(), "XSCALE", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YSCALE", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZSCALE", _calibration_index) + }; + + // CAL_{}n_{X,Y,Z}ODIAG + Vector3f odiag{ + GetCalibrationParamFloat(SensorString(), "XODIAG", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YODIAG", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZODIAG", _calibration_index) + }; + + // CAL_{}n_{X,Y,Z}COMP + Vector3f comp{ + GetCalibrationParamFloat(SensorString(), "XCOMP", _calibration_index), + GetCalibrationParamFloat(SensorString(), "YCOMP", _calibration_index), + GetCalibrationParamFloat(SensorString(), "ZCOMP", _calibration_index) + }; + _power_compensation = comp; + + return set_offset(offset) && set_scale(scale) && set_offdiagonal(odiag); + } + + return false; +} + +void Magnetometer::Reset() +{ + _offset.zero(); + _scale.setIdentity(); + + _power_compensation.zero(); + _power = 0.f; + + _thermal_offset.zero(); + + _calibration_index = -1; + + _calibration_count = 0; +} + +bool Magnetometer::ParametersSave(int desired_calibration_index, bool force) +{ + _configuration.ParametersSave(); + + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { + _calibration_index = desired_calibration_index; + + } else if (!force || (_calibration_index < 0) + || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) { + + // ensure we have a valid calibration slot (matching existing or first available slot) + int8_t calibration_index_prev = _calibration_index; + _calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index); + + if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(), + calibration_index_prev, _calibration_index); + } + } + + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { + // save calibration + bool success = true; + + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id()); + + // CAL_{}n_{X,Y,Z}OFF + success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0)); + success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1)); + success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2)); + + // CAL_{}n_{X,Y,Z}SCALE + success &= SetCalibrationParam(SensorString(), "XSCALE", _calibration_index, _scale(0, 0)); + success &= SetCalibrationParam(SensorString(), "YSCALE", _calibration_index, _scale(1, 1)); + success &= SetCalibrationParam(SensorString(), "ZSCALE", _calibration_index, _scale(2, 2)); + + // CAL_{}n_{X,Y,Z}ODIAG + success &= SetCalibrationParam(SensorString(), "XODIAG", _calibration_index, _scale(0, 1)); + success &= SetCalibrationParam(SensorString(), "YODIAG", _calibration_index, _scale(0, 2)); + success &= SetCalibrationParam(SensorString(), "ZODIAG", _calibration_index, _scale(1, 2)); + + // CAL_{}n_{X,Y,Z}COMP + success &= SetCalibrationParam(SensorString(), "XCOMP", _calibration_index, _power_compensation(0)); + success &= SetCalibrationParam(SensorString(), "YCOMP", _calibration_index, _power_compensation(1)); + success &= SetCalibrationParam(SensorString(), "ZCOMP", _calibration_index, _power_compensation(2)); + + return success; + } + + return false; +} + +void Magnetometer::PrintStatus() +{ + if (external()) { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n", + SensorString(), device_id(), enabled(), + (double)_offset(0), (double)_offset(1), (double)_offset(2), + (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2), + rotation_enum()); + + } else { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n", + SensorString(), device_id(), enabled(), + (double)_offset(0), (double)_offset(1), (double)_offset(2), + (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2)); + } + + if (_thermal_offset.norm() > 0.f) { + PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(), + (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); + } +} + +} // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor/calibration/Magnetometer.hpp similarity index 69% rename from src/lib/sensor_calibration/Magnetometer.hpp rename to src/lib/sensor/calibration/Magnetometer.hpp index 8d328e7a7d..f2d25e012a 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor/calibration/Magnetometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -33,14 +33,16 @@ #pragma once -#include -#include #include -#include + +#include #include -#include #include +#include + +namespace sensor +{ namespace calibration { class Magnetometer @@ -48,9 +50,6 @@ class Magnetometer public: static constexpr int MAX_SENSOR_COUNT = 4; - static constexpr uint8_t DEFAULT_PRIORITY = 50; - static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; - static constexpr const char *SensorString() { return "MAG"; } Magnetometer(); @@ -62,48 +61,40 @@ public: bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); + bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); bool set_offdiagonal(const matrix::Vector3f &offdiagonal); - /** - * @brief Set the rotation enum & corresponding rotation matrix for Magnetometer - * - * @param rotation Rotation enum - */ - void set_rotation(Rotation rotation); + void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); } - /** - * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer - * - * @param rotation Rotation euler angles - */ - void set_custom_rotation(const matrix::Eulerf &rotation); - - bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } + bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } - uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } - bool external() const { return _external; } + const matrix::Vector3f &offset() const { return _offset; } - const int32_t &priority() const { return _priority; } - const matrix::Dcmf &rotation() const { return _rotation; } - const Rotation &rotation_enum() const { return _rotation_enum; } const matrix::Matrix3f &scale() const { return _scale; } + uint32_t device_id() const { return _configuration.device_id(); } + bool enabled() const { return _configuration.enabled(); } + bool external() const { return _configuration.external(); } + + const int32_t &priority() const { return _configuration.priority(); } + const matrix::Dcmf &rotation() const { return _configuration.rotation(); } + const Rotation &rotation_enum() const { return _configuration.rotation_enum(); } + // apply offsets and scale // rotate corrected measurements from sensor to body frame inline matrix::Vector3f Correct(const matrix::Vector3f &data) const { - return _rotation * (_scale * ((data + _power * _power_compensation) - _offset)); + return _configuration.rotation() * (_scale * ((data + _power * _power_compensation) - _offset)); } // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { // updated calibration offset = existing offset + bias rotated to sensor frame and unscaled - return _offset + (_scale.I() * _rotation.I() * bias); + return _offset + (_scale.I() * _configuration.rotation().I() * bias); } bool ParametersLoad(); @@ -117,17 +108,10 @@ public: void UpdatePower(float power) { _power = power; } private: + sensor::configuration::Magnetometer _configuration{}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; - Rotation _rotation_enum{ROTATION_NONE}; - - /** - * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD) - */ - matrix::Dcmf _rotation; - - matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional) - matrix::Vector3f _offset; matrix::Matrix3f _scale; matrix::Vector3f _thermal_offset; @@ -136,11 +120,8 @@ private: float _power{0.f}; int8_t _calibration_index{-1}; - uint32_t _device_id{0}; - int32_t _priority{-1}; - - bool _external{false}; - uint8_t _calibration_count{0}; }; + } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor/calibration/Utilities.cpp similarity index 58% rename from src/lib/sensor_calibration/Utilities.cpp rename to src/lib/sensor/calibration/Utilities.cpp index f1c864bf3f..50dceb9226 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor/calibration/Utilities.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -31,26 +31,20 @@ * ****************************************************************************/ +#include "Utilities.hpp" + #include #include #include -#include #include #include -#if defined(CONFIG_I2C) -# include -#endif // CONFIG_I2C - -#if defined(CONFIG_SPI) -# include -#endif // CONFIG_SPI - -using math::radians; using matrix::Eulerf; using matrix::Dcmf; using matrix::Vector3f; +namespace sensor +{ namespace calibration { @@ -63,7 +57,7 @@ int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id) } for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { - char str[20] {}; + char str[16 + 1] {}; snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i); int32_t device_id_val = 0; @@ -102,7 +96,7 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id uint32_t cal_device_ids[MAX_SENSOR_COUNT] {}; for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { - char str[20] {}; + char str[16 + 1] {}; snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i); int32_t device_id_val = 0; @@ -136,8 +130,8 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance) { - // eg CAL_MAGn_ID/CAL_MAGn_ROT - char str[20] {}; + // eg CAL_{}n_ID + char str[16 + 1] {}; snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); int32_t value = 0; @@ -151,8 +145,8 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance) { - // eg CAL_BAROn_OFF - char str[20] {}; + // eg CAL_{}n_OFF + char str[16 + 1] {}; snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); float value = NAN; @@ -164,124 +158,5 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui return value; } -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 - snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%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; -} - -bool 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 - snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%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 == PX4_OK; -} - -Eulerf GetSensorLevelAdjustment() -{ - 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); - - return Eulerf{radians(x_offset), radians(y_offset), radians(z_offset)}; -} - -enum Rotation GetBoardRotation() -{ - // get transformation matrix from sensor/board to body frame - int32_t board_rot = -1; - param_get(param_find("SENS_BOARD_ROT"), &board_rot); - - if (board_rot >= 0 && board_rot < Rotation::ROTATION_MAX) { - return static_cast(board_rot); - - } else { - PX4_ERR("invalid SENS_BOARD_ROT: %" PRId32, board_rot); - } - - return Rotation::ROTATION_NONE; -} - -Dcmf GetBoardRotationMatrix() -{ - return get_rot_matrix(GetBoardRotation()); -} - -bool DeviceExternal(uint32_t device_id) -{ - bool external = true; - - // decode device id to determine if external - union device::Device::DeviceId id {}; - id.devid = device_id; - - const device::Device::DeviceBusType bus_type = id.devid_s.bus_type; - - switch (bus_type) { - case device::Device::DeviceBusType_I2C: -#if defined(CONFIG_I2C) - external = px4_i2c_device_external(device_id); -#endif // CONFIG_I2C - break; - - case device::Device::DeviceBusType_SPI: -#if defined(CONFIG_SPI) - external = px4_spi_bus_external(id.devid_s.bus); -#endif // CONFIG_SPI - break; - - case device::Device::DeviceBusType_UAVCAN: - external = true; - break; - - case device::Device::DeviceBusType_SIMULATION: - external = false; - break; - - case device::Device::DeviceBusType_SERIAL: - external = true; - break; - - case device::Device::DeviceBusType_MAVLINK: - external = true; - break; - - case device::Device::DeviceBusType_UNKNOWN: - external = true; - break; - } - - return external; -} - } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor/calibration/Utilities.hpp similarity index 69% rename from src/lib/sensor_calibration/Utilities.hpp rename to src/lib/sensor/calibration/Utilities.hpp index b825ad1496..7bbb94480b 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor/calibration/Utilities.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -40,6 +40,8 @@ #include #include +namespace sensor +{ namespace calibration { @@ -85,9 +87,9 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui template bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, T value) { - char str[20] {}; + char str[16 + 1] {}; - // eg CAL_MAGn_ID/CAL_MAGn_ROT + // eg CAL_{}n_ID snprintf(str, sizeof(str), "CAL_%s%u_%s", sensor_type, instance, cal_type); int ret = param_set_no_notification(param_find(str), &value); @@ -99,54 +101,5 @@ bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t return ret == PX4_OK; } -/** - * @brief Get the Calibration Params Vector 3f object - * - * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") - * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") - * @param instance Calibration index (0 - 3) - * @return matrix::Vector3f Vector of calibration values. - */ -matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance); - -/** - * @brief Set the Calibration Params Vector 3f object - * - * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") - * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") - * @param instance Calibration index (0 - 3) - * @param values Vector of calibration values x, y, z. - * @return true if the parameter name was valid and all values saved successfully, false otherwise. - */ -bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, - matrix::Vector3f values); - -/** - * @brief Get the board sensor level adjustment (SENS_BOARD_X_OFF, SENS_BOARD_Y_OFF, SENS_BOARD_Z_OFF). - * - * @return matrix::Eulerf - */ -matrix::Eulerf GetSensorLevelAdjustment(); - -/** - * @brief Get the board rotation. - * - * @return enum Rotation - */ -Rotation GetBoardRotation(); - -/** - * @brief Get the board rotation Dcm. - * - * @return matrix::Dcmf - */ -matrix::Dcmf GetBoardRotationMatrix(); - -/** - * @brief Determine if device is on an external bus - * - * @return true if device is on an external bus - */ -bool DeviceExternal(uint32_t device_id); - -} // namespace calibration +} // namespace utilities +} // namespace sensor diff --git a/src/lib/sensor/configuration/Accelerometer.cpp b/src/lib/sensor/configuration/Accelerometer.cpp new file mode 100644 index 0000000000..af5943e86e --- /dev/null +++ b/src/lib/sensor/configuration/Accelerometer.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 +#include + +using namespace matrix; +using namespace time_literals; +using namespace sensor::utilities; + +namespace sensor +{ +namespace configuration +{ + +Accelerometer::Accelerometer() +{ + Reset(); +} + +Accelerometer::Accelerometer(uint32_t device_id) +{ + set_device_id(device_id); +} + +void Accelerometer::set_device_id(uint32_t device_id) +{ + bool external = DeviceExternal(device_id); + + if (_device_id != device_id || _external != external) { + + _device_id = device_id; + _external = external; + + Reset(); + + ParametersUpdate(); + } +} + +bool Accelerometer::set_configuration_index(int configuration_index) +{ + if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) { + _configuration_index = configuration_index; + return true; + } + + return false; +} + +void Accelerometer::set_rotation(Rotation rotation) +{ + _rotation_enum = rotation; + + // always apply board level adjustments + _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); +} + +void Accelerometer::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id); + + if (_configuration_index == -1) { + // no saved configuration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool Accelerometer::ParametersLoad() +{ + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // SENS_{}n_ROT + int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index); + + if (_external) { + if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { + // invalid rotation, resetting + rotation_value = ROTATION_NONE; + } + + set_rotation(static_cast(rotation_value)); + + } else { + // internal sensors follow board rotation + set_rotation(GetBoardRotation()); + } + + // SENS_{}n_PRIO + _priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index); + + if ((_priority < 0) || (_priority > 100)) { + // reset to default, -1 is the uninitialized parameter value + static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1; + + if (_priority != SENS_PRIO_UNINITIALIZED) { + PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, + _configuration_index, _priority); + + SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + } + + return true; + } + + return false; +} + +void Accelerometer::Reset() +{ + if (_external) { + set_rotation(ROTATION_NONE); + + } else { + // internal sensors follow board rotation + set_rotation(GetBoardRotation()); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + _configuration_index = -1; + + _configuration_count = 0; +} + +bool Accelerometer::ParametersSave(int desired_configuration_index, bool force) +{ + if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) { + _configuration_index = desired_configuration_index; + + } else if (!force || (_configuration_index < 0) + || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) { + + // ensure we have a valid configuration slot (matching existing or first available slot) + int8_t configuration_index_prev = _configuration_index; + _configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index); + + if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) { + PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + configuration_index_prev, _configuration_index); + } + } + + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // save configuration + bool success = true; + + success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id); + success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority); + + if (_external) { + success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum); + + } else { + success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal + } + + return success; + } + + return false; +} + +void Accelerometer::PrintStatus() +{ + if (external()) { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Ext ROT: %d\n", + SensorString(), device_id(), enabled(), + rotation_enum()); + + } else { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Internal\n", + SensorString(), device_id(), enabled()); + } +} + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Accelerometer.hpp b/src/lib/sensor/configuration/Accelerometer.hpp new file mode 100644 index 0000000000..a16dd197d7 --- /dev/null +++ b/src/lib/sensor/configuration/Accelerometer.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 + +namespace sensor +{ +namespace configuration +{ + +class Accelerometer +{ +public: + static constexpr int MAX_SENSOR_COUNT = 4; + + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + + static constexpr const char *SensorString() { return "ACC"; } + + Accelerometer(); + explicit Accelerometer(uint32_t device_id); + + ~Accelerometer() = default; + + void PrintStatus(); + + bool set_configuration_index(int configuration_index); + void set_device_id(uint32_t device_id); + + void set_rotation(Rotation rotation); + + bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); } + uint8_t configuration_count() const { return _configuration_count; } + int8_t configuration_index() const { return _configuration_index; } + uint32_t device_id() const { return _device_id; } + bool enabled() const { return (_priority > 0); } + bool external() const { return _external; } + + const int32_t &priority() const { return _priority; } + const matrix::Dcmf &rotation() const { return _rotation; } + const Rotation &rotation_enum() const { return _rotation_enum; } + + bool ParametersLoad(); + bool ParametersSave(int desired_configuration_index = -1, bool force = false); + void ParametersUpdate(); + + void Reset(); + +private: + Rotation _rotation_enum{ROTATION_NONE}; + + matrix::Dcmf _rotation; + + uint32_t _device_id{0}; + int32_t _priority{-1}; + + bool _external{false}; + + int8_t _configuration_index{-1}; + uint8_t _configuration_count{0}; +}; + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Barometer.cpp b/src/lib/sensor/configuration/Barometer.cpp new file mode 100644 index 0000000000..99ee577a88 --- /dev/null +++ b/src/lib/sensor/configuration/Barometer.cpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "Barometer.hpp" + +#include "Utilities.hpp" + +#include +#include + +using namespace matrix; +using namespace time_literals; +using namespace sensor::utilities; + +namespace sensor +{ +namespace configuration +{ + +Barometer::Barometer() +{ + Reset(); +} + +Barometer::Barometer(uint32_t device_id) +{ + set_device_id(device_id); +} + +void Barometer::set_device_id(uint32_t device_id) +{ + bool external = DeviceExternal(device_id); + + if (_device_id != device_id || _external != external) { + + _device_id = device_id; + _external = external; + + Reset(); + + ParametersUpdate(); + } +} + +bool Barometer::set_configuration_index(int configuration_index) +{ + if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) { + _configuration_index = configuration_index; + return true; + } + + return false; +} + +void Barometer::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id); + + if (_configuration_index == -1) { + // no saved configuration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool Barometer::ParametersLoad() +{ + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // SENS_{}n_PRIO + _priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index); + + if ((_priority < 0) || (_priority > 100)) { + // reset to default, -1 is the uninitialized parameter value + static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1; + + if (_priority != SENS_PRIO_UNINITIALIZED) { + PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, + _configuration_index, _priority); + + SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + } + + return true; + } + + return false; +} + +void Barometer::Reset() +{ + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + _configuration_index = -1; + + _configuration_count = 0; +} + +bool Barometer::ParametersSave(int desired_configuration_index, bool force) +{ + if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) { + _configuration_index = desired_configuration_index; + + } else if (!force || (_configuration_index < 0) + || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) { + + // ensure we have a valid configuration slot (matching existing or first available slot) + int8_t configuration_index_prev = _configuration_index; + _configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index); + + if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) { + PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + configuration_index_prev, _configuration_index); + } + } + + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // save configuration + bool success = true; + + success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id); + success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority); + + return success; + } + + return false; +} + +void Barometer::PrintStatus() +{ + if (external()) { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, External\n", + SensorString(), device_id(), enabled()); + + } else { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Internal\n", + SensorString(), device_id(), enabled()); + } +} + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Barometer.hpp b/src/lib/sensor/configuration/Barometer.hpp new file mode 100644 index 0000000000..29395c338a --- /dev/null +++ b/src/lib/sensor/configuration/Barometer.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +namespace sensor +{ +namespace configuration +{ + +class Barometer +{ +public: + static constexpr int MAX_SENSOR_COUNT = 4; + + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + + static constexpr const char *SensorString() { return "BARO"; } + + Barometer(); + explicit Barometer(uint32_t device_id); + + ~Barometer() = default; + + void PrintStatus(); + + bool set_configuration_index(int configuration_index); + void set_device_id(uint32_t device_id); + + bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); } + uint8_t configuration_count() const { return _configuration_count; } + int8_t configuration_index() const { return _configuration_index; } + uint32_t device_id() const { return _device_id; } + bool enabled() const { return (_priority > 0); } + bool external() const { return _external; } + + const int32_t &priority() const { return _priority; } + + bool ParametersLoad(); + bool ParametersSave(int desired_configuration_index = -1, bool force = false); + void ParametersUpdate(); + + void Reset(); + +private: + + uint32_t _device_id{0}; + int32_t _priority{-1}; + + bool _external{false}; + + int8_t _configuration_index{-1}; + uint8_t _configuration_count{0}; +}; + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/CMakeLists.txt b/src/lib/sensor/configuration/CMakeLists.txt new file mode 100644 index 0000000000..e1d7696747 --- /dev/null +++ b/src/lib/sensor/configuration/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +px4_add_library(sensor_configuration + Accelerometer.cpp + Accelerometer.hpp + Barometer.cpp + Barometer.hpp + GNSS.cpp + GNSS.hpp + Gyroscope.cpp + Gyroscope.hpp + Magnetometer.cpp + Magnetometer.hpp + Utilities.cpp + Utilities.hpp +) + +target_link_libraries(sensor_configuration + PRIVATE + conversion + sensor_utilities +) diff --git a/src/lib/sensor/configuration/GNSS.cpp b/src/lib/sensor/configuration/GNSS.cpp new file mode 100644 index 0000000000..1c56e6a6f6 --- /dev/null +++ b/src/lib/sensor/configuration/GNSS.cpp @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "GNSS.hpp" + +#include "Utilities.hpp" + +#include +#include + +using namespace matrix; +using namespace time_literals; +using namespace sensor::utilities; + +namespace sensor +{ +namespace configuration +{ + +GNSS::GNSS() +{ + Reset(); +} + +GNSS::GNSS(uint32_t device_id) +{ + set_device_id(device_id); +} + +void GNSS::set_device_id(uint32_t device_id) +{ + if (_device_id != device_id) { + + _device_id = device_id; + + Reset(); + + ParametersUpdate(); + } +} + +bool GNSS::set_configuration_index(int configuration_index) +{ + if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) { + _configuration_index = configuration_index; + return true; + } + + return false; +} + +bool GNSS::set_position(const matrix::Vector3f &position) +{ + static constexpr float MIN_POSITION_CHANGE = 1e-6; + + if (position.isAllFinite() + && (Vector3f(_position - position).longerThan(MIN_POSITION_CHANGE) || (_configuration_count == 0)) + ) { + _position = position; + + _configuration_count++; + return true; + } + + return false; +} + +void GNSS::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id); + + if (_configuration_index == -1) { + // no saved configuration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool GNSS::ParametersLoad() +{ + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // SENS_{}n_PRIO + _priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index); + + if ((_priority < 0) || (_priority > 100)) { + // reset to default, -1 is the uninitialized parameter value + static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1; + + if (_priority != SENS_PRIO_UNINITIALIZED) { + PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, + _configuration_index, _priority); + + SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED); + } + + _priority = DEFAULT_PRIORITY; + } + + // SENS_{}n_{X,Y,Z}POS: + Vector3f position{0.f, 0.f, 0.f}; + position(0) = GetConfigurationParamFloat(SensorString(), "XPOS", _configuration_index); + position(1) = GetConfigurationParamFloat(SensorString(), "YPOS", _configuration_index); + position(2) = GetConfigurationParamFloat(SensorString(), "ZPOS", _configuration_index); + set_position(position); + + return true; + } + + return false; +} + +void GNSS::Reset() +{ + _position.zero(); + + _priority = DEFAULT_PRIORITY; + + _configuration_index = -1; + + _configuration_count = 0; +} + +bool GNSS::ParametersSave(int desired_configuration_index, bool force) +{ + if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) { + _configuration_index = desired_configuration_index; + + } else if (!force || (_configuration_index < 0) + || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) { + + // ensure we have a valid configuration slot (matching existing or first available slot) + int8_t configuration_index_prev = _configuration_index; + _configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index); + + if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) { + PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + configuration_index_prev, _configuration_index); + } + } + + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // save configuration + bool success = true; + + success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id); + success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority); + + // SENS_{}n_{X,Y,Z}POS + success &= SetConfigurationParam(SensorString(), "XPOS", _configuration_index, _position(0)); + success &= SetConfigurationParam(SensorString(), "YPOS", _configuration_index, _position(0)); + success &= SetConfigurationParam(SensorString(), "ZPOS", _configuration_index, _position(0)); + + return success; + } + + return false; +} + +void GNSS::PrintStatus() +{ + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, position: [%05.3f %05.3f %05.3f]\n", + SensorString(), device_id(), enabled(), + (double)_position(0), (double)_position(1), (double)_position(2)); +} + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/GNSS.hpp b/src/lib/sensor/configuration/GNSS.hpp new file mode 100644 index 0000000000..51442dd4cb --- /dev/null +++ b/src/lib/sensor/configuration/GNSS.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +namespace sensor +{ +namespace configuration +{ + +class GNSS +{ +public: + static constexpr int MAX_SENSOR_COUNT = 4; + + static constexpr uint8_t DEFAULT_PRIORITY = 50; + + static constexpr const char *SensorString() { return "GNSS"; } + + GNSS(); + explicit GNSS(uint32_t device_id); + + ~GNSS() = default; + + void PrintStatus(); + + bool set_configuration_index(int configuration_index); + void set_device_id(uint32_t device_id); + + bool set_position(const matrix::Vector3f &position); + + bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); } + uint8_t configuration_count() const { return _configuration_count; } + int8_t configuration_index() const { return _configuration_index; } + uint32_t device_id() const { return _device_id; } + bool enabled() const { return (_priority > 0); } + + const matrix::Vector3f &position() const { return _position; } + const int32_t &priority() const { return _priority; } + + bool ParametersLoad(); + bool ParametersSave(int desired_configuration_index = -1, bool force = false); + void ParametersUpdate(); + + void Reset(); + +private: + + uint32_t _device_id{0}; + int32_t _priority{-1}; + + matrix::Vector3f _position{}; + + int8_t _configuration_index{-1}; + uint8_t _configuration_count{0}; +}; + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Gyroscope.cpp b/src/lib/sensor/configuration/Gyroscope.cpp new file mode 100644 index 0000000000..73d4010bcb --- /dev/null +++ b/src/lib/sensor/configuration/Gyroscope.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 +#include + +using namespace matrix; +using namespace time_literals; +using namespace sensor::utilities; + +namespace sensor +{ +namespace configuration +{ + +Gyroscope::Gyroscope() +{ + Reset(); +} + +Gyroscope::Gyroscope(uint32_t device_id) +{ + set_device_id(device_id); +} + +void Gyroscope::set_device_id(uint32_t device_id) +{ + bool external = DeviceExternal(device_id); + + if (_device_id != device_id || _external != external) { + + _device_id = device_id; + _external = external; + + Reset(); + + ParametersUpdate(); + } +} + +bool Gyroscope::set_configuration_index(int configuration_index) +{ + if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) { + _configuration_index = configuration_index; + return true; + } + + return false; +} + +void Gyroscope::set_rotation(Rotation rotation) +{ + _rotation_enum = rotation; + + // always apply board level adjustments + _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); +} + +void Gyroscope::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id); + + if (_configuration_index == -1) { + // no saved configuration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool Gyroscope::ParametersLoad() +{ + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // SENS_{}n_ROT + int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index); + + if (_external) { + if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { + // invalid rotation, resetting + rotation_value = ROTATION_NONE; + } + + set_rotation(static_cast(rotation_value)); + + } else { + // internal sensors follow board rotation + set_rotation(GetBoardRotation()); + } + + // SENS_{}n_PRIO + _priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index); + + if ((_priority < 0) || (_priority > 100)) { + // reset to default, -1 is the uninitialized parameter value + static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1; + + if (_priority != SENS_PRIO_UNINITIALIZED) { + PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, + _configuration_index, _priority); + + SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + } + + return true; + } + + return false; +} + +void Gyroscope::Reset() +{ + if (_external) { + set_rotation(ROTATION_NONE); + + } else { + // internal sensors follow board rotation + set_rotation(GetBoardRotation()); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + _configuration_index = -1; + + _configuration_count = 0; +} + +bool Gyroscope::ParametersSave(int desired_configuration_index, bool force) +{ + if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) { + _configuration_index = desired_configuration_index; + + } else if (!force || (_configuration_index < 0) + || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) { + + // ensure we have a valid configuration slot (matching existing or first available slot) + int8_t configuration_index_prev = _configuration_index; + _configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index); + + if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) { + PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + configuration_index_prev, _configuration_index); + } + } + + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // save configuration + bool success = true; + + success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id); + success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority); + + if (_external) { + success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum); + + } else { + success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal + } + + return success; + } + + return false; +} + +void Gyroscope::PrintStatus() +{ + if (external()) { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Ext ROT: %d\n", + SensorString(), device_id(), enabled(), + rotation_enum()); + + } else { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Internal\n", + SensorString(), device_id(), enabled()); + } +} + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Gyroscope.hpp b/src/lib/sensor/configuration/Gyroscope.hpp new file mode 100644 index 0000000000..3114739932 --- /dev/null +++ b/src/lib/sensor/configuration/Gyroscope.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 + +namespace sensor +{ +namespace configuration +{ + +class Gyroscope +{ +public: + static constexpr int MAX_SENSOR_COUNT = 4; + + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + + static constexpr const char *SensorString() { return "GYRO"; } + + Gyroscope(); + explicit Gyroscope(uint32_t device_id); + + ~Gyroscope() = default; + + void PrintStatus(); + + bool set_configuration_index(int configuration_index); + void set_device_id(uint32_t device_id); + + void set_rotation(Rotation rotation); + + bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); } + uint8_t configuration_count() const { return _configuration_count; } + int8_t configuration_index() const { return _configuration_index; } + uint32_t device_id() const { return _device_id; } + bool enabled() const { return (_priority > 0); } + bool external() const { return _external; } + + const int32_t &priority() const { return _priority; } + const matrix::Dcmf &rotation() const { return _rotation; } + const Rotation &rotation_enum() const { return _rotation_enum; } + + bool ParametersLoad(); + bool ParametersSave(int desired_configuration_index = -1, bool force = false); + void ParametersUpdate(); + + void Reset(); + +private: + Rotation _rotation_enum{ROTATION_NONE}; + + matrix::Dcmf _rotation; + + uint32_t _device_id{0}; + int32_t _priority{-1}; + + bool _external{false}; + + int8_t _configuration_index{-1}; + uint8_t _configuration_count{0}; +}; + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Magnetometer.cpp b/src/lib/sensor/configuration/Magnetometer.cpp new file mode 100644 index 0000000000..39bfe362ec --- /dev/null +++ b/src/lib/sensor/configuration/Magnetometer.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 "Magnetometer.hpp" + +#include "Utilities.hpp" + +#include +#include + +using namespace matrix; +using namespace time_literals; +using namespace sensor::utilities; + +namespace sensor +{ +namespace configuration +{ + +Magnetometer::Magnetometer() +{ + Reset(); +} + +Magnetometer::Magnetometer(uint32_t device_id) +{ + set_device_id(device_id); +} + +void Magnetometer::set_device_id(uint32_t device_id) +{ + bool external = DeviceExternal(device_id); + + if (_device_id != device_id || _external != external) { + + _device_id = device_id; + _external = external; + + Reset(); + + ParametersUpdate(); + } +} + +bool Magnetometer::set_configuration_index(int configuration_index) +{ + if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) { + _configuration_index = configuration_index; + return true; + } + + return false; +} + +void Magnetometer::set_rotation(const Rotation rotation) +{ + if (rotation < ROTATION_MAX) { + _rotation_enum = rotation; + + } else { + // invalid rotation, resetting + _rotation_enum = ROTATION_NONE; + } + + // always apply level adjustments + _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum); + + // clear any custom rotation + _rotation_custom_euler.zero(); +} + +void Magnetometer::set_custom_rotation(const Eulerf &rotation) +{ + _rotation_enum = ROTATION_CUSTOM; + + // store custom rotation + _rotation_custom_euler = rotation; + + // always apply board level adjustments + _rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler); + + // TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation + // between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot. + // however, since while doing the 'level-configuration', users don't put the vehicle truly *horizontal, the + // measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply + // internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset + // values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective) +} + +void Magnetometer::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id); + + if (_configuration_index == -1) { + // no saved configuration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool Magnetometer::ParametersLoad() +{ + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // SENS_{}n_ROT + int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index); + + const float euler_roll_deg = GetConfigurationParamFloat(SensorString(), "ROLL", _configuration_index); + const float euler_pitch_deg = GetConfigurationParamFloat(SensorString(), "PITCH", _configuration_index); + const float euler_yaw_deg = GetConfigurationParamFloat(SensorString(), "YAW", _configuration_index); + + if (_external) { + if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) { + // invalid rotation, resetting + rotation_value = ROTATION_NONE; + } + + // if SENS_{}n_{ROLL,PITCH,YAW} manually set then SENS_{}n_ROT needs to be ROTATION_CUSTOM + if ((rotation_value != ROTATION_CUSTOM) + && ((fabsf(euler_roll_deg) > FLT_EPSILON) + || (fabsf(euler_pitch_deg) > FLT_EPSILON) + || (fabsf(euler_yaw_deg) > FLT_EPSILON))) { + + rotation_value = ROTATION_CUSTOM; + SetConfigurationParam(SensorString(), "ROT", _configuration_index, rotation_value); + } + + // Handle custom specified euler angle + if (rotation_value == ROTATION_CUSTOM) { + + const matrix::Eulerf rotation_custom_euler{ + math::radians(euler_roll_deg), + math::radians(euler_pitch_deg), + math::radians(euler_yaw_deg)}; + + set_custom_rotation(rotation_custom_euler); + + } else { + set_rotation(static_cast(rotation_value)); + } + + } else { + // internal sensors follow board rotation + set_rotation(GetBoardRotation()); + } + + // SENS_{}n_PRIO + _priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index); + + if ((_priority < 0) || (_priority > 100)) { + // reset to default, -1 is the uninitialized parameter value + static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1; + + if (_priority != SENS_PRIO_UNINITIALIZED) { + PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, + _configuration_index, _priority); + + SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + } + + return true; + } + + return false; +} + +void Magnetometer::Reset() +{ + if (_external) { + set_rotation(ROTATION_NONE); + + } else { + // internal sensors follow board rotation + set_rotation(GetBoardRotation()); + } + + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + _configuration_index = -1; + + _configuration_count = 0; +} + +bool Magnetometer::ParametersSave(int desired_configuration_index, bool force) +{ + if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) { + _configuration_index = desired_configuration_index; + + } else if (!force || (_configuration_index < 0) + || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) { + + // ensure we have a valid configuration slot (matching existing or first available slot) + int8_t configuration_index_prev = _configuration_index; + _configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index); + + if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) { + PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + configuration_index_prev, _configuration_index); + } + } + + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // save configuration + bool success = true; + + success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id); + success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority); + + if (_external) { + success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum); + + } else { + success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal + } + + success &= SetConfigurationParam(SensorString(), "ROLL", _configuration_index, + math::degrees(_rotation_custom_euler(0))); + success &= SetConfigurationParam(SensorString(), "PITCH", _configuration_index, + math::degrees(_rotation_custom_euler(1))); + success &= SetConfigurationParam(SensorString(), "YAW", _configuration_index, + math::degrees(_rotation_custom_euler(2))); + + return success; + } + + return false; +} + +void Magnetometer::PrintStatus() +{ + if (external()) { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Ext ROT: %d\n", + SensorString(), device_id(), enabled(), + rotation_enum()); + + } else { + PX4_INFO_RAW("%s %" PRIu32 + " EN: %d, Internal\n", + SensorString(), device_id(), enabled()); + } +} + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Magnetometer.hpp b/src/lib/sensor/configuration/Magnetometer.hpp new file mode 100644 index 0000000000..b07b8e997c --- /dev/null +++ b/src/lib/sensor/configuration/Magnetometer.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2023 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 + +namespace sensor +{ +namespace configuration +{ + +class Magnetometer +{ +public: + static constexpr int MAX_SENSOR_COUNT = 4; + + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + + static constexpr const char *SensorString() { return "MAG"; } + + Magnetometer(); + explicit Magnetometer(uint32_t device_id); + + ~Magnetometer() = default; + + void PrintStatus(); + + bool set_configuration_index(int configuration_index); + void set_device_id(uint32_t device_id); + + void set_rotation(Rotation rotation); + + /** + * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer + * + * @param rotation Rotation euler angles + */ + void set_custom_rotation(const matrix::Eulerf &rotation); + + bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); } + uint8_t configuration_count() const { return _configuration_count; } + int8_t configuration_index() const { return _configuration_index; } + uint32_t device_id() const { return _device_id; } + bool enabled() const { return (_priority > 0); } + bool external() const { return _external; } + + const int32_t &priority() const { return _priority; } + const matrix::Dcmf &rotation() const { return _rotation; } + const Rotation &rotation_enum() const { return _rotation_enum; } + + bool ParametersLoad(); + bool ParametersSave(int desired_configuration_index = -1, bool force = false); + void ParametersUpdate(); + + void Reset(); + +private: + Rotation _rotation_enum{ROTATION_NONE}; + + /** + * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD) + */ + matrix::Dcmf _rotation; + + matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional) + + uint32_t _device_id{0}; + int32_t _priority{-1}; + + bool _external{false}; + + int8_t _configuration_index{-1}; + uint8_t _configuration_count{0}; +}; + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Utilities.cpp b/src/lib/sensor/configuration/Utilities.cpp new file mode 100644 index 0000000000..1965c8a98a --- /dev/null +++ b/src/lib/sensor/configuration/Utilities.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "Utilities.hpp" + +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Dcmf; +using matrix::Vector3f; + +namespace sensor +{ +namespace configuration +{ + +static constexpr int MAX_SENSOR_COUNT = 4; // TODO: per sensor? + +int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id) +{ + if (device_id == 0) { + return -1; + } + + for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { + char str[16 + 1] {}; + snprintf(str, sizeof(str), "SENS_%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; +} + +int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index) +{ + // if this device is already using a configuration slot then keep it + int configuration_index = FindCurrentConfigurationIndex(sensor_type, device_id); + + if (configuration_index >= 0) { + return configuration_index; + } + + + // device isn't currently using a configuration slot, select user preference (preferred_index) + // if available, otherwise use the first available slot + uint32_t conf_device_ids[MAX_SENSOR_COUNT] {}; + + for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { + char str[16 + 1] {}; + snprintf(str, sizeof(str), "SENS_%s%u_ID", sensor_type, i); + int32_t device_id_val = 0; + + if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) { + conf_device_ids[i] = device_id_val; + } + } + + // use preferred_index if it's available + if ((preferred_index >= 0) && (preferred_index < MAX_SENSOR_COUNT) + && (conf_device_ids[preferred_index] == 0)) { + + configuration_index = preferred_index; + + } else { + // otherwise select first available slot + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (conf_device_ids[i] == 0) { + configuration_index = i; + break; + } + } + } + + if (configuration_index == -1) { + PX4_ERR("no %s configuration slots available", sensor_type); + } + + return configuration_index; +} + +int32_t GetConfigurationParamInt32(const char *sensor_type, const char *conf_type, uint8_t instance) +{ + // eg SENS_{}n_ID + char str[16 + 1] {}; + snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, conf_type); + + int32_t value = 0; + + if (param_get(param_find(str), &value) != 0) { + PX4_ERR("failed to get %s", str); + } + + return value; +} + +float GetConfigurationParamFloat(const char *sensor_type, const char *conf_type, uint8_t instance) +{ + // eg SENS_{}n_OFF + char str[16 + 1] {}; + snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, conf_type); + + float value = NAN; + + if (param_get(param_find(str), &value) != 0) { + PX4_ERR("failed to get %s", str); + } + + return value; +} + +} // namespace configuration +} // namespace sensor diff --git a/src/lib/sensor/configuration/Utilities.hpp b/src/lib/sensor/configuration/Utilities.hpp new file mode 100644 index 0000000000..1d2251115e --- /dev/null +++ b/src/lib/sensor/configuration/Utilities.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 sensor +{ +namespace configuration +{ + +/** + * @brief Find sensor's configuration index if it exists. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param device_id + * @return int8_t Valid configuration index on success, -1 otherwise + */ +int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id); + +/** + * @brief Find sensor's configuration index if it exists, otherwise select an available slot. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param device_id + * @param preferred_index preferred index (optional) + * @return int8_t Valid configuration index on success, -1 otherwise + */ +int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index = -1); + +/** + * @brief Get sensor configuration parameter value. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param conf_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance + * @return int32_t The configuration value. + */ +int32_t GetConfigurationParamInt32(const char *sensor_type, const char *conf_type, uint8_t instance); +float GetConfigurationParamFloat(const char *sensor_type, const char *conf_type, uint8_t instance); + +/** + * @brief Set a single configuration paramter. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GNSS", "GYRO", "MAG") + * @param conf_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance Configuration index (0 - 3) + * @param value int32_t parameter value + * @return true if the parameter name was valid and value saved successfully, false otherwise. + */ +template +bool SetConfigurationParam(const char *sensor_type, const char *conf_type, uint8_t instance, T value) +{ + char str[16 + 1] {}; + + // eg SENS_{}n_ID + snprintf(str, sizeof(str), "SENS_%s%u_%s", sensor_type, instance, conf_type); + + int ret = param_set_no_notification(param_find(str), &value); + + if (ret != PX4_OK) { + PX4_ERR("failed to set %s", str); + } + + return ret == PX4_OK; +} + +} // namespace utilities +} // namespace sensor diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp deleted file mode 100644 index 95079371ec..0000000000 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ /dev/null @@ -1,400 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "Magnetometer.hpp" - -#include "Utilities.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -namespace calibration -{ - -Magnetometer::Magnetometer() -{ - Reset(); -} - -Magnetometer::Magnetometer(uint32_t device_id) -{ - set_device_id(device_id); -} - -void Magnetometer::set_device_id(uint32_t device_id) -{ - bool external = DeviceExternal(device_id); - - if (_device_id != device_id || _external != external) { - - _device_id = device_id; - _external = external; - - Reset(); - - ParametersUpdate(); - SensorCorrectionsUpdate(true); - } -} - -void Magnetometer::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.mag_device_ids[i] == _device_id) { - switch (i) { - case 0: - _thermal_offset = Vector3f{corrections.mag_offset_0}; - return; - case 1: - _thermal_offset = Vector3f{corrections.mag_offset_1}; - return; - case 2: - _thermal_offset = Vector3f{corrections.mag_offset_2}; - return; - case 3: - _thermal_offset = Vector3f{corrections.mag_offset_3}; - return; - } - } - } - } - - // zero thermal offset if not found - _thermal_offset.zero(); - } -} - -bool Magnetometer::set_offset(const Vector3f &offset) -{ - if (Vector3f(_offset - offset).longerThan(0.005f)) { - if (offset.isAllFinite()) { - _offset = offset; - _calibration_count++; - return true; - } - } - - return false; -} - -bool Magnetometer::set_scale(const Vector3f &scale) -{ - if (Vector3f(_scale.diag() - scale).longerThan(0.01f)) { - if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) { - _scale(0, 0) = scale(0); - _scale(1, 1) = scale(1); - _scale(2, 2) = scale(2); - - _calibration_count++; - return true; - } - } - - return false; -} - -bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal) -{ - if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)) { - if (offdiagonal.isAllFinite()) { - - _scale(0, 1) = offdiagonal(0); - _scale(1, 0) = offdiagonal(0); - - _scale(0, 2) = offdiagonal(1); - _scale(2, 0) = offdiagonal(1); - - _scale(1, 2) = offdiagonal(2); - _scale(2, 1) = offdiagonal(2); - - _calibration_count++; - return true; - } - } - - return false; -} - -void Magnetometer::set_rotation(const Rotation rotation) -{ - if (rotation < ROTATION_MAX) { - _rotation_enum = rotation; - - } else { - // invalid rotation, resetting - _rotation_enum = ROTATION_NONE; - } - - // always apply level adjustments - _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum); - - // clear any custom rotation - _rotation_custom_euler.zero(); -} - -void Magnetometer::set_custom_rotation(const Eulerf &rotation) -{ - _rotation_enum = ROTATION_CUSTOM; - - // store custom rotation - _rotation_custom_euler = rotation; - - // always apply board level adjustments - _rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler); - - // TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation - // between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot. - // however, since while doing the 'level-calibration', users don't put the vehicle truly *horizontal, the - // measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply - // internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset - // values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective) -} - -bool Magnetometer::set_calibration_index(int calibration_index) -{ - if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { - _calibration_index = calibration_index; - return true; - } - - return false; -} - -void Magnetometer::ParametersUpdate() -{ - if (_device_id == 0) { - return; - } - - _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); - - if (_calibration_index == -1) { - // no saved calibration available - Reset(); - - } else { - ParametersLoad(); - } -} - -bool Magnetometer::ParametersLoad() -{ - if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { - // CAL_MAGx_ROT - int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); - - const float euler_roll_deg = GetCalibrationParamFloat(SensorString(), "ROLL", _calibration_index); - const float euler_pitch_deg = GetCalibrationParamFloat(SensorString(), "PITCH", _calibration_index); - const float euler_yaw_deg = GetCalibrationParamFloat(SensorString(), "YAW", _calibration_index); - - if (_external) { - if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) { - // invalid rotation, resetting - rotation_value = ROTATION_NONE; - } - - // if CAL_MAGx_{ROLL,PITCH,YAW} manually set then CAL_MAGx_ROT needs to be ROTATION_CUSTOM - if ((rotation_value != ROTATION_CUSTOM) - && ((fabsf(euler_roll_deg) > FLT_EPSILON) - || (fabsf(euler_pitch_deg) > FLT_EPSILON) - || (fabsf(euler_yaw_deg) > FLT_EPSILON))) { - - rotation_value = ROTATION_CUSTOM; - SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value); - } - - // Handle custom specified euler angle - if (rotation_value == ROTATION_CUSTOM) { - - const matrix::Eulerf rotation_custom_euler{ - math::radians(euler_roll_deg), - math::radians(euler_pitch_deg), - math::radians(euler_yaw_deg)}; - - set_custom_rotation(rotation_custom_euler); - - } else { - set_rotation(static_cast(rotation_value)); - } - - } else { - // internal sensors follow board rotation - set_rotation(GetBoardRotation()); - } - - // CAL_MAGx_PRIO - _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); - - if ((_priority < 0) || (_priority > 100)) { - // reset to default, -1 is the uninitialized parameter value - static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1; - - if (_priority != CAL_PRIO_UNINITIALIZED) { - PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id, - _calibration_index, _priority); - - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED); - } - - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - } - - // CAL_MAGx_OFF{X,Y,Z} - set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); - - // CAL_MAGx_SCALE{X,Y,Z} - set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index)); - - // CAL_MAGx_ODIAG{X,Y,Z} - set_offdiagonal(GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index)); - - // CAL_MAGx_COMP{X,Y,Z} - _power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index); - - return true; - } - - return false; -} - -void Magnetometer::Reset() -{ - if (_external) { - set_rotation(ROTATION_NONE); - - } else { - // internal sensors follow board rotation - set_rotation(GetBoardRotation()); - } - - _offset.zero(); - _scale.setIdentity(); - - _power_compensation.zero(); - _power = 0.f; - - _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; - - _calibration_index = -1; - - _calibration_count = 0; -} - -bool Magnetometer::ParametersSave(int desired_calibration_index, bool force) -{ - if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { - _calibration_index = desired_calibration_index; - - } else if (!force || (_calibration_index < 0) - || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) { - - // ensure we have a valid calibration slot (matching existing or first available slot) - int8_t calibration_index_prev = _calibration_index; - _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); - - if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { - PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, - calibration_index_prev, _calibration_index); - } - } - - if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { - // save calibration - bool success = true; - success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); - success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); - - const Vector3f scale{_scale.diag()}; - success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale); - - const Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)}; - success &= SetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index, off_diag); - - success &= SetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index, _power_compensation); - - if (_external) { - success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); - - } else { - success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal - } - - success &= SetCalibrationParam(SensorString(), "ROLL", _calibration_index, math::degrees(_rotation_custom_euler(0))); - success &= SetCalibrationParam(SensorString(), "PITCH", _calibration_index, math::degrees(_rotation_custom_euler(1))); - success &= SetCalibrationParam(SensorString(), "YAW", _calibration_index, math::degrees(_rotation_custom_euler(2))); - - return success; - } - - return false; -} - -void Magnetometer::PrintStatus() -{ - if (external()) { - PX4_INFO_RAW("%s %" PRIu32 - " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n", - SensorString(), device_id(), enabled(), - (double)_offset(0), (double)_offset(1), (double)_offset(2), - (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2), - rotation_enum()); - - } else { - PX4_INFO_RAW("%s %" PRIu32 - " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n", - SensorString(), device_id(), enabled(), - (double)_offset(0), (double)_offset(1), (double)_offset(2), - (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2)); - } - -#if defined(DEBUG_BUILD) - _scale.print(); -#endif // DEBUG_BUILD -} - -} // namespace calibration diff --git a/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp index c7e0684dd8..09aae86d86 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp @@ -33,7 +33,7 @@ #include "accelerometerCheck.hpp" -#include +#include using namespace time_literals; @@ -59,7 +59,7 @@ void AccelerometerChecks::checkAndReport(const Context &context, Report &reporte is_calibration_valid = true; } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0); + is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0); } reporter.setIsPresent(health_component_t::gyro); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp index 53f9b749b3..c4594f4f56 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include class AccelerometerChecks : public HealthAndArmingCheckBase { @@ -52,6 +52,6 @@ public: private: bool isAccelRequired(int instance); - uORB::SubscriptionMultiArray _sensor_accel_sub{ORB_ID::sensor_accel}; + uORB::SubscriptionMultiArray _sensor_accel_sub{ORB_ID::sensor_accel}; uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp index 8689f9ff96..cfce0db93c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include class BaroChecks : public HealthAndArmingCheckBase { @@ -52,7 +52,7 @@ public: private: bool isBaroRequired(int instance); - uORB::SubscriptionMultiArray _sensor_baro_sub{ORB_ID::sensor_baro}; + uORB::SubscriptionMultiArray _sensor_baro_sub{ORB_ID::sensor_baro}; uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp index 778f48dd75..af769a7491 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp @@ -33,7 +33,7 @@ #include "gyroCheck.hpp" -#include +#include using namespace time_literals; @@ -59,7 +59,7 @@ void GyroChecks::checkAndReport(const Context &context, Report &reporter) is_calibration_valid = true; } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0); + is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0); } reporter.setIsPresent(health_component_t::gyro); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp index 705e27b0af..d22de04580 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include class GyroChecks : public HealthAndArmingCheckBase { @@ -52,6 +52,6 @@ public: private: bool isGyroRequired(int instance); - uORB::SubscriptionMultiArray _sensor_gyro_sub{ORB_ID::sensor_gyro}; + uORB::SubscriptionMultiArray _sensor_gyro_sub{ORB_ID::sensor_gyro}; uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 117a23a2b8..fbbf45445b 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -33,7 +33,8 @@ #include "magnetometerCheck.hpp" -#include +#include +#include using namespace time_literals; @@ -68,14 +69,18 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter num_enabled_and_valid_calibration++; } else { - int calibration_index = calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id); + int calibration_index = sensor::calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id); is_calibration_valid = (calibration_index >= 0); if (is_calibration_valid) { - int priority = calibration::GetCalibrationParamInt32("MAG", "PRIO", calibration_index); + int configuration_index = sensor::configuration::FindCurrentConfigurationIndex("MAG", mag_data.device_id); - if (priority > 0) { - ++num_enabled_and_valid_calibration; + if (configuration_index >= 0) { + int priority = sensor::configuration::GetConfigurationParamInt32("MAG", "PRIO", configuration_index); + + if (priority > 0) { + ++num_enabled_and_valid_calibration; + } } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp index 510dd305ab..a05f06e1b6 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include class MagnetometerChecks : public HealthAndArmingCheckBase { @@ -54,7 +54,7 @@ private: bool isMagRequired(int instance, bool &mag_fault); void consistencyCheck(const Context &context, Report &reporter); - uORB::SubscriptionMultiArray _sensor_mag_sub{ORB_ID::sensor_mag}; + uORB::SubscriptionMultiArray _sensor_mag_sub{ORB_ID::sensor_mag}; uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; uORB::Subscription _sensor_preflight_mag_sub{ORB_ID(sensor_preflight_mag)}; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 63d04957af..0520341d03 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -131,8 +131,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -229,7 +229,7 @@ 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::GetBoardRotationMatrix(); + const Dcmf board_rotation = sensor::utilities::GetBoardRotationMatrix(); for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { accel_sum[s] = board_rotation * accel_sum[s]; @@ -325,7 +325,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) { calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; + sensor::calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; unsigned active_sensors = 0; for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) { @@ -360,7 +360,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data, false) == calibrate_return_ok) { - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); + const Dcmf board_rotation = sensor::utilities::GetBoardRotationMatrix(); const Dcmf board_rotation_t = board_rotation.transpose(); bool param_save = false; @@ -556,7 +556,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) calibrated = true; } - calibration::Accelerometer calibration{arp.device_id}; + sensor::calibration::Accelerometer calibration{arp.device_id}; if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) || !offset.isAllFinite()) { PX4_ERR("accel %d quick calibrate failed", accel_index); diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index ab5267d2d6..781994ff5f 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -49,8 +49,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -77,7 +77,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub) uORB::SubscriptionMultiArray sensor_baro_subs{ORB_ID::sensor_baro}; - calibration::Barometer calibration[MAX_SENSOR_COUNT] {}; + sensor::calibration::Barometer calibration[MAX_SENSOR_COUNT] {}; uint64_t timestamp_sample_sum[MAX_SENSOR_COUNT] {0}; float data_sum[MAX_SENSOR_COUNT] {}; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 0ebcaebfc1..4abc42560e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -52,8 +52,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -68,7 +68,7 @@ using matrix::Vector3f; struct gyro_worker_data_t { orb_advert_t *mavlink_log_pub{nullptr}; - calibration::Gyroscope calibrations[MAX_GYROS] {}; + sensor::calibration::Gyroscope calibrations[MAX_GYROS] {}; Vector3f offset[MAX_GYROS] {}; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 3fff697d49..895bd0048b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -50,8 +50,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -94,7 +94,7 @@ struct mag_worker_data_t { float *y[MAX_MAGS]; float *z[MAX_MAGS]; - calibration::Magnetometer calibration[MAX_MAGS] {}; + sensor::calibration::Magnetometer calibration[MAX_MAGS] {}; }; int do_mag_calibration(orb_advert_t *mavlink_log_pub) @@ -714,7 +714,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // only proceed if there's a valid internal if (internal_index >= 0) { - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); + const Dcmf board_rotation = sensor::utilities::GetBoardRotationMatrix(); // apply new calibrations to all raw sensor data before comparison for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { @@ -1024,7 +1024,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) { - calibration::Magnetometer cal{mag.device_id}; + sensor::calibration::Magnetometer cal{mag.device_id}; // use any existing scale and store the offset to the expected earth field const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index aba810ceb4..7d3662f19f 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -177,6 +177,7 @@ struct gpsMessage { bool vel_ned_valid{}; ///< GPS ground speed is valid uint8_t nsats{}; ///< number of satellites used float pdop{}; ///< position dilution of precision + Vector3f position_body{}; ///< xyz position of the GPS antenna in body frame (m) }; struct imuSample { @@ -198,6 +199,7 @@ struct gpsSample { float vacc{}; ///< 1-std vertical position error (m) float sacc{}; ///< 1-std speed error (m/sec) float yaw_acc{}; ///< 1-std yaw error (rad) + Vector3f position_body{}; ///< xyz position of the GPS antenna in body frame (m) }; struct magSample { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 850e409bdb..e5fe2ec208 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -217,6 +217,8 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) gps_sample_new.pos(1) = 0.0f; } + gps_sample_new.position_body = gps.position_body; + _gps_buffer->push(gps_sample_new); _time_last_gps_buffer_push = _time_latest_us; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 4e1afc5ff8..ee4d7ccade 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -53,7 +53,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_gps_data_ready) { // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + const Vector3f gps_pos_body = _gps_sample_delayed.position_body; + const Vector3f pos_offset_body = gps_pos_body - _params.imu_pos_body; const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bafb94bab5..e58976f686 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -76,9 +76,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #if defined(CONFIG_EKF2_GNSS) _param_ekf2_gps_ctrl(_params->gnss_ctrl), _param_ekf2_gps_delay(_params->gps_delay_ms), - _param_ekf2_gps_pos_x(_params->gps_pos_body(0)), - _param_ekf2_gps_pos_y(_params->gps_pos_body(1)), - _param_ekf2_gps_pos_z(_params->gps_pos_body(2)), _param_ekf2_gps_v_noise(_params->gps_vel_noise), _param_ekf2_gps_p_noise(_params->gps_pos_noise), _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), @@ -2454,6 +2451,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) .nsats = vehicle_gps_position.satellites_used, .pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop + vehicle_gps_position.vdop * vehicle_gps_position.vdop), + .position_body = Vector3f{vehicle_gps_position.position_offset}, }; _ekf.setGpsData(gps_msg); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 060ce108db..359c0b7e0e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -545,10 +545,6 @@ private: (ParamExtInt) _param_ekf2_gps_ctrl, (ParamExtFloat) _param_ekf2_gps_delay, - (ParamExtFloat) _param_ekf2_gps_pos_x, - (ParamExtFloat) _param_ekf2_gps_pos_y, - (ParamExtFloat) _param_ekf2_gps_pos_z, - (ParamExtFloat) _param_ekf2_gps_v_noise, (ParamExtFloat) _param_ekf2_gps_p_noise, diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 2916b78c2f..69df24aa61 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -941,33 +941,6 @@ PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Y, 0.0f); */ PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Z, 0.0f); -/** - * X position of GPS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_POS_X, 0.0f); - -/** - * Y position of GPS antenna in body frame (right axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Y, 0.0f); - -/** - * Z position of GPS antenna in body frame (down axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Z, 0.0f); - /** * X position of range finder origin in body frame (forward axis with origin relative to vehicle centre of gravity) * diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp index 53092855d0..2c06efa374 100644 --- a/src/modules/gyro_calibration/GyroCalibration.hpp +++ b/src/modules/gyro_calibration/GyroCalibration.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include #include @@ -96,7 +96,7 @@ private: uORB::SubscriptionMultiArray _sensor_accel_subs{ORB_ID::sensor_accel}; uORB::SubscriptionMultiArray _sensor_gyro_subs{ORB_ID::sensor_gyro}; - calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; + sensor::calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; math::WelfordMeanVector _gyro_mean[MAX_SENSORS] {}; matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {}; float _temperature[MAX_SENSORS] {}; diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp index d47e7d0a8f..98be94675c 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include @@ -93,7 +93,7 @@ private: uORB::Publication _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)}; - calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; + sensor::calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; hrt_abstime _time_valid[MAX_SENSOR_COUNT] {}; diff --git a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp index d52e26e718..64706ffe58 100644 --- a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp +++ b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp @@ -34,7 +34,7 @@ #ifndef MAG_CAL_REPORT_HPP #define MAG_CAL_REPORT_HPP -#include +#include #include #include @@ -73,7 +73,7 @@ private: sensor_mag_s sensor_mag; if (_sensor_mag_subs[mag].update(&sensor_mag) && (sensor_mag.device_id != 0)) { - calibration::Magnetometer calibration{sensor_mag.device_id}; + sensor::calibration::Magnetometer calibration{sensor_mag.device_id}; if (calibration.calibrated()) { mavlink_mag_cal_report_t msg{}; diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 1f84dec4db..d4c7d3398a 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -78,6 +78,7 @@ px4_add_module( data_validator mathlib sensor_calibration + sensor_configuration vehicle_imu ) diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml index 5fa440d5ff..4257b769fd 100644 --- a/src/modules/sensors/module.yaml +++ b/src/modules/sensors/module.yaml @@ -14,81 +14,6 @@ parameters: category: System type: int32 default: 0 - decimal: 3 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_PRIO: - description: - short: Accelerometer ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - decimal: 3 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_ROT: - description: - short: Accelerometer ${i} rotation relative to airframe - long: | - An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. - category: System - type: enum - values: - -1: Internal - 0: No rotation - 1: Yaw 45° - 2: Yaw 90° - 3: Yaw 135° - 4: Yaw 180° - 5: Yaw 225° - 6: Yaw 270° - 7: Yaw 315° - 8: Roll 180° - 9: Roll 180°, Yaw 45° - 10: Roll 180°, Yaw 90° - 11: Roll 180°, Yaw 135° - 12: Pitch 180° - 13: Roll 180°, Yaw 225° - 14: Roll 180°, Yaw 270° - 15: Roll 180°, Yaw 315° - 16: Roll 90° - 17: Roll 90°, Yaw 45° - 18: Roll 90°, Yaw 90° - 19: Roll 90°, Yaw 135° - 20: Roll 270° - 21: Roll 270°, Yaw 45° - 22: Roll 270°, Yaw 90° - 23: Roll 270°, Yaw 135° - 24: Pitch 90° - 25: Pitch 270° - 26: Pitch 180°, Yaw 90° - 27: Pitch 180°, Yaw 270° - 28: Roll 90°, Pitch 90° - 29: Roll 180°, Pitch 90° - 30: Roll 270°, Pitch 90° - 31: Roll 90°, Pitch 180° - 32: Roll 270°, Pitch 180° - 33: Roll 90°, Pitch 270° - 34: Roll 180°, Pitch 270° - 35: Roll 270°, Pitch 270° - 36: Roll 90°, Pitch 180°, Yaw 90° - 37: Roll 90°, Yaw 270° - 38: Roll 90°, Pitch 68°, Yaw 293° - 39: Pitch 315° - 40: Roll 90°, Pitch 315° - min: -1 - max: 40 - default: -1 num_instances: *max_num_sensor_instances instance_start: 0 @@ -167,6 +92,7 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 + # Barometer calibration CAL_BARO${i}_ID: description: @@ -178,23 +104,6 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 - CAL_BARO${i}_PRIO: - description: - short: Barometer ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - CAL_BARO${i}_OFF: description: short: Barometer ${i} offset @@ -207,6 +116,7 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 + # Gyroscope calibration CAL_GYRO${i}_ID: description: @@ -218,79 +128,6 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 - CAL_GYRO${i}_PRIO: - description: - short: Gyroscope ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_GYRO${i}_ROT: - description: - short: Gyroscope ${i} rotation relative to airframe - long: | - An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. - category: System - type: enum - values: - -1: Internal - 0: No rotation - 1: Yaw 45° - 2: Yaw 90° - 3: Yaw 135° - 4: Yaw 180° - 5: Yaw 225° - 6: Yaw 270° - 7: Yaw 315° - 8: Roll 180° - 9: Roll 180°, Yaw 45° - 10: Roll 180°, Yaw 90° - 11: Roll 180°, Yaw 135° - 12: Pitch 180° - 13: Roll 180°, Yaw 225° - 14: Roll 180°, Yaw 270° - 15: Roll 180°, Yaw 315° - 16: Roll 90° - 17: Roll 90°, Yaw 45° - 18: Roll 90°, Yaw 90° - 19: Roll 90°, Yaw 135° - 20: Roll 270° - 21: Roll 270°, Yaw 45° - 22: Roll 270°, Yaw 90° - 23: Roll 270°, Yaw 135° - 24: Pitch 90° - 25: Pitch 270° - 26: Pitch 180°, Yaw 90° - 27: Pitch 180°, Yaw 270° - 28: Roll 90°, Pitch 90° - 29: Roll 180°, Pitch 90° - 30: Roll 270°, Pitch 90° - 31: Roll 90°, Pitch 180° - 32: Roll 270°, Pitch 180° - 33: Roll 90°, Pitch 270° - 34: Roll 180°, Pitch 270° - 35: Roll 270°, Pitch 270° - 36: Roll 90°, Pitch 180°, Yaw 90° - 37: Roll 90°, Yaw 270° - 38: Roll 90°, Pitch 68°, Yaw 293° - 39: Pitch 315° - 40: Roll 90°, Pitch 315° - min: -1 - max: 40 - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - CAL_GYRO${i}_XOFF: description: short: Gyroscope ${i} X-axis offset @@ -327,6 +164,7 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 + # Magnetometer calibration CAL_MAG${i}_ID: description: @@ -338,120 +176,6 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 - CAL_MAG${i}_PRIO: - description: - short: Magnetometer ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ROT: - description: - short: Magnetometer ${i} rotation relative to airframe - long: | - An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. - Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW. - category: System - type: enum - values: - -1: Internal - 0: No rotation - 1: Yaw 45° - 2: Yaw 90° - 3: Yaw 135° - 4: Yaw 180° - 5: Yaw 225° - 6: Yaw 270° - 7: Yaw 315° - 8: Roll 180° - 9: Roll 180°, Yaw 45° - 10: Roll 180°, Yaw 90° - 11: Roll 180°, Yaw 135° - 12: Pitch 180° - 13: Roll 180°, Yaw 225° - 14: Roll 180°, Yaw 270° - 15: Roll 180°, Yaw 315° - 16: Roll 90° - 17: Roll 90°, Yaw 45° - 18: Roll 90°, Yaw 90° - 19: Roll 90°, Yaw 135° - 20: Roll 270° - 21: Roll 270°, Yaw 45° - 22: Roll 270°, Yaw 90° - 23: Roll 270°, Yaw 135° - 24: Pitch 90° - 25: Pitch 270° - 26: Pitch 180°, Yaw 90° - 27: Pitch 180°, Yaw 270° - 28: Roll 90°, Pitch 90° - 29: Roll 180°, Pitch 90° - 30: Roll 270°, Pitch 90° - 31: Roll 90°, Pitch 180° - 32: Roll 270°, Pitch 180° - 33: Roll 90°, Pitch 270° - 34: Roll 180°, Pitch 270° - 35: Roll 270°, Pitch 270° - 36: Roll 90°, Pitch 180°, Yaw 90° - 37: Roll 90°, Yaw 270° - 38: Roll 90°, Pitch 68°, Yaw 293° - 39: Pitch 315° - 40: Roll 90°, Pitch 315° - 100: Custom Euler Angle - min: -1 - max: 100 - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ROLL: - description: - short: Magnetometer ${i} Custom Euler Roll Angle - long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" - category: System - type: float - default: 0.0 - min: -180 - max: 180 - unit: deg - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_PITCH: - description: - short: Magnetometer ${i} Custom Euler Pitch Angle - long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" - category: System - type: float - default: 0.0 - min: -180 - max: 180 - unit: deg - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_YAW: - description: - short: Magnetometer ${i} Custom Euler Yaw Angle - long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" - category: System - type: float - default: 0.0 - min: -180 - max: 180 - unit: deg - num_instances: *max_num_sensor_instances - instance_start: 0 - CAL_MAG${i}_XOFF: description: short: Magnetometer ${i} X-axis offset @@ -609,3 +333,399 @@ parameters: volatile: true num_instances: *max_num_sensor_instances instance_start: 0 + + + - group: Sensor Configuration + definitions: + + # Accelerometer configuration + SENS_ACC${i}_ID: + description: + short: Accelerometer ${i} configuration device ID + long: Device ID of the accelerometer this configuration applies to. + category: System + type: int32 + default: 0 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_ACC${i}_PRIO: + description: + short: Accelerometer ${i} priority + category: System + type: enum + values: + -1: Uninitialized + 0: Disabled + 1: Min + 25: Low + 50: Medium (Default) + 75: High + 100: Max + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_ACC${i}_ROT: + description: + short: Accelerometer ${i} rotation relative to airframe + long: | + An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + category: System + type: enum + values: + -1: Internal + 0: No rotation + 1: Yaw 45° + 2: Yaw 90° + 3: Yaw 135° + 4: Yaw 180° + 5: Yaw 225° + 6: Yaw 270° + 7: Yaw 315° + 8: Roll 180° + 9: Roll 180°, Yaw 45° + 10: Roll 180°, Yaw 90° + 11: Roll 180°, Yaw 135° + 12: Pitch 180° + 13: Roll 180°, Yaw 225° + 14: Roll 180°, Yaw 270° + 15: Roll 180°, Yaw 315° + 16: Roll 90° + 17: Roll 90°, Yaw 45° + 18: Roll 90°, Yaw 90° + 19: Roll 90°, Yaw 135° + 20: Roll 270° + 21: Roll 270°, Yaw 45° + 22: Roll 270°, Yaw 90° + 23: Roll 270°, Yaw 135° + 24: Pitch 90° + 25: Pitch 270° + 26: Pitch 180°, Yaw 90° + 27: Pitch 180°, Yaw 270° + 28: Roll 90°, Pitch 90° + 29: Roll 180°, Pitch 90° + 30: Roll 270°, Pitch 90° + 31: Roll 90°, Pitch 180° + 32: Roll 270°, Pitch 180° + 33: Roll 90°, Pitch 270° + 34: Roll 180°, Pitch 270° + 35: Roll 270°, Pitch 270° + 36: Roll 90°, Pitch 180°, Yaw 90° + 37: Roll 90°, Yaw 270° + 38: Roll 90°, Pitch 68°, Yaw 293° + 39: Pitch 315° + 40: Roll 90°, Pitch 315° + min: -1 + max: 40 + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + + # Barometer configuration + SENS_BARO${i}_ID: + description: + short: Barometer ${i} configuration device ID + long: Device ID of the barometer this configuration applies to. + category: System + type: int32 + default: 0 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_BARO${i}_PRIO: + description: + short: Barometer ${i} priority + category: System + type: enum + values: + -1: Uninitialized + 0: Disabled + 1: Min + 25: Low + 50: Medium (Default) + 75: High + 100: Max + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + + # GNSS configuration + SENS_GNSS${i}_ID: + description: + short: GNSS ${i} configuration device ID + long: Device ID of the GNSS this configuration applies to. + category: System + type: int32 + default: 0 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_GNSS${i}_PRIO: + description: + short: GNSS ${i} priority + category: System + type: enum + values: + -1: Uninitialized + 0: Disabled + 1: Min + 25: Low + 50: Medium (Default) + 75: High + 100: Max + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_GNSS${i}_XPOS: + description: + short: GNSS ${i} antenna X position + long: | + X position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) + category: System + type: float + default: 0.0 + unit: m + volatile: true + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_GNSS${i}_YPOS: + description: + short: GNSS ${i} antenna Y position + long: | + Y position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) + category: System + type: float + default: 0.0 + unit: m + volatile: true + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_GNSS${i}_ZPOS: + description: + short: GNSS ${i} antenna Z position + long: | + Z position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) + category: System + type: float + default: 0.0 + unit: m + volatile: true + num_instances: *max_num_sensor_instances + instance_start: 0 + + + # Gyroscope configuration + SENS_GYRO${i}_ID: + description: + short: Gyroscope ${i} configuration device ID + long: Device ID of the gyroscope this configuration applies to. + category: System + type: int32 + default: 0 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_GYRO${i}_PRIO: + description: + short: Gyroscope ${i} priority + category: System + type: enum + values: + -1: Uninitialized + 0: Disabled + 1: Min + 25: Low + 50: Medium (Default) + 75: High + 100: Max + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_GYRO${i}_ROT: + description: + short: Gyroscope ${i} rotation relative to airframe + long: | + An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + category: System + type: enum + values: + -1: Internal + 0: No rotation + 1: Yaw 45° + 2: Yaw 90° + 3: Yaw 135° + 4: Yaw 180° + 5: Yaw 225° + 6: Yaw 270° + 7: Yaw 315° + 8: Roll 180° + 9: Roll 180°, Yaw 45° + 10: Roll 180°, Yaw 90° + 11: Roll 180°, Yaw 135° + 12: Pitch 180° + 13: Roll 180°, Yaw 225° + 14: Roll 180°, Yaw 270° + 15: Roll 180°, Yaw 315° + 16: Roll 90° + 17: Roll 90°, Yaw 45° + 18: Roll 90°, Yaw 90° + 19: Roll 90°, Yaw 135° + 20: Roll 270° + 21: Roll 270°, Yaw 45° + 22: Roll 270°, Yaw 90° + 23: Roll 270°, Yaw 135° + 24: Pitch 90° + 25: Pitch 270° + 26: Pitch 180°, Yaw 90° + 27: Pitch 180°, Yaw 270° + 28: Roll 90°, Pitch 90° + 29: Roll 180°, Pitch 90° + 30: Roll 270°, Pitch 90° + 31: Roll 90°, Pitch 180° + 32: Roll 270°, Pitch 180° + 33: Roll 90°, Pitch 270° + 34: Roll 180°, Pitch 270° + 35: Roll 270°, Pitch 270° + 36: Roll 90°, Pitch 180°, Yaw 90° + 37: Roll 90°, Yaw 270° + 38: Roll 90°, Pitch 68°, Yaw 293° + 39: Pitch 315° + 40: Roll 90°, Pitch 315° + min: -1 + max: 40 + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + + # Magnetometer configuration + SENS_MAG${i}_ID: + description: + short: Magnetometer ${i} configuration device ID + long: Device ID of the magnetometer this configuration applies to. + category: System + type: int32 + default: 0 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_MAG${i}_PRIO: + description: + short: Magnetometer ${i} priority + category: System + type: enum + values: + -1: Uninitialized + 0: Disabled + 1: Min + 25: Low + 50: Medium (Default) + 75: High + 100: Max + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_MAG${i}_ROT: + description: + short: Magnetometer ${i} rotation relative to airframe + long: | + An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW. + category: System + type: enum + values: + -1: Internal + 0: No rotation + 1: Yaw 45° + 2: Yaw 90° + 3: Yaw 135° + 4: Yaw 180° + 5: Yaw 225° + 6: Yaw 270° + 7: Yaw 315° + 8: Roll 180° + 9: Roll 180°, Yaw 45° + 10: Roll 180°, Yaw 90° + 11: Roll 180°, Yaw 135° + 12: Pitch 180° + 13: Roll 180°, Yaw 225° + 14: Roll 180°, Yaw 270° + 15: Roll 180°, Yaw 315° + 16: Roll 90° + 17: Roll 90°, Yaw 45° + 18: Roll 90°, Yaw 90° + 19: Roll 90°, Yaw 135° + 20: Roll 270° + 21: Roll 270°, Yaw 45° + 22: Roll 270°, Yaw 90° + 23: Roll 270°, Yaw 135° + 24: Pitch 90° + 25: Pitch 270° + 26: Pitch 180°, Yaw 90° + 27: Pitch 180°, Yaw 270° + 28: Roll 90°, Pitch 90° + 29: Roll 180°, Pitch 90° + 30: Roll 270°, Pitch 90° + 31: Roll 90°, Pitch 180° + 32: Roll 270°, Pitch 180° + 33: Roll 90°, Pitch 270° + 34: Roll 180°, Pitch 270° + 35: Roll 270°, Pitch 270° + 36: Roll 90°, Pitch 180°, Yaw 90° + 37: Roll 90°, Yaw 270° + 38: Roll 90°, Pitch 68°, Yaw 293° + 39: Pitch 315° + 40: Roll 90°, Pitch 315° + 100: Custom Euler Angle + min: -1 + max: 100 + default: -1 + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_MAG${i}_ROLL: + description: + short: Magnetometer ${i} Custom Euler Roll Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_MAG${i}_PITCH: + description: + short: Magnetometer ${i} Custom Euler Pitch Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + + SENS_MAG${i}_YAW: + description: + short: Magnetometer ${i} Custom Euler Yaw Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..8b43625370 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -43,6 +43,8 @@ #include "sensors.hpp" +#include + Sensors::Sensors(bool hil_enabled) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), @@ -184,58 +186,96 @@ int Sensors::parameters_update() for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { // sensor_accel { - const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); + const uint32_t device_id_accel = sensor::calibration::GetCalibrationParamInt32("ACC", "ID", i); if (device_id_accel != 0) { - calibration::Accelerometer accel_cal(device_id_accel); + sensor::calibration::Accelerometer accel_cal(device_id_accel); } uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { - calibration::Accelerometer cal; + sensor::calibration::Accelerometer cal; cal.set_calibration_index(i); cal.ParametersLoad(); } + + uint32_t device_id_acc_cfg = sensor::configuration::GetConfigurationParamInt32("ACC", "ID", i); + + if (device_id_acc_cfg != 0) { + sensor::configuration::Accelerometer mag_cal(device_id_acc_cfg); + } } // sensor_gyro { - const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); + const uint32_t device_id_gyro = sensor::calibration::GetCalibrationParamInt32("GYRO", "ID", i); if (device_id_gyro != 0) { - calibration::Gyroscope gyro_cal(device_id_gyro); + sensor::calibration::Gyroscope gyro_cal(device_id_gyro); } uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { - calibration::Gyroscope cal; + sensor::calibration::Gyroscope cal; cal.set_calibration_index(i); cal.ParametersLoad(); } + + uint32_t device_id_gyro_cfg = sensor::configuration::GetConfigurationParamInt32("GYRO", "ID", i); + + if (device_id_gyro_cfg != 0) { + sensor::configuration::Accelerometer mag_cal(device_id_gyro_cfg); + } } #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) // sensor_mag { - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); + uint32_t device_id_mag_cal = sensor::calibration::GetCalibrationParamInt32("MAG", "ID", i); - if (device_id_mag != 0) { - calibration::Magnetometer mag_cal(device_id_mag); + if (device_id_mag_cal != 0) { + sensor::calibration::Magnetometer mag_cal(device_id_mag_cal); } uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { - calibration::Magnetometer cal; + sensor::calibration::Magnetometer cal; cal.set_calibration_index(i); cal.ParametersLoad(); } + + uint32_t device_id_mag_cfg = sensor::configuration::GetConfigurationParamInt32("MAG", "ID", i); + + if (device_id_mag_cfg != 0) { + sensor::configuration::Magnetometer mag_cal(device_id_mag_cfg); + } } #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + // sensor_gps + { + uint32_t device_id_gnss = sensor::configuration::GetConfigurationParamInt32("GNSS", "ID", i); + + if (device_id_gnss != 0) { + sensor::configuration::GNSS gnss_cal(device_id_gnss); + } + + uORB::SubscriptionData sensor_gps_sub{ORB_ID(sensor_gps), i}; + + if (sensor_gps_sub.advertised() && (sensor_gps_sub.get().device_id != 0)) { + sensor::configuration::GNSS conf; + conf.set_configuration_index(i); + conf.ParametersLoad(); + } + } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION } #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) @@ -713,6 +753,15 @@ int Sensors::print_status() _airspeed_validator.print(); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + + if (_vehicle_gps_position) { + PX4_INFO_RAW("\n"); + _vehicle_gps_position->PrintStatus(); + } + +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) if (_vehicle_optical_flow) { @@ -732,15 +781,6 @@ int Sensors::print_status() _vehicle_angular_velocity.PrintStatus(); #endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - - if (_vehicle_gps_position) { - PX4_INFO_RAW("\n"); - _vehicle_gps_position->PrintStatus(); - } - -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - PX4_INFO_RAW("\n"); for (auto &i : _vehicle_imu_list) { diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 102be087b1..8e8ccdca17 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -41,7 +41,7 @@ * @author Beat Küng */ -#include +#include #include #include #include @@ -80,7 +80,7 @@ #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) # include "vehicle_magnetometer/VehicleMagnetometer.hpp" -# include +# include # include #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER @@ -263,4 +263,4 @@ private: #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER (ParamBool) _param_sens_imu_mode ) -}; \ No newline at end of file +}; diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 68c0652082..c812a4619f 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -33,7 +33,7 @@ #pragma once -#include +#include #include #include #include @@ -87,7 +87,7 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)}; - calibration::Accelerometer _calibration{}; + sensor::calibration::Accelerometer _calibration{}; matrix::Vector3f _bias{}; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index fd28de34f8..a91291f848 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -35,7 +35,7 @@ #include "data_validator/DataValidatorGroup.hpp" -#include +#include #include #include #include @@ -96,7 +96,7 @@ private: {this, ORB_ID(sensor_baro), 3}, }; - calibration::Barometer _calibration[MAX_SENSOR_COUNT]; + sensor::calibration::Barometer _calibration[MAX_SENSOR_COUNT]; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8aae8a3c03..5bb5094411 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -34,7 +34,7 @@ #pragma once #include -#include +#include #include #include #include @@ -113,7 +113,7 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; - calibration::Gyroscope _calibration{}; + sensor::calibration::Gyroscope _calibration{}; matrix::Vector3f _bias{}; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 964b5fdecc..4a86205ba2 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -91,11 +91,29 @@ void VehicleGPSPosition::ParametersUpdate(bool force) } } + for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) { + _configuration[instance].ParametersUpdate(); + } + _gps_blending.setBlendingUseSpeedAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC); _gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC); _gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC); _gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get()); - _gps_blending.setPrimaryInstance(_param_sens_gps_prime.get()); + + // TODO: select highest priority + int primary_instance = -1; + uint8_t highest_priority = 0; + + for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) { + if (_configuration[instance].enabled() && _configuration[instance].priority() > highest_priority) { + primary_instance = instance; + highest_priority = _configuration[instance].priority(); + } + } + + if (primary_instance >= 0) { + //_gps_blending.setPrimaryInstance(primary_instance); + } } } @@ -116,11 +134,14 @@ void VehicleGPSPosition::Run() if (gps_updated) { any_gps_updated = true; - _sensor_gps_sub[i].copy(&gps_data); - _gps_blending.setGpsData(gps_data, i); + if (_sensor_gps_sub[i].copy(&gps_data)) { + _configuration[i].set_device_id(gps_data.device_id); - if (!_sensor_gps_sub[i].registered()) { - _sensor_gps_sub[i].registerCallback(); + _gps_blending.setGpsData(gps_data, i); + + if (!_sensor_gps_sub[i].registered()) { + _sensor_gps_sub[i].registerCallback(); + } } } } @@ -131,12 +152,29 @@ void VehicleGPSPosition::Run() if (_gps_blending.isNewOutputDataAvailable()) { sensor_gps_s gps_output{_gps_blending.getOutputGpsData()}; + const int selected_instance = _gps_blending.getSelectedGps(); + // clear device_id if blending - if (_gps_blending.getSelectedGps() == GpsBlending::GPS_MAX_RECEIVERS_BLEND) { + if (selected_instance == GpsBlending::GPS_MAX_RECEIVERS_BLEND) { gps_output.device_id = 0; + + gps_output.position_offset[0] = 0.f; + gps_output.position_offset[1] = 0.f; + gps_output.position_offset[2] = 0.f; + + } else if (selected_instance >= 0 && selected_instance < GpsBlending::GPS_MAX_RECEIVERS_BLEND) { + + _configuration[selected_instance].position().copyTo(gps_output.position_offset); } _vehicle_gps_position_pub.publish(gps_output); + + // populate initial SENS_GNSSx configuration slots if necessary + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if ((_configuration[i].device_id() != 0) && (!_configuration[i].configured())) { + _configuration[i].ParametersSave(i); + } + } } } @@ -148,6 +186,12 @@ void VehicleGPSPosition::Run() void VehicleGPSPosition::PrintStatus() { PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_blending.getSelectedGps()); + + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_configuration[i].device_id() != 0) { + _configuration[i].PrintStatus(); + } + } } }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 971f24ae4a..8ff1721c1f 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -92,10 +93,11 @@ private: GpsBlending _gps_blending; + sensor::configuration::GNSS _configuration[GPS_MAX_RECEIVERS] {}; + DEFINE_PARAMETERS( (ParamInt) _param_sens_gps_mask, - (ParamFloat) _param_sens_gps_tau, - (ParamInt) _param_sens_gps_prime + (ParamFloat) _param_sens_gps_tau ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 3673ca14bb..39b4816506 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -54,6 +54,7 @@ void GpsBlending::update(uint64_t hrt_now_us) for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { if (_gps_state[i].fix_type > best_fix) { best_fix = _gps_state[i].fix_type; + gps_select_index = i; } } @@ -73,15 +74,8 @@ void GpsBlending::update(uint64_t hrt_now_us) _hgt_offset_m[i] = 0.0; } - // Only use a secondary instance if the fallback is allowed - if ((_primary_instance > -1) - && (gps_select_index != _primary_instance) - && !_fallback_allowed) { - gps_select_index = _primary_instance; - } - _selected_gps = gps_select_index; - _is_new_output_data_available = _gps_updated[gps_select_index]; + _is_new_output_data_available = _gps_updated[gps_select_index]; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { _gps_updated[gps_select_index] = false; @@ -109,7 +103,7 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) float raw_dt = 0.f; if (_gps_state[i].timestamp > _time_prev_us[i]) { - raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]); + raw_dt = 1e-6f * ((int64_t)_gps_state[i].timestamp - (int64_t)_time_prev_us[i]); } float present_dt = 0.f; @@ -372,6 +366,14 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS gps_blended_state.vel_e_m_s = 0; gps_blended_state.vel_d_m_s = 0; + // these will be a sum of weighted values and must start from zero. + gps_blended_state.timestamp = 0; + gps_blended_state.timestamp_sample = 0; + gps_blended_state.vel_m_s = 0; + gps_blended_state.vel_n_m_s = 0; + gps_blended_state.vel_e_m_s = 0; + gps_blended_state.vel_d_m_s = 0; + // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers @@ -429,11 +431,6 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS gps_blended_state.vel_ned_valid = true; } } - - // TODO read parameters for individual GPS antenna positions and blend - // Vector3f temp_antenna_offset = _antenna_offset[i]; - // temp_antenna_offset *= blend_weights[i]; - // _blended_antenna_offset += temp_antenna_offset; } /* @@ -500,14 +497,15 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) { // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering + // A weighting of 0 will make the offset adjust the slowest, a weighting of 1 will make it adjust with zero filtering float alpha[GPS_MAX_RECEIVERS_BLEND] {}; float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f); for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { if (_gps_state[i].timestamp > _time_prev_us[i]) { // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter - alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), - 0.0f, 1.0f); + alpha[i] = constrain(omega_lpf * 1e-6f * (float)((int64_t)_gps_state[i].timestamp - (int64_t)_time_prev_us[i]), 0.f, + 1.f); _time_prev_us[i] = _gps_state[i].timestamp; } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index 6d33a643cd..8056c00d5f 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -72,6 +72,7 @@ public: _gps_updated[instance] = true; } } + void setBlendingUseSpeedAccuracy(bool enabled) { _blend_use_spd_acc = enabled; } void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; } void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; } @@ -82,6 +83,7 @@ public: bool isNewOutputDataAvailable() const { return _is_new_output_data_available; } int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; } + const sensor_gps_s &getOutputGpsData() const { if (_selected_gps < GPS_MAX_RECEIVERS_BLEND) { @@ -91,6 +93,7 @@ public: return _gps_blended_state; } } + int getSelectedGps() const { return _selected_gps; } private: @@ -122,11 +125,12 @@ private: sensor_gps_s _gps_state[GPS_MAX_RECEIVERS_BLEND] {}; ///< internal state data for the physical GPS sensor_gps_s _gps_blended_state {}; + bool _gps_updated[GPS_MAX_RECEIVERS_BLEND] {}; int _selected_gps{0}; int _np_gps_suitable_for_blending{0}; - int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id - bool _fallback_allowed{false}; + int _primary_instance{0}; ///< if -1, there is no primary instance and the best receiver is used // TODO: use device_id + bool _fallback_allowed{true}; bool _is_new_output_data_available{false}; diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index af5b1feab7..7b8c4774fa 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -204,9 +204,9 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) gps_blending.setGpsData(gps_data1, 1); gps_blending.update(_time_now_us); - // THEN: the blended instance should be selected (2) + // THEN: the blended instance should be selected (3) // and the eph should be adjusted - EXPECT_EQ(gps_blending.getSelectedGps(), 2); + EXPECT_EQ(gps_blending.getSelectedGps(), 3); EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); EXPECT_LT(gps_blending.getOutputGpsData().eph, gps_data0.eph); diff --git a/src/modules/sensors/vehicle_gps_position/params.c b/src/modules/sensors/vehicle_gps_position/params.c index 651e6e15fa..9ea7513277 100644 --- a/src/modules/sensors/vehicle_gps_position/params.c +++ b/src/modules/sensors/vehicle_gps_position/params.c @@ -46,7 +46,7 @@ * @bit 1 use hpos accuracy * @bit 2 use vpos accuracy */ -PARAM_DEFINE_INT32(SENS_GPS_MASK, 7); +PARAM_DEFINE_INT32(SENS_GPS_MASK, 0); /** * Multi GPS Blending Time Constant @@ -61,22 +61,3 @@ PARAM_DEFINE_INT32(SENS_GPS_MASK, 7); * @decimal 1 */ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); - -/** - * Multi GPS primary instance - * - * When no blending is active, this defines the preferred GPS receiver instance. - * The GPS selection logic waits until the primary receiver is available to - * send data to the EKF even if a secondary instance is already available. - * The secondary instance is then only used if the primary one times out. - * - * To have an equal priority of all the instances, set this parameter to -1 and - * the best receiver will be used. - * - * This parameter has no effect if blending is active. - * - * @group Sensors - * @min -1 - * @max 1 - */ -PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0); diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 125d56758c..f7f996d6a8 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 689d72f614..3e513c3848 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -40,8 +40,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -111,8 +111,8 @@ private: uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - calibration::Accelerometer _accel_calibration{}; - calibration::Gyroscope _gyro_calibration{}; + sensor::calibration::Accelerometer _accel_calibration{}; + sensor::calibration::Gyroscope _gyro_calibration{}; sensors::Integrator _accel_integrator{}; sensors::IntegratorConing _gyro_integrator{}; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 6bb7c5ef5b..25132cb70d 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include namespace sensors { @@ -63,7 +63,7 @@ VehicleMagnetometer::VehicleMagnetometer() : // if publishing multiple mags advertise instances immediately for existing calibrations if (!_param_sens_mag_mode.get()) { for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); + uint32_t device_id_mag = sensor::calibration::GetCalibrationParamInt32("MAG", "ID", i); if (device_id_mag != 0) { _vehicle_magnetometer_pub[i].advertise(); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index c90398fe1f..1c912dbfc1 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -35,7 +35,7 @@ #include "data_validator/DataValidatorGroup.hpp" -#include +#include #include #include #include @@ -137,7 +137,7 @@ private: matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {}; - calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; + sensor::calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; // Magnetometer interference compensation enum class MagCompensationType { diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index fb424ea5af..ecde065230 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include #include @@ -102,7 +102,7 @@ private: hrt_abstime _gyro_timestamp_sample_last{0}; - calibration::Gyroscope _gyro_calibration{}; + sensor::calibration::Gyroscope _gyro_calibration{}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 3e89e18644..045d6cb847 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -39,7 +39,7 @@ #include "voted_sensors_update.h" -#include +#include #include #include #include @@ -88,13 +88,14 @@ void VotedSensorsUpdate::parametersUpdate() && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) { // find corresponding configured accel priority - int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id); + int8_t accel_conf_index = sensor::configuration::FindCurrentConfigurationIndex("ACC", imu.get().accel_device_id); - if (accel_cal_index >= 0) { + if (accel_conf_index >= 0) { // found matching CAL_ACCx_PRIO int32_t accel_priority_old = _accel.priority_configured[uorb_index]; - _accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index); + _accel.priority_configured[uorb_index] = sensor::configuration::GetConfigurationParamInt32("ACC", "PRIO", + accel_conf_index); if (accel_priority_old != _accel.priority_configured[uorb_index]) { if (_accel.priority_configured[uorb_index] == 0) { @@ -111,13 +112,14 @@ void VotedSensorsUpdate::parametersUpdate() } // find corresponding configured gyro priority - int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id); + int8_t gyro_conf_index = sensor::configuration::FindCurrentConfigurationIndex("GYRO", imu.get().gyro_device_id); - if (gyro_cal_index >= 0) { + if (gyro_conf_index >= 0) { // found matching CAL_GYROx_PRIO int32_t gyro_priority_old = _gyro.priority_configured[uorb_index]; - _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index); + _gyro.priority_configured[uorb_index] = sensor::configuration::GetConfigurationParamInt32("GYRO", "PRIO", + gyro_conf_index); if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { if (_gyro.priority_configured[uorb_index] == 0) {