diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index ea53dc3d65..615214a190 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 EKF2_TAU_VEL 0.5 param set-default EKF2_GPS_P_GATE 10 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index feb06fc88a..c1dca950c1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -27,8 +27,8 @@ param set-default COM_DISARM_LAND 0.5 # EKF2 parameters param set-default EKF2_AID_MASK 35 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_POS_X 0.055 param set-default EKF2_OF_POS_Y 0.02 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 2dacd54acb..9d8fed5075 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -55,9 +55,9 @@ param set-default EKF2_BCOEF_X 31.5 param set-default EKF2_BCOEF_Y 25.5 param set-default EKF2_GPS_DELAY 100 -param set-default EKF2_GPS_POS_X 0.06 -param set-default EKF2_GPS_POS_Y 0.0 -param set-default EKF2_GPS_POS_Z 0.0 +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/sensor_gps.msg b/msg/sensor_gps.msg index 2b62787fe3..dd3d462526 100644 --- a/msg/sensor_gps.msg +++ b/msg/sensor_gps.msg @@ -49,4 +49,6 @@ float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) float32 rtcm_injection_rate # RTCM message injection rate Hz uint8 selected_rtcm_instance # uorb instance that is being used for RTCM corrections +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 bb27f0eccb..da6131e400 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -60,7 +60,7 @@ add_subdirectory(pid) add_subdirectory(pid_design) add_subdirectory(pwm) add_subdirectory(rc) -add_subdirectory(sensor_calibration) +add_subdirectory(sensor) add_subdirectory(slew_rate) add_subdirectory(systemlib) add_subdirectory(system_identification) diff --git a/src/lib/sensor/CMakeLists.txt b/src/lib/sensor/CMakeLists.txt new file mode 100644 index 0000000000..b48f259f7c --- /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(calibration) +add_subdirectory(configuration) 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 98% rename from src/lib/sensor_calibration/Accelerometer.cpp rename to src/lib/sensor/calibration/Accelerometer.cpp index b738dc862e..b67b664e99 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor/calibration/Accelerometer.cpp @@ -33,13 +33,15 @@ #include "Accelerometer.hpp" -#include "Utilities.hpp" - #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -305,3 +307,4 @@ void Accelerometer::PrintStatus() } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor/calibration/Accelerometer.hpp similarity index 98% rename from src/lib/sensor_calibration/Accelerometer.hpp rename to src/lib/sensor/calibration/Accelerometer.hpp index e22b58ab09..28900fe77e 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor/calibration/Accelerometer.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Utilities.hpp" + #include #include #include @@ -40,6 +42,8 @@ #include #include +namespace sensor +{ namespace calibration { class Accelerometer @@ -116,4 +120,6 @@ private: 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 98% rename from src/lib/sensor_calibration/Barometer.cpp rename to src/lib/sensor/calibration/Barometer.cpp index 430dfc1af3..7c921df05e 100644 --- a/src/lib/sensor_calibration/Barometer.cpp +++ b/src/lib/sensor/calibration/Barometer.cpp @@ -33,13 +33,15 @@ #include "Barometer.hpp" -#include "Utilities.hpp" - #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -244,3 +246,4 @@ void Barometer::PrintStatus() } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Barometer.hpp b/src/lib/sensor/calibration/Barometer.hpp similarity index 98% rename from src/lib/sensor_calibration/Barometer.hpp rename to src/lib/sensor/calibration/Barometer.hpp index dedd45d54e..6e589e7489 100644 --- a/src/lib/sensor_calibration/Barometer.hpp +++ b/src/lib/sensor/calibration/Barometer.hpp @@ -33,13 +33,18 @@ #pragma once +#include "Utilities.hpp" + #include #include #include #include +namespace sensor +{ namespace calibration { + class Barometer { public: @@ -110,4 +115,6 @@ private: 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 92% rename from src/lib/sensor_calibration/CMakeLists.txt rename to src/lib/sensor/calibration/CMakeLists.txt index c85bde638e..efa8b17ff4 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,8 @@ px4_add_library(sensor_calibration Utilities.hpp ) -target_link_libraries(sensor_calibration PRIVATE conversion) +target_link_libraries(sensor_calibration + PRIVATE + conversion + sensor_utilities +) diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor/calibration/Gyroscope.cpp similarity index 98% rename from src/lib/sensor_calibration/Gyroscope.cpp rename to src/lib/sensor/calibration/Gyroscope.cpp index c953e98592..c160ce7a38 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor/calibration/Gyroscope.cpp @@ -33,13 +33,15 @@ #include "Gyroscope.hpp" -#include "Utilities.hpp" - #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -282,3 +284,4 @@ void Gyroscope::PrintStatus() } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor/calibration/Gyroscope.hpp similarity index 98% rename from src/lib/sensor_calibration/Gyroscope.hpp rename to src/lib/sensor/calibration/Gyroscope.hpp index 70aa88cd47..49510bbd98 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor/calibration/Gyroscope.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Utilities.hpp" + #include #include #include @@ -40,8 +42,11 @@ #include #include +namespace sensor +{ namespace calibration { + class Gyroscope { public: @@ -119,4 +124,6 @@ private: 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 similarity index 98% rename from src/lib/sensor_calibration/Magnetometer.cpp rename to src/lib/sensor/calibration/Magnetometer.cpp index 4ea0e6982e..1df5ac1dd6 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor/calibration/Magnetometer.cpp @@ -32,14 +32,17 @@ ****************************************************************************/ #include "Magnetometer.hpp" - #include "Utilities.hpp" #include +#include using namespace matrix; using namespace time_literals; +using namespace sensor::utilities; +namespace sensor +{ namespace calibration { @@ -303,3 +306,4 @@ void Magnetometer::PrintStatus() } } // namespace calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor/calibration/Magnetometer.hpp similarity index 98% rename from src/lib/sensor_calibration/Magnetometer.hpp rename to src/lib/sensor/calibration/Magnetometer.hpp index 24b9fa0ee0..f0f1cd3ccc 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor/calibration/Magnetometer.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Utilities.hpp" + #include #include #include @@ -41,8 +43,11 @@ #include #include +namespace sensor +{ namespace calibration { + class Magnetometer { public: @@ -117,4 +122,6 @@ private: 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 66% rename from src/lib/sensor_calibration/Utilities.cpp rename to src/lib/sensor/calibration/Utilities.cpp index 8cdc760e1e..0a1a401fb3 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-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 @@ -31,26 +31,21 @@ * ****************************************************************************/ +#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,8 +58,8 @@ int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id) } for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { - char str[20] {}; - sprintf(str, "CAL_%s%u_ID", sensor_type, i); + char str[16 + 1] {}; + snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i); int32_t device_id_val = 0; @@ -102,8 +97,8 @@ 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] {}; - sprintf(str, "CAL_%s%u_ID", sensor_type, i); + char str[16 + 1] {}; + snprintf(str, sizeof(str), "CAL_%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) { @@ -136,9 +131,9 @@ 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] {}; - sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); + // eg CAL_MAGn_ID + char str[16 + 1] {}; + snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); int32_t value = 0; @@ -152,8 +147,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] {}; - sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); + char str[16 + 1] {}; + snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); float value = NAN; @@ -168,13 +163,13 @@ Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_t { Vector3f values{0.f, 0.f, 0.f}; - char str[20] {}; + char str[16 + 1] {}; for (int axis = 0; axis < 3; axis++) { char axis_char = 'X' + axis; // eg CAL_MAGn_{X,Y,Z}OFF - sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type); + 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); @@ -187,13 +182,13 @@ Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_t bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values) { int ret = PX4_OK; - char str[20] {}; + char str[16 + 1] {}; for (int axis = 0; axis < 3; axis++) { char axis_char = 'X' + axis; // eg CAL_MAGn_{X,Y,Z}OFF - sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type); + 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)); @@ -204,84 +199,5 @@ bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, 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_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 calibration +} // namespace sensor diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor/calibration/Utilities.hpp similarity index 86% rename from src/lib/sensor_calibration/Utilities.hpp rename to src/lib/sensor/calibration/Utilities.hpp index 4696f01f36..be659c4573 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-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 @@ -40,6 +40,8 @@ #include #include +namespace sensor +{ namespace calibration { @@ -85,10 +87,10 @@ 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 - sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type); + // eg CAL_MAGn_ID + snprintf(str, sizeof(str), "CAL_%s%u_%s", sensor_type, instance, cal_type); int ret = param_set_no_notification(param_find(str), &value); @@ -121,32 +123,5 @@ matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const cha 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/CMakeLists.txt b/src/lib/sensor/configuration/CMakeLists.txt new file mode 100644 index 0000000000..b4eb146112 --- /dev/null +++ b/src/lib/sensor/configuration/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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_configuration + GNSS.cpp + GNSS.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..0e80a8d225 --- /dev/null +++ b/src/lib/sensor/configuration/GNSS.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 "GNSS.hpp" + +#include "Utilities.hpp" +#include +#include + +using namespace matrix; +using namespace time_literals; + +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 calibration_index) +{ + if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { + _configuration_index = calibration_index; + return true; + } + + return false; +} + +void GNSS::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id); + + if (_configuration_index == -1) { + // no saved calibration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool GNSS::ParametersLoad() +{ + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // SENS_GNSSx_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_GNSSx_POS{X,Y,Z} + set_position(GetConfigurationParamsVector3f(SensorString(), "POS", _configuration_index)); + + return true; + } + + return false; +} + +void GNSS::Reset() +{ + _position.zero(); + + _priority = DEFAULT_PRIORITY; + + _configuration_index = -1; +} + +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 calibration slot (matching existing or first available slot) + int8_t calibration_index_prev = _configuration_index; + _configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index); + + if (calibration_index_prev >= 0 && (calibration_index_prev != _configuration_index)) { + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + calibration_index_prev, _configuration_index); + } + } + + if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) { + // save calibration + bool success = true; + success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id); + success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority); + success &= SetConfigurationParamsVector3f(SensorString(), "POS", _configuration_index, _position); + + 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..520781aa27 --- /dev/null +++ b/src/lib/sensor/configuration/GNSS.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 + +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); + void set_position(const matrix::Vector3f &position) { _position = position; } + + bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); } + 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: + int8_t _configuration_index{-1}; + uint32_t _device_id{0}; + int32_t _priority{-1}; + + matrix::Vector3f _position{}; +}; + +} // 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..a71a6ed1e8 --- /dev/null +++ b/src/lib/sensor/configuration/Utilities.cpp @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * 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 math::radians; +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 calibration slot then keep it + int calibration_index = FindCurrentConfigurationIndex(sensor_type, device_id); + + if (calibration_index >= 0) { + return calibration_index; + } + + + // device isn't currently using a calibration slot, select user preference (preferred_index) + // if available, otherwise use the first available slot + uint32_t cal_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) { + cal_device_ids[i] = device_id_val; + } + } + + // use preferred_index if it's available + if ((preferred_index >= 0) && (preferred_index < MAX_SENSOR_COUNT) + && (cal_device_ids[preferred_index] == 0)) { + + calibration_index = preferred_index; + + } else { + // otherwise select first available slot + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (cal_device_ids[i] == 0) { + calibration_index = i; + break; + } + } + } + + if (calibration_index == -1) { + PX4_ERR("no %s calibration slots available", sensor_type); + } + + return calibration_index; +} + +int32_t GetConfigurationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance) +{ + // eg SENS_GNSSn_ID + char str[16 + 1] {}; + snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); + + int32_t value = 0; + + if (param_get(param_find(str), &value) != 0) { + PX4_ERR("failed to get %s", str); + } + + return value; +} + +float GetConfigurationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance) +{ + // eg SENS_GNSSn_OFF + char str[16 + 1] {}; + snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); + + float value = NAN; + + if (param_get(param_find(str), &value) != 0) { + PX4_ERR("failed to get %s", str); + } + + return value; +} + +Vector3f GetConfigurationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance) +{ + Vector3f values{0.f, 0.f, 0.f}; + + char str[16 + 1] {}; + + for (int axis = 0; axis < 3; axis++) { + char axis_char = 'X' + axis; + + // eg SENS_GNSSn_{X,Y,Z}POS + snprintf(str, sizeof(str), "SENS_%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 SetConfigurationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values) +{ + int ret = PX4_OK; + char str[16 + 1] {}; + + for (int axis = 0; axis < 3; axis++) { + char axis_char = 'X' + axis; + + // eg SENS_GNSSn_{X,Y,Z}POS + snprintf(str, sizeof(str), "SENS_%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; +} + +} // 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..548f843b9b --- /dev/null +++ b/src/lib/sensor/configuration/Utilities.hpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * 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 calibration index if it exists. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param device_id + * @return int8_t Valid calibration index on success, -1 otherwise + */ +int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id); + +/** + * @brief Find sensor's calibration 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 calibration index on success, -1 otherwise + */ +int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index = -1); + +/** + * @brief Get sensor calibration parameter value. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance + * @return int32_t The calibration value. + */ +int32_t GetConfigurationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance); +float GetConfigurationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance); + +/** + * @brief Set a single configuration paramter. + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GNSS", "GYRO", "MAG") + * @param cal_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 *cal_type, uint8_t instance, T value) +{ + char str[16 + 1] {}; + + // eg SENS_GNSSn_ID + snprintf(str, sizeof(str), "SENS_%s%u_%s", sensor_type, instance, cal_type); + + int ret = param_set_no_notification(param_find(str), &value); + + if (ret != PX4_OK) { + PX4_ERR("failed to set %s", str); + } + + return ret == PX4_OK; +} + +/** + * @brief Get the Configuration Params Vector 3f object + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance Configuration index (0 - 3) + * @return matrix::Vector3f Vector of calibration values. + */ +matrix::Vector3f GetConfigurationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance); + +/** + * @brief Set the Configuration Params Vector 3f object + * + * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance Configuration 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 SetConfigurationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, + matrix::Vector3f values); + +} // namespace utilities +} // namespace sensor diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index bc2cee9b43..e6f24e5936 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include @@ -84,7 +84,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s is_calibration_valid = true; } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0); + is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0); } const float accel_magnitude = sqrtf(accel.get().x * accel.get().x diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index 0f330969fa..7a553fc992 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include #include @@ -82,7 +82,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & is_calibration_valid = true; } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0); + is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0); } } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index fe0f803a97..9d4c994eef 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include #include @@ -83,7 +83,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st is_calibration_valid = true; } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0); + is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0); } for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index eedf0b3233..9f574ec6f9 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) || !PX4_ISFINITE(offset(0)) diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 4d69f37311..9c590309c6 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 @@ -102,7 +102,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 a32a0511cf..90b976b7d1 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 9903990a53..086b260b41 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++) { @@ -1015,7 +1015,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 2387dc5a15..45caf73773 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -142,6 +142,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 outputSample { @@ -176,6 +177,7 @@ struct gpsSample { float hacc{}; ///< 1-std horizontal position error (m) float vacc{}; ///< 1-std vertical position error (m) float sacc{}; ///< 1-std speed error (m/sec) + Vector3f position_body{}; ///< xyz position of the GPS antenna in body frame (m) }; struct magSample { @@ -361,7 +363,6 @@ struct parameters { // XYZ offset of sensors in body axes (m) Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m) - Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 68e4f61267..a85671e2a3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -109,7 +109,7 @@ void Ekf::controlFusionModes() 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 pos_offset_body = _gps_sample_delayed.position_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/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 326390cfae..107f9ddecc 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -182,6 +182,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); } else { ECL_ERR("GPS data too fast %" PRIu64, gps.time_usec - _time_last_gps); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a2d72c3ec5..f0fa46841a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -129,9 +129,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_imu_pos_x(_params->imu_pos_body(0)), _param_ekf2_imu_pos_y(_params->imu_pos_body(1)), _param_ekf2_imu_pos_z(_params->imu_pos_body(2)), - _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_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), @@ -1833,6 +1830,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 4c94bfd24f..8be02a3bc1 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -511,12 +511,11 @@ private: (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 8a4fddcc8b..f272408a44 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -842,33 +842,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 c12d24f8bc..a0066f6aef 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 @@ -92,7 +92,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::WelfordMean _gyro_mean[MAX_SENSORS] {}; float _temperature[MAX_SENSORS] {}; hrt_abstime _gyro_last_update[MAX_SENSORS] {}; diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp index 3d0723ac0d..a9d7183aff 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 @@ -95,7 +95,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 d64b87de93..aa74a6dded 100644 --- a/src/modules/sensors/module.yaml +++ b/src/modules/sensors/module.yaml @@ -545,3 +545,70 @@ parameters: volatile: true num_instances: *max_num_sensor_instances instance_start: 0 + + # GNSS configuration + SENS_GNSS${i}_ID: + description: + short: GNSS ${i} calibration device ID + long: Device ID of the GNSS this calibration 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 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ae7f718bb8..da6328edef 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -45,8 +45,8 @@ #include #include #include - -#include +#include +#include #include #include #include @@ -95,7 +95,7 @@ #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) # include "vehicle_magnetometer/VehicleMagnetometer.hpp" -# include +# include # include #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER @@ -419,16 +419,16 @@ 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(); } @@ -437,16 +437,16 @@ int Sensors::parameters_update() // 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(); } @@ -456,21 +456,41 @@ int Sensors::parameters_update() #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) // sensor_mag { - 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) { - calibration::Magnetometer mag_cal(device_id_mag); + sensor::calibration::Magnetometer mag_cal(device_id_mag); } 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(); } } #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) @@ -948,6 +968,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) { @@ -967,15 +996,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/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 390f25767f..cfdad39057 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 @@ -98,7 +98,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 47b9bf98a0..27ef7bd77f 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 @@ -115,7 +115,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..12d3053c69 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -91,11 +91,31 @@ void VehicleGPSPosition::ParametersUpdate(bool force) } } + for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) { + _configuration[instance].ParametersUpdate(); + + _gps_blending.setAntennaOffset(_configuration[instance].position(), instance); + } + _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); + } } } @@ -104,6 +124,17 @@ void VehicleGPSPosition::Run() perf_begin(_cycle_perf); ParametersUpdate(); + // update attitude + if (_vehicle_attitude_sub.updated()) { + // Transform offset from NED to body frame + vehicle_attitude_s attitude; + + if (_vehicle_attitude_sub.update(&attitude)) { + // Quaternion rotation from the FRD body frame to the NED earth frame + _gps_blending.setAttitude(matrix::Quatf{attitude.q}); + } + } + // Check all GPS instance bool any_gps_updated = false; bool gps_updated = false; @@ -116,11 +147,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 +165,27 @@ 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_blending.blended_antenna_offset().copyTo(gps_output.position_offset); + + } 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 +197,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..adbc9154da 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 @@ -45,6 +46,7 @@ #include #include #include +#include #include "gps_blending.hpp" @@ -66,7 +68,6 @@ public: private: void Run() override; - void ParametersUpdate(bool force = false); // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm @@ -75,27 +76,30 @@ private: static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4; // define max number of GPS receivers supported - static constexpr int GPS_MAX_RECEIVERS = 2; + static constexpr int GPS_MAX_RECEIVERS = 3; static_assert(GPS_MAX_RECEIVERS == GpsBlending::GPS_MAX_RECEIVERS_BLEND, "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND"); uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] { /**< sensor data subscription */ {this, ORB_ID(sensor_gps), 0}, {this, ORB_ID(sensor_gps), 1}, + {this, ORB_ID(sensor_gps), 2}, }; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; 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 35e88c9964..e9be651fba 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -331,14 +331,16 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) // With updated weights we can calculate a blended GPS solution and // offsets for each physical receiver sensor_gps_s gps_blended_state = gps_blend_states(blend_weights); + matrix::Vector3f blended_antenna_offset{}; update_gps_offsets(gps_blended_state); // calculate a blended output from the offset corrected receiver data // publish if blending was successful - calc_gps_blend_output(gps_blended_state, blend_weights); + calc_gps_blend_output(gps_blended_state, blended_antenna_offset, blend_weights); _gps_blended_state = gps_blended_state; + _blended_antenna_offset = blended_antenna_offset; _selected_gps = GPS_MAX_RECEIVERS_BLEND; _is_new_output_data_available = true; } @@ -425,11 +427,6 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS gps_blended_state.vel_ned_valid = false; } } - - // 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; } /* @@ -493,6 +490,7 @@ 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); @@ -545,7 +543,7 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) } } -void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, +void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, matrix::Vector3f &antenna_offset, float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const { // Convert each GPS position to a local NEU offset relative to the reference position @@ -582,6 +580,10 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, // sum weighted offsets blended_alt_offset_mm += vert_offset * blend_weights[i]; + + // TODO: review + matrix::Vector3f temp_antenna_offset = _antenna_offset[i] * blend_weights[i]; + antenna_offset += temp_antenna_offset; } } @@ -597,4 +599,24 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); gps_blended_state.alt = gps_blended_state.alt + (int32_t)blended_alt_offset_mm; + + // Get the antenna offset + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + // Compute the offset of each GPS relative to the blended position in NEU + matrix::Vector3f offset_ned; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + &offset_ned(0), &offset_ned(1)); + + offset_ned(2) = -1.0 * (gps_blended_state.alt - _gps_state[i].alt); + + const matrix::Vector3f offset_body_frd = _R.transpose() * offset_ned; + + // Get the temp antenna offset between the blended position and the center of the drone according to each GPS + matrix::Vector3f temp_antenna_offset = _antenna_offset[i] - offset_body_frd; + + // Since we have measurements errors, the offset between the blended position and the center of the will be different for each GPS + // Therefore, we can take a weighted average + antenna_offset += temp_antenna_offset * blend_weights[i]; + } } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index fa52806632..e22dcad3b0 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -63,7 +63,7 @@ public: ~GpsBlending() = default; // define max number of GPS receivers supported for blending - static constexpr int GPS_MAX_RECEIVERS_BLEND = 2; + static constexpr int GPS_MAX_RECEIVERS_BLEND = 3; void setGpsData(const sensor_gps_s &gps_data, int instance) { @@ -72,6 +72,16 @@ public: _gps_updated[instance] = true; } } + + void setAntennaOffset(const matrix::Vector3f &antenna_offset, int instance) + { + if (instance < GPS_MAX_RECEIVERS_BLEND) { + _antenna_offset[instance] = antenna_offset; + } + } + + void setAttitude(const matrix::Quatf &q) { _R = q; } + 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 +92,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,8 +102,11 @@ public: return _gps_blended_state; } } + int getSelectedGps() const { return _selected_gps; } + const matrix::Vector3f &blended_antenna_offset() const { return _blended_antenna_offset; } + private: /* * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical @@ -118,14 +132,19 @@ private: /* Calculate GPS output that is a blend of the offset corrected physical receiver data */ - void calc_gps_blend_output(sensor_gps_s &gps_blended_state, float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const; + void calc_gps_blend_output(sensor_gps_s &gps_blended_state, matrix::Vector3f &antenna_offset, + float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const; sensor_gps_s _gps_state[GPS_MAX_RECEIVERS_BLEND] {}; ///< internal state data for the physical GPS + matrix::Vector3f _antenna_offset[GPS_MAX_RECEIVERS_BLEND] {}; + sensor_gps_s _gps_blended_state {}; + matrix::Vector3f _blended_antenna_offset{}; + 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 + int _primary_instance{-1}; ///< if -1, there is no primary instance and the best receiver is used bool _fallback_allowed{false}; bool _is_new_output_data_available{false}; @@ -144,4 +163,6 @@ private: bool _blend_use_vpos_acc{false}; float _blending_time_constant{0.f}; + + matrix::Dcmf _R{matrix::eye()}; ///< rotation from the FRD body frame to the NED earth frame }; 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 8d46073cb1..195250cc89 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..4cb0d0c5d9 100644 --- a/src/modules/sensors/vehicle_gps_position/params.c +++ b/src/modules/sensors/vehicle_gps_position/params.c @@ -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 261b09a6ff..dd7f03fc79 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 f4f5f520c1..8eded64961 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -39,8 +39,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -105,8 +105,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 6ea7d8e617..04ebde6463 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 d47fa87428..e1064ce07b 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 611cf7c2e8..f384c22bd9 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..7119263a1b 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,13 @@ 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_cal_index = sensor::calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id); if (accel_cal_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::calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index); if (accel_priority_old != _accel.priority_configured[uorb_index]) { if (_accel.priority_configured[uorb_index] == 0) { @@ -111,13 +111,13 @@ void VotedSensorsUpdate::parametersUpdate() } // find corresponding configured gyro priority - int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id); + int8_t gyro_cal_index = sensor::calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id); if (gyro_cal_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::calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index); if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { if (_gyro.priority_configured[uorb_index] == 0) {