fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: SENS_GNSS instance configuration and GPS blending updates

Browse Source 
- SENS_GNSSx_ID
 - SENS_GNSSx_{X,Y,Z}POS
This commit is contained in:
Daniel Agar 2022-03-25 15:04:31 -04:00
parent 7f2fea1cca
commit 93232f9ee7
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
58 changed files with 1275 additions and 300 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
View File

@ -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

4
ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
View File

@ -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

6
ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
View File

@ -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

2
msg/sensor_gps.msg
View File

@ -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

2
src/lib/CMakeLists.txt
View File

@ -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)

42
src/lib/sensor/CMakeLists.txt Normal file
View File

@ -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)

140
src/lib/sensor/Utilities.cpp Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#if defined(CONFIG_I2C)
# include <px4_platform_common/i2c.h>
#endif // CONFIG_I2C
#if defined(CONFIG_SPI)
# include <px4_platform_common/spi.h>
#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<enum Rotation>(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

77
src/lib/sensor/Utilities.hpp Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <matrix/math.hpp>
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

7
src/lib/sensor_calibration/Accelerometer.cpp → src/lib/sensor/calibration/Accelerometer.cpp
View File

@ -33,13 +33,15 @@
#include "Accelerometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
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

6
src/lib/sensor_calibration/Accelerometer.hpp → src/lib/sensor/calibration/Accelerometer.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Utilities.hpp"
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
@ -40,6 +42,8 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
namespace sensor
{
namespace calibration
{
class Accelerometer
@ -116,4 +120,6 @@ private:
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

7
src/lib/sensor_calibration/Barometer.cpp → src/lib/sensor/calibration/Barometer.cpp
View File

@ -33,13 +33,15 @@
#include "Barometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
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

7
src/lib/sensor_calibration/Barometer.hpp → src/lib/sensor/calibration/Barometer.hpp
View File

@ -33,13 +33,18 @@
#pragma once
#include "Utilities.hpp"
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
namespace sensor
{
namespace calibration
{
class Barometer
{
public:
@ -110,4 +115,6 @@ private:
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

8
src/lib/sensor_calibration/CMakeLists.txt → src/lib/sensor/calibration/CMakeLists.txt
View File

@ -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
)

7
src/lib/sensor_calibration/Gyroscope.cpp → src/lib/sensor/calibration/Gyroscope.cpp
View File

@ -33,13 +33,15 @@
#include "Gyroscope.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
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

7
src/lib/sensor_calibration/Gyroscope.hpp → src/lib/sensor/calibration/Gyroscope.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Utilities.hpp"
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
@ -40,8 +42,11 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
namespace sensor
{
namespace calibration
{
class Gyroscope
{
public:
@ -119,4 +124,6 @@ private:
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

6
src/lib/sensor_calibration/Magnetometer.cpp → src/lib/sensor/calibration/Magnetometer.cpp
View File

@ -32,14 +32,17 @@
****************************************************************************/
#include "Magnetometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
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

7
src/lib/sensor_calibration/Magnetometer.hpp → src/lib/sensor/calibration/Magnetometer.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Utilities.hpp"
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
@ -41,8 +43,11 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
namespace sensor
{
namespace calibration
{
class Magnetometer
{
public:
@ -117,4 +122,6 @@ private:
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

122
src/lib/sensor_calibration/Utilities.cpp → src/lib/sensor/calibration/Utilities.cpp
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#if defined(CONFIG_I2C)
# include <px4_platform_common/i2c.h>
#endif // CONFIG_I2C
#if defined(CONFIG_SPI)
# include <px4_platform_common/spi.h>
#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<enum Rotation>(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

41
src/lib/sensor_calibration/Utilities.hpp → src/lib/sensor/calibration/Utilities.hpp
View File

@ -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 <lib/parameters/param.h>
#include <matrix/math.hpp>
namespace sensor
{
namespace calibration
{
@ -85,10 +87,10 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui
template<typename T>
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

45
src/lib/sensor/configuration/CMakeLists.txt Normal file
View File

@ -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
)

174
src/lib/sensor/configuration/GNSS.cpp Normal file
View File

@ -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 <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
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

86
src/lib/sensor/configuration/GNSS.hpp Normal file
View File

@ -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 <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
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

203
src/lib/sensor/configuration/Utilities.cpp Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
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

127
src/lib/sensor/configuration/Utilities.hpp Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <matrix/math.hpp>
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<typename T>
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

4
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
View File

@ -37,7 +37,7 @@
#include <HealthFlags.h>
#include <math.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
@ -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

4
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
View File

@ -36,7 +36,7 @@
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
@ -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);
}
}

4
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
View File

@ -36,7 +36,7 @@
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
@ -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++) {

12
src/modules/commander/accelerometer_calibration.cpp
View File

@ -131,8 +131,8 @@
#include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h>
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Accelerometer.hpp>
#include <lib/sensor/Utilities.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <matrix/math.hpp>
@ -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))

6
src/modules/commander/baro_calibration.cpp
View File

@ -49,8 +49,8 @@
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include <lib/geo/geo.h>
#include <lib/sensor_calibration/Barometer.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Barometer.hpp>
#include <lib/sensor/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <lib/systemlib/err.h>
#include <uORB/Subscription.hpp>
@ -102,7 +102,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
uORB::SubscriptionMultiArray<sensor_baro_s, MAX_SENSOR_COUNT> 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] {};

6
src/modules/commander/gyro_calibration.cpp
View File

@ -52,8 +52,8 @@
#include <lib/mathlib/math/filter/MedianFilter.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Gyroscope.hpp>
#include <lib/sensor/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
@ -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] {};

10
src/modules/commander/mag_calibration.cpp
View File

@ -50,8 +50,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <matrix/math.hpp>
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Magnetometer.hpp>
#include <lib/sensor/Utilities.hpp>
#include <lib/conversion/rotation.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <lib/systemlib/mavlink_log.h>
@ -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);

3
src/modules/ekf2/EKF/common.h
View File

@ -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)

2
src/modules/ekf2/EKF/control.cpp
View File

@ -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;

2
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -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);

4
src/modules/ekf2/EKF2.cpp
View File

@ -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);

5
src/modules/ekf2/EKF2.hpp
View File

@ -511,12 +511,11 @@ private:
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)

27
src/modules/ekf2/ekf2_params.c
View File

@ -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)
*

4
src/modules/gyro_calibration/GyroCalibration.hpp
View File

@ -41,7 +41,7 @@
#include <drivers/drv_hrt.h>
#include <lib/mathlib/math/WelfordMean.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor/calibration/Gyroscope.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
@ -92,7 +92,7 @@ private:
uORB::SubscriptionMultiArray<sensor_accel_s, MAX_SENSORS> _sensor_accel_subs{ORB_ID::sensor_accel};
uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
sensor::calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
math::WelfordMean<matrix::Vector3f> _gyro_mean[MAX_SENSORS] {};
float _temperature[MAX_SENSORS] {};
hrt_abstime _gyro_last_update[MAX_SENSORS] {};

4
src/modules/mag_bias_estimator/MagBiasEstimator.hpp
View File

@ -37,7 +37,7 @@
#include <lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/sensor/calibration/Magnetometer.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
@ -95,7 +95,7 @@ private:
uORB::Publication<magnetometer_bias_estimate_s> _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] {};

4
src/modules/mavlink/streams/MAG_CAL_REPORT.hpp
View File

@ -34,7 +34,7 @@
#ifndef MAG_CAL_REPORT_HPP
#define MAG_CAL_REPORT_HPP
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/sensor/calibration/Magnetometer.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_mag.h>
@ -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{};

1
src/modules/sensors/CMakeLists.txt
View File

@ -78,6 +78,7 @@ px4_add_module(
data_validator
mathlib
sensor_calibration
sensor_configuration
vehicle_imu
)

67
src/modules/sensors/module.yaml
View File

@ -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

62
src/modules/sensors/sensors.cpp
View File

@ -45,8 +45,8 @@
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <lib/sensor/configuration/Utilities.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
@ -95,7 +95,7 @@
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
# include "vehicle_magnetometer/VehicleMagnetometer.hpp"
# include <lib/sensor_calibration/Magnetometer.hpp>
# include <lib/sensor/calibration/Magnetometer.hpp>
# include <uORB/topics/sensor_mag.h>
#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_s> 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_s> 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_s> 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_s> 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) {

4
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@ -33,7 +33,7 @@
#pragma once
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/sensor/calibration/Accelerometer.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
@ -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{};

4
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
View File

@ -35,7 +35,7 @@
#include "data_validator/DataValidatorGroup.hpp"
#include <lib/sensor_calibration/Barometer.hpp>
#include <lib/sensor/calibration/Barometer.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
@ -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")};

4
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -34,7 +34,7 @@
#pragma once
#include <containers/Bitset.hpp>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor/calibration/Gyroscope.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
@ -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{};

67
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
View File

@ -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

12
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
View File

@ -36,6 +36,7 @@
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor/configuration/GNSS.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@ -45,6 +46,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_attitude.h>
#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<sensor_gps_s> _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<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,
(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime
(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
)
};
}; // namespace sensors

36
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
View File

@ -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];
}
}

27
src/modules/sensors/vehicle_gps_position/gps_blending.hpp
View File

@ -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<float, 3>()}; ///< rotation from the FRD body frame to the NED earth frame
};

4
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
View File

@ -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);

19
src/modules/sensors/vehicle_gps_position/params.c
View File

@ -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);

2
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -35,7 +35,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/events.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <float.h>

8
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -39,8 +39,8 @@
#include <lib/mathlib/math/WelfordMean.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor/calibration/Accelerometer.hpp>
#include <lib/sensor/calibration/Gyroscope.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@ -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{};

4
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -36,7 +36,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/events.h>
#include <lib/geo/geo.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
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();

4
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
View File

@ -35,7 +35,7 @@
#include "data_validator/DataValidatorGroup.hpp"
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/sensor/calibration/Magnetometer.hpp>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
@ -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 {

4
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp
View File

@ -41,7 +41,7 @@
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor/calibration/Gyroscope.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@ -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")};

10
src/modules/sensors/voted_sensors_update.cpp
View File

@ -39,7 +39,7 @@
#include "voted_sensors_update.h"
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <lib/geo/geo.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
@ -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) {