Compare commits

...

1 Commits

Author SHA1 Message Date
Daniel Agar bbb19f7c7f split sensor calibration and configuration
- introduce SENS_GNSS instance configuration and GPS blending updates
 - SENS_GNSSx_ID
 - SENS_GNSSx_{X,Y,Z}POS
2023-11-30 21:23:05 -05:00
74 changed files with 3460 additions and 1369 deletions

View File

@ -143,9 +143,12 @@ then
param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
param set CAL_MAG0_ID 197388
param set CAL_MAG0_PRIO 50
param set SENS_MAG0_ID 197388
param set SENS_MAG0_PRIO 50
param set CAL_MAG1_ID 197644
param set CAL_MAG1_PRIO 50
param set SENS_MAG1_ID 197644
param set SENS_MAG1_PRIO 50
param set SENS_BOARD_X_OFF 0.000001
param set SENS_DPRES_OFF 0.001

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 FW_ARSP_MODE 1

View File

@ -18,8 +18,8 @@ param set-default COM_DISARM_LAND 0.5
# EKF2 parameters
param set-default EKF2_DRAG_CTRL 1
param set-default EKF2_IMU_POS_X 0.02
param set-default EKF2_GPS_POS_X 0.055
param set-default EKF2_GPS_POS_Z -0.15
param set-default SENS_GNSS0_XPOS 0.055
param set-default SENS_GNSS0_ZPOS -0.15
param set-default EKF2_MIN_RNG 0.03
param set-default EKF2_OF_CTRL 1
param set-default EKF2_OF_POS_X 0.055

View File

@ -46,7 +46,9 @@ param set-default EKF2_BCOEF_Y 25.5
param set-default EKF2_DRAG_CTRL 1
param set-default EKF2_GPS_DELAY 100
param set-default EKF2_GPS_POS_X 0.06
param set-default SENS_GNSS0_XPOS 0.06
param set-default SENS_GNSS0_YPOS 0.0
param set-default SENS_GNSS0_ZPOS 0.0
param set-default EKF2_GPS_V_NOISE 0.5
param set-default EKF2_IMU_POS_X 0.06

View File

@ -62,4 +62,6 @@ uint8 RTCM_MSG_USED_NOT_USED = 1
uint8 RTCM_MSG_USED_USED = 2
uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver
float32[3] position_offset # position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
# TOPICS sensor_gps vehicle_gps_position

View File

@ -67,7 +67,7 @@ add_subdirectory(pid_design EXCLUDE_FROM_ALL)
add_subdirectory(rate_control EXCLUDE_FROM_ALL)
add_subdirectory(rc EXCLUDE_FROM_ALL)
add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
add_subdirectory(sensor EXCLUDE_FROM_ALL)
add_subdirectory(slew_rate EXCLUDE_FROM_ALL)
add_subdirectory(systemlib EXCLUDE_FROM_ALL)
add_subdirectory(system_identification EXCLUDE_FROM_ALL)

View File

@ -44,5 +44,24 @@ bool param_modify_on_import(bson_node_t node)
{
// TODO:
// ACC, BARO, GYRO, MAG
// prio, rot
// 2023-11-30: translate sensor calibration to configuration
{
if (strcmp("CAL_ACC0_PRIO", node->name) == 0) {
strcpy(node->name, "SENS_ACC0_PRIO");
PX4_INFO("copying %s -> %s", "CAL_ACC0_PRIO", "SENS_ACC0_PRIO");
// TODO: set SENS_ACC0_ID=CAL_ACC0_ID
return true;
}
}
return false;
}

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(configuration)
add_subdirectory(calibration)

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

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

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,10 +36,14 @@
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace calibration
{
@ -48,7 +52,8 @@ Accelerometer::Accelerometer()
Reset();
}
Accelerometer::Accelerometer(uint32_t device_id)
Accelerometer::Accelerometer(uint32_t device_id) :
_configuration(device_id)
{
set_device_id(device_id);
}
@ -57,10 +62,9 @@ void Accelerometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
if (_configuration.device_id() != device_id || _configuration.external() != external) {
_device_id = device_id;
_external = external;
_configuration.set_device_id(device_id);
Reset();
@ -75,7 +79,7 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
if (_sensor_correction_sub.updated() || force) {
// valid device id required
if (_device_id == 0) {
if (device_id() == 0) {
return;
}
@ -84,17 +88,20 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
if (_sensor_correction_sub.copy(&corrections)) {
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.accel_device_ids[i] == _device_id) {
if (corrections.accel_device_ids[i] == device_id()) {
switch (i) {
case 0:
_thermal_offset = Vector3f{corrections.accel_offset_0};
return;
case 1:
_thermal_offset = Vector3f{corrections.accel_offset_1};
return;
case 2:
_thermal_offset = Vector3f{corrections.accel_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.accel_offset_3};
return;
@ -110,12 +117,15 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
bool Accelerometer::set_offset(const Vector3f &offset)
{
if (Vector3f(_offset - offset).longerThan(0.01f)) {
if (offset.isAllFinite()) {
_offset = offset;
_calibration_count++;
return true;
}
static constexpr float MIN_OFFSET_CHANGE = 0.01f;
if (offset.isAllFinite()
&& (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0))
) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
@ -123,25 +133,20 @@ bool Accelerometer::set_offset(const Vector3f &offset)
bool Accelerometer::set_scale(const Vector3f &scale)
{
if (Vector3f(_scale - scale).longerThan(0.01f)) {
if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) {
_scale = scale;
_calibration_count++;
return true;
}
static constexpr float MIN_SCALE_CHANGE = 0.01f;
if (scale.isAllFinite() && scale.longerThan(0.f)
&& (Vector3f(_scale - scale).longerThan(MIN_SCALE_CHANGE) || (_calibration_count == 0))
) {
_scale = scale;
_calibration_count++;
return true;
}
return false;
}
void Accelerometer::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
// always apply board level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
}
bool Accelerometer::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
@ -154,11 +159,11 @@ bool Accelerometer::set_calibration_index(int calibration_index)
void Accelerometer::ParametersUpdate()
{
if (_device_id == 0) {
if (device_id() == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
if (_calibration_index == -1) {
// no saved calibration available
@ -166,52 +171,32 @@ void Accelerometer::ParametersUpdate()
} else {
ParametersLoad();
if (calibrated() && !_configuration.configured()) {
_configuration.ParametersSave(calibration_index());
}
}
}
bool Accelerometer::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_ACCx_ROT
int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
}
// CAL_{}n_{X,Y,Z}OFF
Vector3f offset{
GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index)
};
set_rotation(static_cast<Rotation>(rotation_value));
// CAL_{}n_{X,Y,Z}SCALE
Vector3f scale{
GetCalibrationParamFloat(SensorString(), "XSCALE", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YSCALE", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZSCALE", _calibration_index)
};
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
// CAL_ACCx_PRIO
_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
if (_priority != CAL_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_calibration_index, _priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
// CAL_ACCx_OFF{X,Y,Z}
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
// CAL_ACCx_SCALE{X,Y,Z}
set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
return true;
return set_offset(offset) && set_scale(scale);
}
return false;
@ -219,21 +204,11 @@ bool Accelerometer::ParametersLoad()
void Accelerometer::Reset()
{
if (_external) {
set_rotation(ROTATION_NONE);
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
_offset.zero();
_scale = Vector3f{1.f, 1.f, 1.f};
_thermal_offset.zero();
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
@ -241,6 +216,8 @@ void Accelerometer::Reset()
bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
{
_configuration.ParametersSave();
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
@ -249,10 +226,10 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
calibration_index_prev, _calibration_index);
}
}
@ -260,17 +237,18 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
}
// CAL_{}n_{X,Y,Z}OFF
success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0));
success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1));
success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2));
// CAL_{}n_{X,Y,Z}SCALE
success &= SetCalibrationParam(SensorString(), "XSCALE", _calibration_index, _scale(0));
success &= SetCalibrationParam(SensorString(), "YSCALE", _calibration_index, _scale(1));
success &= SetCalibrationParam(SensorString(), "ZSCALE", _calibration_index, _scale(2));
return success;
}
@ -297,9 +275,10 @@ void Accelerometer::PrintStatus()
}
if (_thermal_offset.norm() > 0.f) {
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), _device_id,
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(),
(double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
}
}
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,13 +33,16 @@
#pragma once
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
#include <lib/sensor/configuration/Accelerometer.hpp>
namespace sensor
{
namespace calibration
{
class Accelerometer
@ -47,9 +50,6 @@ class Accelerometer
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "ACC"; }
Accelerometer();
@ -61,34 +61,39 @@ public:
bool set_calibration_index(int calibration_index);
void set_device_id(uint32_t device_id);
bool set_offset(const matrix::Vector3f &offset);
bool set_scale(const matrix::Vector3f &scale);
void set_rotation(Rotation rotation);
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); }
bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
uint8_t calibration_count() const { return _calibration_count; }
int8_t calibration_index() const { return _calibration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const matrix::Vector3f &offset() const { return _offset; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
const matrix::Vector3f &scale() const { return _scale; }
uint32_t device_id() const { return _configuration.device_id(); }
bool enabled() const { return _configuration.enabled(); }
bool external() const { return _configuration.external(); }
const int32_t &priority() const { return _configuration.priority(); }
const matrix::Dcmf &rotation() const { return _configuration.rotation(); }
const Rotation &rotation_enum() const { return _configuration.rotation_enum(); }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
{
return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
return _configuration.rotation() * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
}
// Compute sensor offset from bias (board frame)
matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
{
// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
return _offset + (_rotation.I() * bias).edivide(_scale);
return _offset + (_configuration.rotation().I() * bias).edivide(_scale);
}
bool ParametersLoad();
@ -100,21 +105,17 @@ public:
void SensorCorrectionsUpdate(bool force = false);
private:
sensor::configuration::Accelerometer _configuration{};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _scale;
matrix::Vector3f _thermal_offset;
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,10 +36,14 @@
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace calibration
{
@ -48,7 +52,8 @@ Barometer::Barometer()
Reset();
}
Barometer::Barometer(uint32_t device_id)
Barometer::Barometer(uint32_t device_id) :
_configuration(device_id)
{
set_device_id(device_id);
}
@ -57,10 +62,9 @@ void Barometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
if (_configuration.device_id() != device_id || _configuration.external() != external) {
_device_id = device_id;
_external = external;
_configuration.set_device_id(device_id);
Reset();
@ -75,7 +79,7 @@ void Barometer::SensorCorrectionsUpdate(bool force)
if (_sensor_correction_sub.updated() || force) {
// valid device id required
if (_device_id == 0) {
if (device_id() == 0) {
return;
}
@ -84,7 +88,7 @@ void Barometer::SensorCorrectionsUpdate(bool force)
if (_sensor_correction_sub.copy(&corrections)) {
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.baro_device_ids[i] == _device_id) {
if (corrections.baro_device_ids[i] == device_id()) {
switch (i) {
case 0:
_thermal_offset = corrections.baro_offset_0;
@ -113,12 +117,15 @@ void Barometer::SensorCorrectionsUpdate(bool force)
bool Barometer::set_offset(const float &offset)
{
if (fabsf(_offset - offset) > 0.01f) {
if (PX4_ISFINITE(offset)) {
_offset = offset;
_calibration_count++;
return true;
}
static constexpr float MIN_OFFSET_CHANGE = 0.01f;
if (PX4_ISFINITE(offset)
&& ((fabsf(_offset - offset) > MIN_OFFSET_CHANGE) || (_calibration_count == 0))
) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
@ -136,11 +143,11 @@ bool Barometer::set_calibration_index(int calibration_index)
void Barometer::ParametersUpdate()
{
if (_device_id == 0) {
if (device_id() == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
if (_calibration_index == -1) {
// no saved calibration available
@ -148,33 +155,21 @@ void Barometer::ParametersUpdate()
} else {
ParametersLoad();
if (calibrated() && !_configuration.configured()) {
_configuration.ParametersSave(calibration_index());
}
}
}
bool Barometer::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_BAROx_PRIO
_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
// CAL_{}n_OFF
float offset = GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index);
if (_priority != CAL_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_calibration_index, _priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
// CAL_BAROx_OFF
set_offset(GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index));
return true;
return set_offset(offset);
}
return false;
@ -186,8 +181,6 @@ void Barometer::Reset()
_thermal_offset = 0;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
@ -195,6 +188,8 @@ void Barometer::Reset()
bool Barometer::ParametersSave(int desired_calibration_index, bool force)
{
_configuration.ParametersSave();
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
@ -203,10 +198,10 @@ bool Barometer::ParametersSave(int desired_calibration_index, bool force)
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
calibration_index_prev, _calibration_index);
}
}
@ -214,8 +209,10 @@ bool Barometer::ParametersSave(int desired_calibration_index, bool force)
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
// CAL_{}n_OFF
success &= SetCalibrationParam(SensorString(), "OFF", _calibration_index, _offset);
return success;
@ -239,8 +236,10 @@ void Barometer::PrintStatus()
}
if (fabsf(_thermal_offset) > 0.f) {
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), _device_id, (double)_thermal_offset);
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), device_id(),
(double)_thermal_offset);
}
}
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,10 +34,14 @@
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
#include <lib/sensor/configuration/Barometer.hpp>
namespace sensor
{
namespace calibration
{
class Barometer
@ -45,9 +49,6 @@ class Barometer
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "BARO"; }
Barometer();
@ -59,18 +60,21 @@ public:
bool set_calibration_index(int calibration_index);
void set_device_id(uint32_t device_id);
bool set_offset(const float &offset);
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
uint8_t calibration_count() const { return _calibration_count; }
int8_t calibration_index() const { return _calibration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const float &offset() const { return _offset; }
const int32_t &priority() const { return _priority; }
const float &thermal_offset() const { return _thermal_offset; }
uint32_t device_id() const { return _configuration.device_id(); }
bool enabled() const { return _configuration.enabled(); }
bool external() const { return _configuration.external(); }
const int32_t &priority() const { return _configuration.priority(); }
// apply offsets
inline float Correct(const float &data) const
{
@ -85,7 +89,8 @@ public:
// Compute sensor offset from bias (board frame)
float BiasCorrectedSensorOffset(const float &bias) const
{
return bias + _thermal_offset + _offset;
// updated calibration offset = existing offset + bias
return _offset + bias;
}
bool ParametersLoad();
@ -97,17 +102,16 @@ public:
void SensorCorrectionsUpdate(bool force = false);
private:
sensor::configuration::Barometer _configuration{};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
float _offset{0};
float _thermal_offset{0};
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

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,9 @@ px4_add_library(sensor_calibration
Utilities.hpp
)
target_link_libraries(sensor_calibration PRIVATE conversion)
target_link_libraries(sensor_calibration
PRIVATE
conversion
sensor_configuration
sensor_utilities
)

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,10 +36,14 @@
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace calibration
{
@ -48,7 +52,8 @@ Gyroscope::Gyroscope()
Reset();
}
Gyroscope::Gyroscope(uint32_t device_id)
Gyroscope::Gyroscope(uint32_t device_id) :
_configuration(device_id)
{
set_device_id(device_id);
}
@ -57,10 +62,9 @@ void Gyroscope::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
if (_configuration.device_id() != device_id || _configuration.external() != external) {
_device_id = device_id;
_external = external;
_configuration.set_device_id(device_id);
Reset();
@ -75,7 +79,7 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
if (_sensor_correction_sub.updated() || force) {
// valid device id required
if (_device_id == 0) {
if (device_id() == 0) {
return;
}
@ -84,17 +88,20 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
if (_sensor_correction_sub.copy(&corrections)) {
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _device_id) {
if (corrections.gyro_device_ids[i] == device_id()) {
switch (i) {
case 0:
_thermal_offset = Vector3f{corrections.gyro_offset_0};
return;
case 1:
_thermal_offset = Vector3f{corrections.gyro_offset_1};
return;
case 2:
_thermal_offset = Vector3f{corrections.gyro_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.gyro_offset_3};
return;
@ -110,25 +117,20 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
bool Gyroscope::set_offset(const Vector3f &offset)
{
if (Vector3f(_offset - offset).longerThan(0.01f) || (_calibration_count == 0)) {
if (offset.isAllFinite()) {
_offset = offset;
_calibration_count++;
return true;
}
static constexpr float MIN_OFFSET_CHANGE = 0.01f;
if (offset.isAllFinite()
&& (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0))
) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
}
void Gyroscope::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
// always apply board level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
}
bool Gyroscope::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
@ -141,11 +143,11 @@ bool Gyroscope::set_calibration_index(int calibration_index)
void Gyroscope::ParametersUpdate()
{
if (_device_id == 0) {
if (device_id() == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
if (_calibration_index == -1) {
// no saved calibration available
@ -153,49 +155,25 @@ void Gyroscope::ParametersUpdate()
} else {
ParametersLoad();
if (calibrated() && !_configuration.configured()) {
_configuration.ParametersSave(calibration_index());
}
}
}
bool Gyroscope::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_GYROx_ROT
int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
}
// CAL_{}n_{X,Y,Z}OFF
Vector3f offset{
GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index)
};
set_rotation(static_cast<Rotation>(rotation_value));
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
// CAL_GYROx_PRIO
_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
if (_priority != CAL_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_calibration_index, _priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
// CAL_GYROx_OFF{X,Y,Z}
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
return true;
return set_offset(offset);
}
return false;
@ -203,20 +181,10 @@ bool Gyroscope::ParametersLoad()
void Gyroscope::Reset()
{
if (_external) {
set_rotation(ROTATION_NONE);
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
_offset.zero();
_thermal_offset.zero();
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
@ -224,6 +192,8 @@ void Gyroscope::Reset()
bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
{
_configuration.ParametersSave();
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
@ -232,10 +202,10 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
calibration_index_prev, _calibration_index);
}
}
@ -243,16 +213,13 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
}
// CAL_{}n_{X,Y,Z}OFF
success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0));
success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1));
success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2));
return success;
}
@ -270,15 +237,17 @@ void Gyroscope::PrintStatus()
rotation_enum());
} else {
PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n",
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n",
SensorString(), device_id(), enabled(),
(double)_offset(0), (double)_offset(1), (double)_offset(2));
}
if (_thermal_offset.norm() > 0.f) {
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), _device_id,
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(),
(double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
}
}
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,13 +33,16 @@
#pragma once
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
#include <lib/sensor/configuration/Gyroscope.hpp>
namespace sensor
{
namespace calibration
{
class Gyroscope
@ -47,9 +50,6 @@ class Gyroscope
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "GYRO"; }
Gyroscope();
@ -61,38 +61,43 @@ public:
bool set_calibration_index(int calibration_index);
void set_device_id(uint32_t device_id);
bool set_offset(const matrix::Vector3f &offset);
void set_rotation(Rotation rotation);
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
bool set_offset(const matrix::Vector3f &offset);
void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); }
bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
uint8_t calibration_count() const { return _calibration_count; }
int8_t calibration_index() const { return _calibration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const matrix::Vector3f &offset() const { return _offset; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
const matrix::Vector3f &thermal_offset() const { return _thermal_offset; }
uint32_t device_id() const { return _configuration.device_id(); }
bool enabled() const { return _configuration.enabled(); }
bool external() const { return _configuration.external(); }
const int32_t &priority() const { return _configuration.priority(); }
const matrix::Dcmf &rotation() const { return _configuration.rotation(); }
const Rotation &rotation_enum() const { return _configuration.rotation_enum(); }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
{
return _rotation * matrix::Vector3f{data - _thermal_offset - _offset};
return _configuration.rotation() * matrix::Vector3f{data - _thermal_offset - _offset};
}
inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
{
return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
return (_configuration.rotation().I() * corrected_data) + _thermal_offset + _offset;
}
// Compute sensor offset from bias (board frame)
matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
{
// updated calibration offset = existing offset + bias rotated to sensor frame
return _offset + (_rotation.I() * bias);
return _offset + (_configuration.rotation().I() * bias);
}
bool ParametersLoad();
@ -104,20 +109,16 @@ public:
void SensorCorrectionsUpdate(bool force = false);
private:
sensor::configuration::Gyroscope _configuration{};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _thermal_offset;
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

View File

@ -0,0 +1,337 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Magnetometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace calibration
{
Magnetometer::Magnetometer()
{
Reset();
}
Magnetometer::Magnetometer(uint32_t device_id) :
_configuration(device_id)
{
set_device_id(device_id);
}
void Magnetometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_configuration.device_id() != device_id || _configuration.external() != external) {
_configuration.set_device_id(device_id);
Reset();
ParametersUpdate();
SensorCorrectionsUpdate(true);
}
}
void Magnetometer::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
// valid device id required
if (device_id() == 0) {
return;
}
sensor_correction_s corrections;
if (_sensor_correction_sub.copy(&corrections)) {
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.mag_device_ids[i] == device_id()) {
switch (i) {
case 0:
_thermal_offset = Vector3f{corrections.mag_offset_0};
return;
case 1:
_thermal_offset = Vector3f{corrections.mag_offset_1};
return;
case 2:
_thermal_offset = Vector3f{corrections.mag_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.mag_offset_3};
return;
}
}
}
}
// zero thermal offset if not found
_thermal_offset.zero();
}
}
bool Magnetometer::set_offset(const Vector3f &offset)
{
static constexpr float MIN_OFFSET_CHANGE = 0.005f;
if (offset.isAllFinite()
&& (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0))
) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
}
bool Magnetometer::set_scale(const Vector3f &scale)
{
static constexpr float MIN_SCALE_CHANGE = 0.01f;
if (scale.isAllFinite() && scale.longerThan(0.f)
&& (Vector3f(_scale.diag() - scale).longerThan(MIN_SCALE_CHANGE) || (_calibration_count == 0))
) {
_scale(0, 0) = scale(0);
_scale(1, 1) = scale(1);
_scale(2, 2) = scale(2);
_calibration_count++;
return true;
}
return false;
}
bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
{
if (offdiagonal.isAllFinite()
&& (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)
|| (_calibration_count == 0))
) {
_scale(0, 1) = offdiagonal(0);
_scale(1, 0) = offdiagonal(0);
_scale(0, 2) = offdiagonal(1);
_scale(2, 0) = offdiagonal(1);
_scale(1, 2) = offdiagonal(2);
_scale(2, 1) = offdiagonal(2);
_calibration_count++;
return true;
}
return false;
}
bool Magnetometer::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
_calibration_index = calibration_index;
return true;
}
return false;
}
void Magnetometer::ParametersUpdate()
{
if (device_id() == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
if (_calibration_index == -1) {
// no saved calibration available
Reset();
} else {
ParametersLoad();
if (calibrated() && !_configuration.configured()) {
_configuration.ParametersSave(calibration_index());
}
}
}
bool Magnetometer::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_{}n_{X,Y,Z}OFF
Vector3f offset{
GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index)
};
// CAL_{}n_{X,Y,Z}SCALE
Vector3f scale{
GetCalibrationParamFloat(SensorString(), "XSCALE", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YSCALE", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZSCALE", _calibration_index)
};
// CAL_{}n_{X,Y,Z}ODIAG
Vector3f odiag{
GetCalibrationParamFloat(SensorString(), "XODIAG", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YODIAG", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZODIAG", _calibration_index)
};
// CAL_{}n_{X,Y,Z}COMP
Vector3f comp{
GetCalibrationParamFloat(SensorString(), "XCOMP", _calibration_index),
GetCalibrationParamFloat(SensorString(), "YCOMP", _calibration_index),
GetCalibrationParamFloat(SensorString(), "ZCOMP", _calibration_index)
};
_power_compensation = comp;
return set_offset(offset) && set_scale(scale) && set_offdiagonal(odiag);
}
return false;
}
void Magnetometer::Reset()
{
_offset.zero();
_scale.setIdentity();
_power_compensation.zero();
_power = 0.f;
_thermal_offset.zero();
_calibration_index = -1;
_calibration_count = 0;
}
bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
{
_configuration.ParametersSave();
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
} else if (!force || (_calibration_index < 0)
|| (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
calibration_index_prev, _calibration_index);
}
}
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
// CAL_{}n_{X,Y,Z}OFF
success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0));
success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1));
success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2));
// CAL_{}n_{X,Y,Z}SCALE
success &= SetCalibrationParam(SensorString(), "XSCALE", _calibration_index, _scale(0, 0));
success &= SetCalibrationParam(SensorString(), "YSCALE", _calibration_index, _scale(1, 1));
success &= SetCalibrationParam(SensorString(), "ZSCALE", _calibration_index, _scale(2, 2));
// CAL_{}n_{X,Y,Z}ODIAG
success &= SetCalibrationParam(SensorString(), "XODIAG", _calibration_index, _scale(0, 1));
success &= SetCalibrationParam(SensorString(), "YODIAG", _calibration_index, _scale(0, 2));
success &= SetCalibrationParam(SensorString(), "ZODIAG", _calibration_index, _scale(1, 2));
// CAL_{}n_{X,Y,Z}COMP
success &= SetCalibrationParam(SensorString(), "XCOMP", _calibration_index, _power_compensation(0));
success &= SetCalibrationParam(SensorString(), "YCOMP", _calibration_index, _power_compensation(1));
success &= SetCalibrationParam(SensorString(), "ZCOMP", _calibration_index, _power_compensation(2));
return success;
}
return false;
}
void Magnetometer::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
SensorString(), device_id(), enabled(),
(double)_offset(0), (double)_offset(1), (double)_offset(2),
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
rotation_enum());
} else {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
SensorString(), device_id(), enabled(),
(double)_offset(0), (double)_offset(1), (double)_offset(2),
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
}
if (_thermal_offset.norm() > 0.f) {
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(),
(double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
}
}
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,14 +33,16 @@
#pragma once
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_correction.h>
#include <lib/sensor/configuration/Magnetometer.hpp>
namespace sensor
{
namespace calibration
{
class Magnetometer
@ -48,9 +50,6 @@ class Magnetometer
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "MAG"; }
Magnetometer();
@ -62,48 +61,40 @@ public:
bool set_calibration_index(int calibration_index);
void set_device_id(uint32_t device_id);
bool set_offset(const matrix::Vector3f &offset);
bool set_scale(const matrix::Vector3f &scale);
bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
/**
* @brief Set the rotation enum & corresponding rotation matrix for Magnetometer
*
* @param rotation Rotation enum
*/
void set_rotation(Rotation rotation);
void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); }
/**
* @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer
*
* @param rotation Rotation euler angles
*/
void set_custom_rotation(const matrix::Eulerf &rotation);
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
uint8_t calibration_count() const { return _calibration_count; }
int8_t calibration_index() const { return _calibration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const matrix::Vector3f &offset() const { return _offset; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
const matrix::Matrix3f &scale() const { return _scale; }
uint32_t device_id() const { return _configuration.device_id(); }
bool enabled() const { return _configuration.enabled(); }
bool external() const { return _configuration.external(); }
const int32_t &priority() const { return _configuration.priority(); }
const matrix::Dcmf &rotation() const { return _configuration.rotation(); }
const Rotation &rotation_enum() const { return _configuration.rotation_enum(); }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
{
return _rotation * (_scale * ((data + _power * _power_compensation) - _offset));
return _configuration.rotation() * (_scale * ((data + _power * _power_compensation) - _offset));
}
// Compute sensor offset from bias (board frame)
matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
{
// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
return _offset + (_scale.I() * _rotation.I() * bias);
return _offset + (_scale.I() * _configuration.rotation().I() * bias);
}
bool ParametersLoad();
@ -117,17 +108,10 @@ public:
void UpdatePower(float power) { _power = power; }
private:
sensor::configuration::Magnetometer _configuration{};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
/**
* @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD)
*/
matrix::Dcmf _rotation;
matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional)
matrix::Vector3f _offset;
matrix::Matrix3f _scale;
matrix::Vector3f _thermal_offset;
@ -136,11 +120,8 @@ private:
float _power{0.f};
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,26 +31,20 @@
*
****************************************************************************/
#include "Utilities.hpp"
#include <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,7 +57,7 @@ int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id)
}
for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
char str[20] {};
char str[16 + 1] {};
snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
int32_t device_id_val = 0;
@ -102,7 +96,7 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
uint32_t cal_device_ids[MAX_SENSOR_COUNT] {};
for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
char str[20] {};
char str[16 + 1] {};
snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
int32_t device_id_val = 0;
@ -136,8 +130,8 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance)
{
// eg CAL_MAGn_ID/CAL_MAGn_ROT
char str[20] {};
// eg CAL_{}n_ID
char str[16 + 1] {};
snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
int32_t value = 0;
@ -151,8 +145,8 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance)
{
// eg CAL_BAROn_OFF
char str[20] {};
// eg CAL_{}n_OFF
char str[16 + 1] {};
snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
float value = NAN;
@ -164,124 +158,5 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui
return value;
}
Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
{
Vector3f values{0.f, 0.f, 0.f};
char str[20] {};
for (int axis = 0; axis < 3; axis++) {
char axis_char = 'X' + axis;
// eg CAL_MAGn_{X,Y,Z}OFF
snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
if (param_get(param_find(str), &values(axis)) != 0) {
PX4_ERR("failed to get %s", str);
}
}
return values;
}
bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values)
{
int ret = PX4_OK;
char str[20] {};
for (int axis = 0; axis < 3; axis++) {
char axis_char = 'X' + axis;
// eg CAL_MAGn_{X,Y,Z}OFF
snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
if (param_set_no_notification(param_find(str), &values(axis)) != 0) {
PX4_ERR("failed to set %s = %.4f", str, (double)values(axis));
ret = PX4_ERROR;
}
}
return ret == PX4_OK;
}
Eulerf GetSensorLevelAdjustment()
{
float x_offset = 0.f;
float y_offset = 0.f;
float z_offset = 0.f;
param_get(param_find("SENS_BOARD_X_OFF"), &x_offset);
param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset);
param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset);
return Eulerf{radians(x_offset), radians(y_offset), radians(z_offset)};
}
enum Rotation GetBoardRotation()
{
// get transformation matrix from sensor/board to body frame
int32_t board_rot = -1;
param_get(param_find("SENS_BOARD_ROT"), &board_rot);
if (board_rot >= 0 && board_rot < Rotation::ROTATION_MAX) {
return static_cast<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_device_external(device_id);
#endif // CONFIG_I2C
break;
case device::Device::DeviceBusType_SPI:
#if defined(CONFIG_SPI)
external = px4_spi_bus_external(id.devid_s.bus);
#endif // CONFIG_SPI
break;
case device::Device::DeviceBusType_UAVCAN:
external = true;
break;
case device::Device::DeviceBusType_SIMULATION:
external = false;
break;
case device::Device::DeviceBusType_SERIAL:
external = true;
break;
case device::Device::DeviceBusType_MAVLINK:
external = true;
break;
case device::Device::DeviceBusType_UNKNOWN:
external = true;
break;
}
return external;
}
} // namespace calibration
} // namespace sensor

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -40,6 +40,8 @@
#include <lib/parameters/param.h>
#include <matrix/math.hpp>
namespace sensor
{
namespace calibration
{
@ -85,9 +87,9 @@ 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
// eg CAL_{}n_ID
snprintf(str, sizeof(str), "CAL_%s%u_%s", sensor_type, instance, cal_type);
int ret = param_set_no_notification(param_find(str), &value);
@ -99,54 +101,5 @@ bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t
return ret == PX4_OK;
}
/**
* @brief Get the Calibration Params Vector 3f object
*
* @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
* @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
* @param instance Calibration index (0 - 3)
* @return matrix::Vector3f Vector of calibration values.
*/
matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance);
/**
* @brief Set the Calibration Params Vector 3f object
*
* @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
* @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
* @param instance Calibration index (0 - 3)
* @param values Vector of calibration values x, y, z.
* @return true if the parameter name was valid and all values saved successfully, false otherwise.
*/
bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance,
matrix::Vector3f values);
/**
* @brief Get the board sensor level adjustment (SENS_BOARD_X_OFF, SENS_BOARD_Y_OFF, SENS_BOARD_Z_OFF).
*
* @return matrix::Eulerf
*/
matrix::Eulerf GetSensorLevelAdjustment();
/**
* @brief Get the board rotation.
*
* @return enum Rotation
*/
Rotation GetBoardRotation();
/**
* @brief Get the board rotation Dcm.
*
* @return matrix::Dcmf
*/
matrix::Dcmf GetBoardRotationMatrix();
/**
* @brief Determine if device is on an external bus
*
* @return true if device is on an external bus
*/
bool DeviceExternal(uint32_t device_id);
} // namespace calibration
} // namespace utilities
} // namespace sensor

View File

@ -0,0 +1,223 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Accelerometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace configuration
{
Accelerometer::Accelerometer()
{
Reset();
}
Accelerometer::Accelerometer(uint32_t device_id)
{
set_device_id(device_id);
}
void Accelerometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
}
}
bool Accelerometer::set_configuration_index(int configuration_index)
{
if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
_configuration_index = configuration_index;
return true;
}
return false;
}
void Accelerometer::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
// always apply board level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
}
void Accelerometer::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
if (_configuration_index == -1) {
// no saved configuration available
Reset();
} else {
ParametersLoad();
}
}
bool Accelerometer::ParametersLoad()
{
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// SENS_{}n_ROT
int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
}
set_rotation(static_cast<Rotation>(rotation_value));
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
// SENS_{}n_PRIO
_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
if (_priority != SENS_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_configuration_index, _priority);
SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
return true;
}
return false;
}
void Accelerometer::Reset()
{
if (_external) {
set_rotation(ROTATION_NONE);
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_configuration_index = -1;
_configuration_count = 0;
}
bool Accelerometer::ParametersSave(int desired_configuration_index, bool force)
{
if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
_configuration_index = desired_configuration_index;
} else if (!force || (_configuration_index < 0)
|| (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
// ensure we have a valid configuration slot (matching existing or first available slot)
int8_t configuration_index_prev = _configuration_index;
_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
configuration_index_prev, _configuration_index);
}
}
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// save configuration
bool success = true;
success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
if (_external) {
success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum);
} else {
success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal
}
return success;
}
return false;
}
void Accelerometer::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Ext ROT: %d\n",
SensorString(), device_id(), enabled(),
rotation_enum());
} else {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Internal\n",
SensorString(), device_id(), enabled());
}
}
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
namespace sensor
{
namespace configuration
{
class Accelerometer
{
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "ACC"; }
Accelerometer();
explicit Accelerometer(uint32_t device_id);
~Accelerometer() = default;
void PrintStatus();
bool set_configuration_index(int configuration_index);
void set_device_id(uint32_t device_id);
void set_rotation(Rotation rotation);
bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
uint8_t configuration_count() const { return _configuration_count; }
int8_t configuration_index() const { return _configuration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
bool ParametersLoad();
bool ParametersSave(int desired_configuration_index = -1, bool force = false);
void ParametersUpdate();
void Reset();
private:
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
int8_t _configuration_index{-1};
uint8_t _configuration_count{0};
};
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,183 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Barometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace configuration
{
Barometer::Barometer()
{
Reset();
}
Barometer::Barometer(uint32_t device_id)
{
set_device_id(device_id);
}
void Barometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
}
}
bool Barometer::set_configuration_index(int configuration_index)
{
if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
_configuration_index = configuration_index;
return true;
}
return false;
}
void Barometer::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
if (_configuration_index == -1) {
// no saved configuration available
Reset();
} else {
ParametersLoad();
}
}
bool Barometer::ParametersLoad()
{
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// SENS_{}n_PRIO
_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
if (_priority != SENS_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_configuration_index, _priority);
SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
return true;
}
return false;
}
void Barometer::Reset()
{
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_configuration_index = -1;
_configuration_count = 0;
}
bool Barometer::ParametersSave(int desired_configuration_index, bool force)
{
if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
_configuration_index = desired_configuration_index;
} else if (!force || (_configuration_index < 0)
|| (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
// ensure we have a valid configuration slot (matching existing or first available slot)
int8_t configuration_index_prev = _configuration_index;
_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
configuration_index_prev, _configuration_index);
}
}
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// save configuration
bool success = true;
success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
return success;
}
return false;
}
void Barometer::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, External\n",
SensorString(), device_id(), enabled());
} else {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Internal\n",
SensorString(), device_id(), enabled());
}
}
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <lib/matrix/matrix/math.hpp>
namespace sensor
{
namespace configuration
{
class Barometer
{
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "BARO"; }
Barometer();
explicit Barometer(uint32_t device_id);
~Barometer() = default;
void PrintStatus();
bool set_configuration_index(int configuration_index);
void set_device_id(uint32_t device_id);
bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
uint8_t configuration_count() const { return _configuration_count; }
int8_t configuration_index() const { return _configuration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
bool ParametersLoad();
bool ParametersSave(int desired_configuration_index = -1, bool force = false);
void ParametersUpdate();
void Reset();
private:
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
int8_t _configuration_index{-1};
uint8_t _configuration_count{0};
};
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,53 @@
############################################################################
#
# Copyright (c) 2023 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(sensor_configuration
Accelerometer.cpp
Accelerometer.hpp
Barometer.cpp
Barometer.hpp
GNSS.cpp
GNSS.hpp
Gyroscope.cpp
Gyroscope.hpp
Magnetometer.cpp
Magnetometer.hpp
Utilities.cpp
Utilities.hpp
)
target_link_libraries(sensor_configuration
PRIVATE
conversion
sensor_utilities
)

View File

@ -0,0 +1,204 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "GNSS.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace configuration
{
GNSS::GNSS()
{
Reset();
}
GNSS::GNSS(uint32_t device_id)
{
set_device_id(device_id);
}
void GNSS::set_device_id(uint32_t device_id)
{
if (_device_id != device_id) {
_device_id = device_id;
Reset();
ParametersUpdate();
}
}
bool GNSS::set_configuration_index(int configuration_index)
{
if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
_configuration_index = configuration_index;
return true;
}
return false;
}
bool GNSS::set_position(const matrix::Vector3f &position)
{
static constexpr float MIN_POSITION_CHANGE = 1e-6;
if (position.isAllFinite()
&& (Vector3f(_position - position).longerThan(MIN_POSITION_CHANGE) || (_configuration_count == 0))
) {
_position = position;
_configuration_count++;
return true;
}
return false;
}
void GNSS::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
if (_configuration_index == -1) {
// no saved configuration available
Reset();
} else {
ParametersLoad();
}
}
bool GNSS::ParametersLoad()
{
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// SENS_{}n_PRIO
_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
if (_priority != SENS_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_configuration_index, _priority);
SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
}
_priority = DEFAULT_PRIORITY;
}
// SENS_{}n_{X,Y,Z}POS:
Vector3f position{0.f, 0.f, 0.f};
position(0) = GetConfigurationParamFloat(SensorString(), "XPOS", _configuration_index);
position(1) = GetConfigurationParamFloat(SensorString(), "YPOS", _configuration_index);
position(2) = GetConfigurationParamFloat(SensorString(), "ZPOS", _configuration_index);
set_position(position);
return true;
}
return false;
}
void GNSS::Reset()
{
_position.zero();
_priority = DEFAULT_PRIORITY;
_configuration_index = -1;
_configuration_count = 0;
}
bool GNSS::ParametersSave(int desired_configuration_index, bool force)
{
if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
_configuration_index = desired_configuration_index;
} else if (!force || (_configuration_index < 0)
|| (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
// ensure we have a valid configuration slot (matching existing or first available slot)
int8_t configuration_index_prev = _configuration_index;
_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
configuration_index_prev, _configuration_index);
}
}
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// save configuration
bool success = true;
success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
// SENS_{}n_{X,Y,Z}POS
success &= SetConfigurationParam(SensorString(), "XPOS", _configuration_index, _position(0));
success &= SetConfigurationParam(SensorString(), "YPOS", _configuration_index, _position(0));
success &= SetConfigurationParam(SensorString(), "ZPOS", _configuration_index, _position(0));
return success;
}
return false;
}
void GNSS::PrintStatus()
{
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, position: [%05.3f %05.3f %05.3f]\n",
SensorString(), device_id(), enabled(),
(double)_position(0), (double)_position(1), (double)_position(2));
}
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,93 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <lib/matrix/matrix/math.hpp>
namespace sensor
{
namespace configuration
{
class GNSS
{
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr const char *SensorString() { return "GNSS"; }
GNSS();
explicit GNSS(uint32_t device_id);
~GNSS() = default;
void PrintStatus();
bool set_configuration_index(int configuration_index);
void set_device_id(uint32_t device_id);
bool set_position(const matrix::Vector3f &position);
bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
uint8_t configuration_count() const { return _configuration_count; }
int8_t configuration_index() const { return _configuration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
const matrix::Vector3f &position() const { return _position; }
const int32_t &priority() const { return _priority; }
bool ParametersLoad();
bool ParametersSave(int desired_configuration_index = -1, bool force = false);
void ParametersUpdate();
void Reset();
private:
uint32_t _device_id{0};
int32_t _priority{-1};
matrix::Vector3f _position{};
int8_t _configuration_index{-1};
uint8_t _configuration_count{0};
};
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,223 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Gyroscope.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace configuration
{
Gyroscope::Gyroscope()
{
Reset();
}
Gyroscope::Gyroscope(uint32_t device_id)
{
set_device_id(device_id);
}
void Gyroscope::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
}
}
bool Gyroscope::set_configuration_index(int configuration_index)
{
if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
_configuration_index = configuration_index;
return true;
}
return false;
}
void Gyroscope::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
// always apply board level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
}
void Gyroscope::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
if (_configuration_index == -1) {
// no saved configuration available
Reset();
} else {
ParametersLoad();
}
}
bool Gyroscope::ParametersLoad()
{
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// SENS_{}n_ROT
int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
}
set_rotation(static_cast<Rotation>(rotation_value));
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
// SENS_{}n_PRIO
_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
if (_priority != SENS_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_configuration_index, _priority);
SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
return true;
}
return false;
}
void Gyroscope::Reset()
{
if (_external) {
set_rotation(ROTATION_NONE);
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_configuration_index = -1;
_configuration_count = 0;
}
bool Gyroscope::ParametersSave(int desired_configuration_index, bool force)
{
if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
_configuration_index = desired_configuration_index;
} else if (!force || (_configuration_index < 0)
|| (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
// ensure we have a valid configuration slot (matching existing or first available slot)
int8_t configuration_index_prev = _configuration_index;
_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
configuration_index_prev, _configuration_index);
}
}
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// save configuration
bool success = true;
success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
if (_external) {
success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum);
} else {
success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal
}
return success;
}
return false;
}
void Gyroscope::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Ext ROT: %d\n",
SensorString(), device_id(), enabled(),
rotation_enum());
} else {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Internal\n",
SensorString(), device_id(), enabled());
}
}
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
namespace sensor
{
namespace configuration
{
class Gyroscope
{
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "GYRO"; }
Gyroscope();
explicit Gyroscope(uint32_t device_id);
~Gyroscope() = default;
void PrintStatus();
bool set_configuration_index(int configuration_index);
void set_device_id(uint32_t device_id);
void set_rotation(Rotation rotation);
bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
uint8_t configuration_count() const { return _configuration_count; }
int8_t configuration_index() const { return _configuration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
bool ParametersLoad();
bool ParametersSave(int desired_configuration_index = -1, bool force = false);
void ParametersUpdate();
void Reset();
private:
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
int8_t _configuration_index{-1};
uint8_t _configuration_count{0};
};
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Magnetometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
#include <lib/sensor/Utilities.hpp>
using namespace matrix;
using namespace time_literals;
using namespace sensor::utilities;
namespace sensor
{
namespace configuration
{
Magnetometer::Magnetometer()
{
Reset();
}
Magnetometer::Magnetometer(uint32_t device_id)
{
set_device_id(device_id);
}
void Magnetometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
}
}
bool Magnetometer::set_configuration_index(int configuration_index)
{
if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
_configuration_index = configuration_index;
return true;
}
return false;
}
void Magnetometer::set_rotation(const Rotation rotation)
{
if (rotation < ROTATION_MAX) {
_rotation_enum = rotation;
} else {
// invalid rotation, resetting
_rotation_enum = ROTATION_NONE;
}
// always apply level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum);
// clear any custom rotation
_rotation_custom_euler.zero();
}
void Magnetometer::set_custom_rotation(const Eulerf &rotation)
{
_rotation_enum = ROTATION_CUSTOM;
// store custom rotation
_rotation_custom_euler = rotation;
// always apply board level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler);
// TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation
// between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot.
// however, since while doing the 'level-configuration', users don't put the vehicle truly *horizontal, the
// measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply
// internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset
// values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective)
}
void Magnetometer::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
if (_configuration_index == -1) {
// no saved configuration available
Reset();
} else {
ParametersLoad();
}
}
bool Magnetometer::ParametersLoad()
{
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// SENS_{}n_ROT
int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index);
const float euler_roll_deg = GetConfigurationParamFloat(SensorString(), "ROLL", _configuration_index);
const float euler_pitch_deg = GetConfigurationParamFloat(SensorString(), "PITCH", _configuration_index);
const float euler_yaw_deg = GetConfigurationParamFloat(SensorString(), "YAW", _configuration_index);
if (_external) {
if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) {
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
}
// if SENS_{}n_{ROLL,PITCH,YAW} manually set then SENS_{}n_ROT needs to be ROTATION_CUSTOM
if ((rotation_value != ROTATION_CUSTOM)
&& ((fabsf(euler_roll_deg) > FLT_EPSILON)
|| (fabsf(euler_pitch_deg) > FLT_EPSILON)
|| (fabsf(euler_yaw_deg) > FLT_EPSILON))) {
rotation_value = ROTATION_CUSTOM;
SetConfigurationParam(SensorString(), "ROT", _configuration_index, rotation_value);
}
// Handle custom specified euler angle
if (rotation_value == ROTATION_CUSTOM) {
const matrix::Eulerf rotation_custom_euler{
math::radians(euler_roll_deg),
math::radians(euler_pitch_deg),
math::radians(euler_yaw_deg)};
set_custom_rotation(rotation_custom_euler);
} else {
set_rotation(static_cast<Rotation>(rotation_value));
}
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
// SENS_{}n_PRIO
_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
if (_priority != SENS_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_configuration_index, _priority);
SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
return true;
}
return false;
}
void Magnetometer::Reset()
{
if (_external) {
set_rotation(ROTATION_NONE);
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_configuration_index = -1;
_configuration_count = 0;
}
bool Magnetometer::ParametersSave(int desired_configuration_index, bool force)
{
if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
_configuration_index = desired_configuration_index;
} else if (!force || (_configuration_index < 0)
|| (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
// ensure we have a valid configuration slot (matching existing or first available slot)
int8_t configuration_index_prev = _configuration_index;
_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
configuration_index_prev, _configuration_index);
}
}
if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
// save configuration
bool success = true;
success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
if (_external) {
success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum);
} else {
success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal
}
success &= SetConfigurationParam(SensorString(), "ROLL", _configuration_index,
math::degrees(_rotation_custom_euler(0)));
success &= SetConfigurationParam(SensorString(), "PITCH", _configuration_index,
math::degrees(_rotation_custom_euler(1)));
success &= SetConfigurationParam(SensorString(), "YAW", _configuration_index,
math::degrees(_rotation_custom_euler(2)));
return success;
}
return false;
}
void Magnetometer::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Ext ROT: %d\n",
SensorString(), device_id(), enabled(),
rotation_enum());
} else {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, Internal\n",
SensorString(), device_id(), enabled());
}
}
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,112 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
namespace sensor
{
namespace configuration
{
class Magnetometer
{
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "MAG"; }
Magnetometer();
explicit Magnetometer(uint32_t device_id);
~Magnetometer() = default;
void PrintStatus();
bool set_configuration_index(int configuration_index);
void set_device_id(uint32_t device_id);
void set_rotation(Rotation rotation);
/**
* @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer
*
* @param rotation Rotation euler angles
*/
void set_custom_rotation(const matrix::Eulerf &rotation);
bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
uint8_t configuration_count() const { return _configuration_count; }
int8_t configuration_index() const { return _configuration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
bool ParametersLoad();
bool ParametersSave(int desired_configuration_index = -1, bool force = false);
void ParametersUpdate();
void Reset();
private:
Rotation _rotation_enum{ROTATION_NONE};
/**
* @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD)
*/
matrix::Dcmf _rotation;
matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional)
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
int8_t _configuration_index{-1};
uint8_t _configuration_count{0};
};
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,162 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Utilities.hpp"
#include <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 matrix::Eulerf;
using matrix::Dcmf;
using matrix::Vector3f;
namespace sensor
{
namespace configuration
{
static constexpr int MAX_SENSOR_COUNT = 4; // TODO: per sensor?
int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id)
{
if (device_id == 0) {
return -1;
}
for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
char str[16 + 1] {};
snprintf(str, sizeof(str), "SENS_%s%u_ID", sensor_type, i);
int32_t device_id_val = 0;
param_t param_handle = param_find_no_notification(str);
if (param_handle == PARAM_INVALID) {
continue;
}
// find again and get value, but this time mark it active
if (param_get(param_find(str), &device_id_val) != OK) {
continue;
}
if ((uint32_t)device_id_val == device_id) {
return i;
}
}
return -1;
}
int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index)
{
// if this device is already using a configuration slot then keep it
int configuration_index = FindCurrentConfigurationIndex(sensor_type, device_id);
if (configuration_index >= 0) {
return configuration_index;
}
// device isn't currently using a configuration slot, select user preference (preferred_index)
// if available, otherwise use the first available slot
uint32_t conf_device_ids[MAX_SENSOR_COUNT] {};
for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
char str[16 + 1] {};
snprintf(str, sizeof(str), "SENS_%s%u_ID", sensor_type, i);
int32_t device_id_val = 0;
if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
conf_device_ids[i] = device_id_val;
}
}
// use preferred_index if it's available
if ((preferred_index >= 0) && (preferred_index < MAX_SENSOR_COUNT)
&& (conf_device_ids[preferred_index] == 0)) {
configuration_index = preferred_index;
} else {
// otherwise select first available slot
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (conf_device_ids[i] == 0) {
configuration_index = i;
break;
}
}
}
if (configuration_index == -1) {
PX4_ERR("no %s configuration slots available", sensor_type);
}
return configuration_index;
}
int32_t GetConfigurationParamInt32(const char *sensor_type, const char *conf_type, uint8_t instance)
{
// eg SENS_{}n_ID
char str[16 + 1] {};
snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, conf_type);
int32_t value = 0;
if (param_get(param_find(str), &value) != 0) {
PX4_ERR("failed to get %s", str);
}
return value;
}
float GetConfigurationParamFloat(const char *sensor_type, const char *conf_type, uint8_t instance)
{
// eg SENS_{}n_OFF
char str[16 + 1] {};
snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, conf_type);
float value = NAN;
if (param_get(param_find(str), &value) != 0) {
PX4_ERR("failed to get %s", str);
}
return value;
}
} // namespace configuration
} // namespace sensor

View File

@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <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 configuration index if it exists.
*
* @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
* @param device_id
* @return int8_t Valid configuration index on success, -1 otherwise
*/
int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id);
/**
* @brief Find sensor's configuration index if it exists, otherwise select an available slot.
*
* @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
* @param device_id
* @param preferred_index preferred index (optional)
* @return int8_t Valid configuration index on success, -1 otherwise
*/
int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index = -1);
/**
* @brief Get sensor configuration parameter value.
*
* @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
* @param conf_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
* @param instance
* @return int32_t The configuration value.
*/
int32_t GetConfigurationParamInt32(const char *sensor_type, const char *conf_type, uint8_t instance);
float GetConfigurationParamFloat(const char *sensor_type, const char *conf_type, uint8_t instance);
/**
* @brief Set a single configuration paramter.
*
* @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GNSS", "GYRO", "MAG")
* @param conf_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
* @param instance Configuration index (0 - 3)
* @param value int32_t parameter value
* @return true if the parameter name was valid and value saved successfully, false otherwise.
*/
template<typename T>
bool SetConfigurationParam(const char *sensor_type, const char *conf_type, uint8_t instance, T value)
{
char str[16 + 1] {};
// eg SENS_{}n_ID
snprintf(str, sizeof(str), "SENS_%s%u_%s", sensor_type, instance, conf_type);
int ret = param_set_no_notification(param_find(str), &value);
if (ret != PX4_OK) {
PX4_ERR("failed to set %s", str);
}
return ret == PX4_OK;
}
} // namespace utilities
} // namespace sensor

View File

@ -1,400 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Magnetometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
using namespace matrix;
using namespace time_literals;
namespace calibration
{
Magnetometer::Magnetometer()
{
Reset();
}
Magnetometer::Magnetometer(uint32_t device_id)
{
set_device_id(device_id);
}
void Magnetometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
SensorCorrectionsUpdate(true);
}
}
void Magnetometer::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
// valid device id required
if (_device_id == 0) {
return;
}
sensor_correction_s corrections;
if (_sensor_correction_sub.copy(&corrections)) {
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.mag_device_ids[i] == _device_id) {
switch (i) {
case 0:
_thermal_offset = Vector3f{corrections.mag_offset_0};
return;
case 1:
_thermal_offset = Vector3f{corrections.mag_offset_1};
return;
case 2:
_thermal_offset = Vector3f{corrections.mag_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.mag_offset_3};
return;
}
}
}
}
// zero thermal offset if not found
_thermal_offset.zero();
}
}
bool Magnetometer::set_offset(const Vector3f &offset)
{
if (Vector3f(_offset - offset).longerThan(0.005f)) {
if (offset.isAllFinite()) {
_offset = offset;
_calibration_count++;
return true;
}
}
return false;
}
bool Magnetometer::set_scale(const Vector3f &scale)
{
if (Vector3f(_scale.diag() - scale).longerThan(0.01f)) {
if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) {
_scale(0, 0) = scale(0);
_scale(1, 1) = scale(1);
_scale(2, 2) = scale(2);
_calibration_count++;
return true;
}
}
return false;
}
bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
{
if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)) {
if (offdiagonal.isAllFinite()) {
_scale(0, 1) = offdiagonal(0);
_scale(1, 0) = offdiagonal(0);
_scale(0, 2) = offdiagonal(1);
_scale(2, 0) = offdiagonal(1);
_scale(1, 2) = offdiagonal(2);
_scale(2, 1) = offdiagonal(2);
_calibration_count++;
return true;
}
}
return false;
}
void Magnetometer::set_rotation(const Rotation rotation)
{
if (rotation < ROTATION_MAX) {
_rotation_enum = rotation;
} else {
// invalid rotation, resetting
_rotation_enum = ROTATION_NONE;
}
// always apply level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum);
// clear any custom rotation
_rotation_custom_euler.zero();
}
void Magnetometer::set_custom_rotation(const Eulerf &rotation)
{
_rotation_enum = ROTATION_CUSTOM;
// store custom rotation
_rotation_custom_euler = rotation;
// always apply board level adjustments
_rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler);
// TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation
// between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot.
// however, since while doing the 'level-calibration', users don't put the vehicle truly *horizontal, the
// measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply
// internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset
// values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective)
}
bool Magnetometer::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
_calibration_index = calibration_index;
return true;
}
return false;
}
void Magnetometer::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
if (_calibration_index == -1) {
// no saved calibration available
Reset();
} else {
ParametersLoad();
}
}
bool Magnetometer::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_MAGx_ROT
int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
const float euler_roll_deg = GetCalibrationParamFloat(SensorString(), "ROLL", _calibration_index);
const float euler_pitch_deg = GetCalibrationParamFloat(SensorString(), "PITCH", _calibration_index);
const float euler_yaw_deg = GetCalibrationParamFloat(SensorString(), "YAW", _calibration_index);
if (_external) {
if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) {
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
}
// if CAL_MAGx_{ROLL,PITCH,YAW} manually set then CAL_MAGx_ROT needs to be ROTATION_CUSTOM
if ((rotation_value != ROTATION_CUSTOM)
&& ((fabsf(euler_roll_deg) > FLT_EPSILON)
|| (fabsf(euler_pitch_deg) > FLT_EPSILON)
|| (fabsf(euler_yaw_deg) > FLT_EPSILON))) {
rotation_value = ROTATION_CUSTOM;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
// Handle custom specified euler angle
if (rotation_value == ROTATION_CUSTOM) {
const matrix::Eulerf rotation_custom_euler{
math::radians(euler_roll_deg),
math::radians(euler_pitch_deg),
math::radians(euler_yaw_deg)};
set_custom_rotation(rotation_custom_euler);
} else {
set_rotation(static_cast<Rotation>(rotation_value));
}
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
// CAL_MAGx_PRIO
_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
if (_priority != CAL_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_calibration_index, _priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
// CAL_MAGx_OFF{X,Y,Z}
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
// CAL_MAGx_SCALE{X,Y,Z}
set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
// CAL_MAGx_ODIAG{X,Y,Z}
set_offdiagonal(GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index));
// CAL_MAGx_COMP{X,Y,Z}
_power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index);
return true;
}
return false;
}
void Magnetometer::Reset()
{
if (_external) {
set_rotation(ROTATION_NONE);
} else {
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
_offset.zero();
_scale.setIdentity();
_power_compensation.zero();
_power = 0.f;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
}
bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
{
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
} else if (!force || (_calibration_index < 0)
|| (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
calibration_index_prev, _calibration_index);
}
}
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
const Vector3f scale{_scale.diag()};
success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale);
const Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)};
success &= SetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index, off_diag);
success &= SetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index, _power_compensation);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
}
success &= SetCalibrationParam(SensorString(), "ROLL", _calibration_index, math::degrees(_rotation_custom_euler(0)));
success &= SetCalibrationParam(SensorString(), "PITCH", _calibration_index, math::degrees(_rotation_custom_euler(1)));
success &= SetCalibrationParam(SensorString(), "YAW", _calibration_index, math::degrees(_rotation_custom_euler(2)));
return success;
}
return false;
}
void Magnetometer::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
SensorString(), device_id(), enabled(),
(double)_offset(0), (double)_offset(1), (double)_offset(2),
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
rotation_enum());
} else {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
SensorString(), device_id(), enabled(),
(double)_offset(0), (double)_offset(1), (double)_offset(2),
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
}
#if defined(DEBUG_BUILD)
_scale.print();
#endif // DEBUG_BUILD
}
} // namespace calibration

View File

@ -33,7 +33,7 @@
#include "accelerometerCheck.hpp"
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
using namespace time_literals;
@ -59,7 +59,7 @@ void AccelerometerChecks::checkAndReport(const Context &context, Report &reporte
is_calibration_valid = true;
} else {
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0);
is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0);
}
reporter.setIsPresent(health_component_t::gyro);

View File

@ -39,7 +39,7 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_accel.h>
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/sensor/calibration/Accelerometer.hpp>
class AccelerometerChecks : public HealthAndArmingCheckBase
{
@ -52,6 +52,6 @@ public:
private:
bool isAccelRequired(int instance);
uORB::SubscriptionMultiArray<sensor_accel_s, calibration::Accelerometer::MAX_SENSOR_COUNT> _sensor_accel_sub{ORB_ID::sensor_accel};
uORB::SubscriptionMultiArray<sensor_accel_s, sensor::calibration::Accelerometer::MAX_SENSOR_COUNT> _sensor_accel_sub{ORB_ID::sensor_accel};
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
};

View File

@ -39,7 +39,7 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/estimator_status.h>
#include <lib/sensor_calibration/Barometer.hpp>
#include <lib/sensor/calibration/Barometer.hpp>
class BaroChecks : public HealthAndArmingCheckBase
{
@ -52,7 +52,7 @@ public:
private:
bool isBaroRequired(int instance);
uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
uORB::SubscriptionMultiArray<sensor_baro_s, sensor::calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,

View File

@ -33,7 +33,7 @@
#include "gyroCheck.hpp"
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
using namespace time_literals;
@ -59,7 +59,7 @@ void GyroChecks::checkAndReport(const Context &context, Report &reporter)
is_calibration_valid = true;
} else {
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0);
is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0);
}
reporter.setIsPresent(health_component_t::gyro);

View File

@ -39,7 +39,7 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_gyro.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor/calibration/Gyroscope.hpp>
class GyroChecks : public HealthAndArmingCheckBase
{
@ -52,6 +52,6 @@ public:
private:
bool isGyroRequired(int instance);
uORB::SubscriptionMultiArray<sensor_gyro_s, calibration::Gyroscope::MAX_SENSOR_COUNT> _sensor_gyro_sub{ORB_ID::sensor_gyro};
uORB::SubscriptionMultiArray<sensor_gyro_s, sensor::calibration::Gyroscope::MAX_SENSOR_COUNT> _sensor_gyro_sub{ORB_ID::sensor_gyro};
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
};

View File

@ -33,7 +33,8 @@
#include "magnetometerCheck.hpp"
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <lib/sensor/configuration/Utilities.hpp>
using namespace time_literals;
@ -68,14 +69,18 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter
num_enabled_and_valid_calibration++;
} else {
int calibration_index = calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
int calibration_index = sensor::calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
is_calibration_valid = (calibration_index >= 0);
if (is_calibration_valid) {
int priority = calibration::GetCalibrationParamInt32("MAG", "PRIO", calibration_index);
int configuration_index = sensor::configuration::FindCurrentConfigurationIndex("MAG", mag_data.device_id);
if (priority > 0) {
++num_enabled_and_valid_calibration;
if (configuration_index >= 0) {
int priority = sensor::configuration::GetConfigurationParamInt32("MAG", "PRIO", configuration_index);
if (priority > 0) {
++num_enabled_and_valid_calibration;
}
}
}
}

View File

@ -40,7 +40,7 @@
#include <uORB/topics/sensor_preflight_mag.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/estimator_status.h>
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/sensor/calibration/Magnetometer.hpp>
class MagnetometerChecks : public HealthAndArmingCheckBase
{
@ -54,7 +54,7 @@ private:
bool isMagRequired(int instance, bool &mag_fault);
void consistencyCheck(const Context &context, Report &reporter);
uORB::SubscriptionMultiArray<sensor_mag_s, calibration::Magnetometer::MAX_SENSOR_COUNT> _sensor_mag_sub{ORB_ID::sensor_mag};
uORB::SubscriptionMultiArray<sensor_mag_s, sensor::calibration::Magnetometer::MAX_SENSOR_COUNT> _sensor_mag_sub{ORB_ID::sensor_mag};
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
uORB::Subscription _sensor_preflight_mag_sub{ORB_ID(sensor_preflight_mag)};

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) || !offset.isAllFinite()) {
PX4_ERR("accel %d quick calibrate failed", accel_index);

View File

@ -49,8 +49,8 @@
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include <lib/atmosphere/atmosphere.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>
@ -77,7 +77,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] {};

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

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++) {
@ -1024,7 +1024,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) {
calibration::Magnetometer cal{mag.device_id};
sensor::calibration::Magnetometer cal{mag.device_id};
// use any existing scale and store the offset to the expected earth field
const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);

View File

@ -177,6 +177,7 @@ struct gpsMessage {
bool vel_ned_valid{}; ///< GPS ground speed is valid
uint8_t nsats{}; ///< number of satellites used
float pdop{}; ///< position dilution of precision
Vector3f position_body{}; ///< xyz position of the GPS antenna in body frame (m)
};
struct imuSample {
@ -198,6 +199,7 @@ struct gpsSample {
float vacc{}; ///< 1-std vertical position error (m)
float sacc{}; ///< 1-std speed error (m/sec)
float yaw_acc{}; ///< 1-std yaw error (rad)
Vector3f position_body{}; ///< xyz position of the GPS antenna in body frame (m)
};
struct magSample {

View File

@ -217,6 +217,8 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
gps_sample_new.pos(1) = 0.0f;
}
gps_sample_new.position_body = gps.position_body;
_gps_buffer->push(gps_sample_new);
_time_last_gps_buffer_push = _time_latest_us;

View File

@ -53,7 +53,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
if (_gps_data_ready) {
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f gps_pos_body = _gps_sample_delayed.position_body;
const Vector3f pos_offset_body = gps_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;

View File

@ -76,9 +76,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
#if defined(CONFIG_EKF2_GNSS)
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
_param_ekf2_gps_delay(_params->gps_delay_ms),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
@ -2454,6 +2451,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
.nsats = vehicle_gps_position.satellites_used,
.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
+ vehicle_gps_position.vdop * vehicle_gps_position.vdop),
.position_body = Vector3f{vehicle_gps_position.position_offset},
};
_ekf.setGpsData(gps_msg);

View File

@ -545,10 +545,6 @@ private:
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z,
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,

View File

@ -941,33 +941,6 @@ PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Y, 0.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Z, 0.0f);
/**
* X position of GPS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_POS_X, 0.0f);
/**
* Y position of GPS antenna in body frame (right axis with origin relative to vehicle centre of gravity)
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Y, 0.0f);
/**
* Z position of GPS antenna in body frame (down axis with origin relative to vehicle centre of gravity)
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Z, 0.0f);
/**
* X position of range finder origin in body frame (forward axis with origin relative to vehicle centre of gravity)
*

View File

@ -41,7 +41,7 @@
#include <drivers/drv_hrt.h>
#include <lib/mathlib/math/WelfordMeanVector.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>
@ -96,7 +96,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::WelfordMeanVector<float, 3> _gyro_mean[MAX_SENSORS] {};
matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {};
float _temperature[MAX_SENSORS] {};

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>
@ -93,7 +93,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] {};

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{};

View File

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

View File

@ -14,81 +14,6 @@ parameters:
category: System
type: int32
default: 0
decimal: 3
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_ACC${i}_PRIO:
description:
short: Accelerometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
decimal: 3
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_ACC${i}_ROT:
description:
short: Accelerometer ${i} rotation relative to airframe
long: |
An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
category: System
type: enum
values:
-1: Internal
0: No rotation
1: Yaw 45°
2: Yaw 90°
3: Yaw 135°
4: Yaw 180°
5: Yaw 225°
6: Yaw 270°
7: Yaw 315°
8: Roll 180°
9: Roll 180°, Yaw 45°
10: Roll 180°, Yaw 90°
11: Roll 180°, Yaw 135°
12: Pitch 180°
13: Roll 180°, Yaw 225°
14: Roll 180°, Yaw 270°
15: Roll 180°, Yaw 315°
16: Roll 90°
17: Roll 90°, Yaw 45°
18: Roll 90°, Yaw 90°
19: Roll 90°, Yaw 135°
20: Roll 270°
21: Roll 270°, Yaw 45°
22: Roll 270°, Yaw 90°
23: Roll 270°, Yaw 135°
24: Pitch 90°
25: Pitch 270°
26: Pitch 180°, Yaw 90°
27: Pitch 180°, Yaw 270°
28: Roll 90°, Pitch 90°
29: Roll 180°, Pitch 90°
30: Roll 270°, Pitch 90°
31: Roll 90°, Pitch 180°
32: Roll 270°, Pitch 180°
33: Roll 90°, Pitch 270°
34: Roll 180°, Pitch 270°
35: Roll 270°, Pitch 270°
36: Roll 90°, Pitch 180°, Yaw 90°
37: Roll 90°, Yaw 270°
38: Roll 90°, Pitch 68°, Yaw 293°
39: Pitch 315°
40: Roll 90°, Pitch 315°
min: -1
max: 40
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
@ -167,6 +92,7 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
# Barometer calibration
CAL_BARO${i}_ID:
description:
@ -178,23 +104,6 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_BARO${i}_PRIO:
description:
short: Barometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_BARO${i}_OFF:
description:
short: Barometer ${i} offset
@ -207,6 +116,7 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
# Gyroscope calibration
CAL_GYRO${i}_ID:
description:
@ -218,79 +128,6 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_GYRO${i}_PRIO:
description:
short: Gyroscope ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_GYRO${i}_ROT:
description:
short: Gyroscope ${i} rotation relative to airframe
long: |
An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
category: System
type: enum
values:
-1: Internal
0: No rotation
1: Yaw 45°
2: Yaw 90°
3: Yaw 135°
4: Yaw 180°
5: Yaw 225°
6: Yaw 270°
7: Yaw 315°
8: Roll 180°
9: Roll 180°, Yaw 45°
10: Roll 180°, Yaw 90°
11: Roll 180°, Yaw 135°
12: Pitch 180°
13: Roll 180°, Yaw 225°
14: Roll 180°, Yaw 270°
15: Roll 180°, Yaw 315°
16: Roll 90°
17: Roll 90°, Yaw 45°
18: Roll 90°, Yaw 90°
19: Roll 90°, Yaw 135°
20: Roll 270°
21: Roll 270°, Yaw 45°
22: Roll 270°, Yaw 90°
23: Roll 270°, Yaw 135°
24: Pitch 90°
25: Pitch 270°
26: Pitch 180°, Yaw 90°
27: Pitch 180°, Yaw 270°
28: Roll 90°, Pitch 90°
29: Roll 180°, Pitch 90°
30: Roll 270°, Pitch 90°
31: Roll 90°, Pitch 180°
32: Roll 270°, Pitch 180°
33: Roll 90°, Pitch 270°
34: Roll 180°, Pitch 270°
35: Roll 270°, Pitch 270°
36: Roll 90°, Pitch 180°, Yaw 90°
37: Roll 90°, Yaw 270°
38: Roll 90°, Pitch 68°, Yaw 293°
39: Pitch 315°
40: Roll 90°, Pitch 315°
min: -1
max: 40
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_GYRO${i}_XOFF:
description:
short: Gyroscope ${i} X-axis offset
@ -327,6 +164,7 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
# Magnetometer calibration
CAL_MAG${i}_ID:
description:
@ -338,120 +176,6 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_PRIO:
description:
short: Magnetometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_ROT:
description:
short: Magnetometer ${i} rotation relative to airframe
long: |
An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW.
category: System
type: enum
values:
-1: Internal
0: No rotation
1: Yaw 45°
2: Yaw 90°
3: Yaw 135°
4: Yaw 180°
5: Yaw 225°
6: Yaw 270°
7: Yaw 315°
8: Roll 180°
9: Roll 180°, Yaw 45°
10: Roll 180°, Yaw 90°
11: Roll 180°, Yaw 135°
12: Pitch 180°
13: Roll 180°, Yaw 225°
14: Roll 180°, Yaw 270°
15: Roll 180°, Yaw 315°
16: Roll 90°
17: Roll 90°, Yaw 45°
18: Roll 90°, Yaw 90°
19: Roll 90°, Yaw 135°
20: Roll 270°
21: Roll 270°, Yaw 45°
22: Roll 270°, Yaw 90°
23: Roll 270°, Yaw 135°
24: Pitch 90°
25: Pitch 270°
26: Pitch 180°, Yaw 90°
27: Pitch 180°, Yaw 270°
28: Roll 90°, Pitch 90°
29: Roll 180°, Pitch 90°
30: Roll 270°, Pitch 90°
31: Roll 90°, Pitch 180°
32: Roll 270°, Pitch 180°
33: Roll 90°, Pitch 270°
34: Roll 180°, Pitch 270°
35: Roll 270°, Pitch 270°
36: Roll 90°, Pitch 180°, Yaw 90°
37: Roll 90°, Yaw 270°
38: Roll 90°, Pitch 68°, Yaw 293°
39: Pitch 315°
40: Roll 90°, Pitch 315°
100: Custom Euler Angle
min: -1
max: 100
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_ROLL:
description:
short: Magnetometer ${i} Custom Euler Roll Angle
long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
category: System
type: float
default: 0.0
min: -180
max: 180
unit: deg
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_PITCH:
description:
short: Magnetometer ${i} Custom Euler Pitch Angle
long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
category: System
type: float
default: 0.0
min: -180
max: 180
unit: deg
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_YAW:
description:
short: Magnetometer ${i} Custom Euler Yaw Angle
long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
category: System
type: float
default: 0.0
min: -180
max: 180
unit: deg
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_XOFF:
description:
short: Magnetometer ${i} X-axis offset
@ -609,3 +333,399 @@ parameters:
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
- group: Sensor Configuration
definitions:
# Accelerometer configuration
SENS_ACC${i}_ID:
description:
short: Accelerometer ${i} configuration device ID
long: Device ID of the accelerometer this configuration applies to.
category: System
type: int32
default: 0
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_ACC${i}_PRIO:
description:
short: Accelerometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_ACC${i}_ROT:
description:
short: Accelerometer ${i} rotation relative to airframe
long: |
An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
category: System
type: enum
values:
-1: Internal
0: No rotation
1: Yaw 45°
2: Yaw 90°
3: Yaw 135°
4: Yaw 180°
5: Yaw 225°
6: Yaw 270°
7: Yaw 315°
8: Roll 180°
9: Roll 180°, Yaw 45°
10: Roll 180°, Yaw 90°
11: Roll 180°, Yaw 135°
12: Pitch 180°
13: Roll 180°, Yaw 225°
14: Roll 180°, Yaw 270°
15: Roll 180°, Yaw 315°
16: Roll 90°
17: Roll 90°, Yaw 45°
18: Roll 90°, Yaw 90°
19: Roll 90°, Yaw 135°
20: Roll 270°
21: Roll 270°, Yaw 45°
22: Roll 270°, Yaw 90°
23: Roll 270°, Yaw 135°
24: Pitch 90°
25: Pitch 270°
26: Pitch 180°, Yaw 90°
27: Pitch 180°, Yaw 270°
28: Roll 90°, Pitch 90°
29: Roll 180°, Pitch 90°
30: Roll 270°, Pitch 90°
31: Roll 90°, Pitch 180°
32: Roll 270°, Pitch 180°
33: Roll 90°, Pitch 270°
34: Roll 180°, Pitch 270°
35: Roll 270°, Pitch 270°
36: Roll 90°, Pitch 180°, Yaw 90°
37: Roll 90°, Yaw 270°
38: Roll 90°, Pitch 68°, Yaw 293°
39: Pitch 315°
40: Roll 90°, Pitch 315°
min: -1
max: 40
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
# Barometer configuration
SENS_BARO${i}_ID:
description:
short: Barometer ${i} configuration device ID
long: Device ID of the barometer this configuration applies to.
category: System
type: int32
default: 0
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_BARO${i}_PRIO:
description:
short: Barometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
# GNSS configuration
SENS_GNSS${i}_ID:
description:
short: GNSS ${i} configuration device ID
long: Device ID of the GNSS this configuration applies to.
category: System
type: int32
default: 0
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_GNSS${i}_PRIO:
description:
short: GNSS ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_GNSS${i}_XPOS:
description:
short: GNSS ${i} antenna X position
long: |
X position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
category: System
type: float
default: 0.0
unit: m
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_GNSS${i}_YPOS:
description:
short: GNSS ${i} antenna Y position
long: |
Y position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
category: System
type: float
default: 0.0
unit: m
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_GNSS${i}_ZPOS:
description:
short: GNSS ${i} antenna Z position
long: |
Z position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
category: System
type: float
default: 0.0
unit: m
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
# Gyroscope configuration
SENS_GYRO${i}_ID:
description:
short: Gyroscope ${i} configuration device ID
long: Device ID of the gyroscope this configuration applies to.
category: System
type: int32
default: 0
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_GYRO${i}_PRIO:
description:
short: Gyroscope ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_GYRO${i}_ROT:
description:
short: Gyroscope ${i} rotation relative to airframe
long: |
An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
category: System
type: enum
values:
-1: Internal
0: No rotation
1: Yaw 45°
2: Yaw 90°
3: Yaw 135°
4: Yaw 180°
5: Yaw 225°
6: Yaw 270°
7: Yaw 315°
8: Roll 180°
9: Roll 180°, Yaw 45°
10: Roll 180°, Yaw 90°
11: Roll 180°, Yaw 135°
12: Pitch 180°
13: Roll 180°, Yaw 225°
14: Roll 180°, Yaw 270°
15: Roll 180°, Yaw 315°
16: Roll 90°
17: Roll 90°, Yaw 45°
18: Roll 90°, Yaw 90°
19: Roll 90°, Yaw 135°
20: Roll 270°
21: Roll 270°, Yaw 45°
22: Roll 270°, Yaw 90°
23: Roll 270°, Yaw 135°
24: Pitch 90°
25: Pitch 270°
26: Pitch 180°, Yaw 90°
27: Pitch 180°, Yaw 270°
28: Roll 90°, Pitch 90°
29: Roll 180°, Pitch 90°
30: Roll 270°, Pitch 90°
31: Roll 90°, Pitch 180°
32: Roll 270°, Pitch 180°
33: Roll 90°, Pitch 270°
34: Roll 180°, Pitch 270°
35: Roll 270°, Pitch 270°
36: Roll 90°, Pitch 180°, Yaw 90°
37: Roll 90°, Yaw 270°
38: Roll 90°, Pitch 68°, Yaw 293°
39: Pitch 315°
40: Roll 90°, Pitch 315°
min: -1
max: 40
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
# Magnetometer configuration
SENS_MAG${i}_ID:
description:
short: Magnetometer ${i} configuration device ID
long: Device ID of the magnetometer this configuration applies to.
category: System
type: int32
default: 0
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_MAG${i}_PRIO:
description:
short: Magnetometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_MAG${i}_ROT:
description:
short: Magnetometer ${i} rotation relative to airframe
long: |
An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW.
category: System
type: enum
values:
-1: Internal
0: No rotation
1: Yaw 45°
2: Yaw 90°
3: Yaw 135°
4: Yaw 180°
5: Yaw 225°
6: Yaw 270°
7: Yaw 315°
8: Roll 180°
9: Roll 180°, Yaw 45°
10: Roll 180°, Yaw 90°
11: Roll 180°, Yaw 135°
12: Pitch 180°
13: Roll 180°, Yaw 225°
14: Roll 180°, Yaw 270°
15: Roll 180°, Yaw 315°
16: Roll 90°
17: Roll 90°, Yaw 45°
18: Roll 90°, Yaw 90°
19: Roll 90°, Yaw 135°
20: Roll 270°
21: Roll 270°, Yaw 45°
22: Roll 270°, Yaw 90°
23: Roll 270°, Yaw 135°
24: Pitch 90°
25: Pitch 270°
26: Pitch 180°, Yaw 90°
27: Pitch 180°, Yaw 270°
28: Roll 90°, Pitch 90°
29: Roll 180°, Pitch 90°
30: Roll 270°, Pitch 90°
31: Roll 90°, Pitch 180°
32: Roll 270°, Pitch 180°
33: Roll 90°, Pitch 270°
34: Roll 180°, Pitch 270°
35: Roll 270°, Pitch 270°
36: Roll 90°, Pitch 180°, Yaw 90°
37: Roll 90°, Yaw 270°
38: Roll 90°, Pitch 68°, Yaw 293°
39: Pitch 315°
40: Roll 90°, Pitch 315°
100: Custom Euler Angle
min: -1
max: 100
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_MAG${i}_ROLL:
description:
short: Magnetometer ${i} Custom Euler Roll Angle
long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
category: System
type: float
default: 0.0
min: -180
max: 180
unit: deg
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_MAG${i}_PITCH:
description:
short: Magnetometer ${i} Custom Euler Pitch Angle
long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
category: System
type: float
default: 0.0
min: -180
max: 180
unit: deg
num_instances: *max_num_sensor_instances
instance_start: 0
SENS_MAG${i}_YAW:
description:
short: Magnetometer ${i} Custom Euler Yaw Angle
long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
category: System
type: float
default: 0.0
min: -180
max: 180
unit: deg
num_instances: *max_num_sensor_instances
instance_start: 0

View File

@ -43,6 +43,8 @@
#include "sensors.hpp"
#include <lib/sensor/configuration/Utilities.hpp>
Sensors::Sensors(bool hil_enabled) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
@ -184,58 +186,96 @@ int Sensors::parameters_update()
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
// sensor_accel
{
const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i);
const uint32_t device_id_accel = sensor::calibration::GetCalibrationParamInt32("ACC", "ID", i);
if (device_id_accel != 0) {
calibration::Accelerometer accel_cal(device_id_accel);
sensor::calibration::Accelerometer accel_cal(device_id_accel);
}
uORB::SubscriptionData<sensor_accel_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();
}
uint32_t device_id_acc_cfg = sensor::configuration::GetConfigurationParamInt32("ACC", "ID", i);
if (device_id_acc_cfg != 0) {
sensor::configuration::Accelerometer mag_cal(device_id_acc_cfg);
}
}
// sensor_gyro
{
const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i);
const uint32_t device_id_gyro = sensor::calibration::GetCalibrationParamInt32("GYRO", "ID", i);
if (device_id_gyro != 0) {
calibration::Gyroscope gyro_cal(device_id_gyro);
sensor::calibration::Gyroscope gyro_cal(device_id_gyro);
}
uORB::SubscriptionData<sensor_gyro_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();
}
uint32_t device_id_gyro_cfg = sensor::configuration::GetConfigurationParamInt32("GYRO", "ID", i);
if (device_id_gyro_cfg != 0) {
sensor::configuration::Accelerometer mag_cal(device_id_gyro_cfg);
}
}
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
// sensor_mag
{
uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i);
uint32_t device_id_mag_cal = sensor::calibration::GetCalibrationParamInt32("MAG", "ID", i);
if (device_id_mag != 0) {
calibration::Magnetometer mag_cal(device_id_mag);
if (device_id_mag_cal != 0) {
sensor::calibration::Magnetometer mag_cal(device_id_mag_cal);
}
uORB::SubscriptionData<sensor_mag_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();
}
uint32_t device_id_mag_cfg = sensor::configuration::GetConfigurationParamInt32("MAG", "ID", i);
if (device_id_mag_cfg != 0) {
sensor::configuration::Magnetometer mag_cal(device_id_mag_cfg);
}
}
#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
// sensor_gps
{
uint32_t device_id_gnss = sensor::configuration::GetConfigurationParamInt32("GNSS", "ID", i);
if (device_id_gnss != 0) {
sensor::configuration::GNSS gnss_cal(device_id_gnss);
}
uORB::SubscriptionData<sensor_gps_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)
@ -713,6 +753,15 @@ int Sensors::print_status()
_airspeed_validator.print();
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
if (_vehicle_gps_position) {
PX4_INFO_RAW("\n");
_vehicle_gps_position->PrintStatus();
}
#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
if (_vehicle_optical_flow) {
@ -732,15 +781,6 @@ int Sensors::print_status()
_vehicle_angular_velocity.PrintStatus();
#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
if (_vehicle_gps_position) {
PX4_INFO_RAW("\n");
_vehicle_gps_position->PrintStatus();
}
#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
PX4_INFO_RAW("\n");
for (auto &i : _vehicle_imu_list) {

View File

@ -41,7 +41,7 @@
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/calibration/Utilities.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
@ -80,7 +80,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
@ -263,4 +263,4 @@ private:
#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
)
};
};

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{};

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>
@ -96,7 +96,7 @@ private:
{this, ORB_ID(sensor_baro), 3},
};
calibration::Barometer _calibration[MAX_SENSOR_COUNT];
sensor::calibration::Barometer _calibration[MAX_SENSOR_COUNT];
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};

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>
@ -113,7 +113,7 @@ private:
uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
calibration::Gyroscope _calibration{};
sensor::calibration::Gyroscope _calibration{};
matrix::Vector3f _bias{};

View File

@ -91,11 +91,29 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
}
}
for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) {
_configuration[instance].ParametersUpdate();
}
_gps_blending.setBlendingUseSpeedAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC);
_gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC);
_gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC);
_gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get());
_gps_blending.setPrimaryInstance(_param_sens_gps_prime.get());
// TODO: select highest priority
int primary_instance = -1;
uint8_t highest_priority = 0;
for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) {
if (_configuration[instance].enabled() && _configuration[instance].priority() > highest_priority) {
primary_instance = instance;
highest_priority = _configuration[instance].priority();
}
}
if (primary_instance >= 0) {
//_gps_blending.setPrimaryInstance(primary_instance);
}
}
}
@ -116,11 +134,14 @@ void VehicleGPSPosition::Run()
if (gps_updated) {
any_gps_updated = true;
_sensor_gps_sub[i].copy(&gps_data);
_gps_blending.setGpsData(gps_data, i);
if (_sensor_gps_sub[i].copy(&gps_data)) {
_configuration[i].set_device_id(gps_data.device_id);
if (!_sensor_gps_sub[i].registered()) {
_sensor_gps_sub[i].registerCallback();
_gps_blending.setGpsData(gps_data, i);
if (!_sensor_gps_sub[i].registered()) {
_sensor_gps_sub[i].registerCallback();
}
}
}
}
@ -131,12 +152,29 @@ void VehicleGPSPosition::Run()
if (_gps_blending.isNewOutputDataAvailable()) {
sensor_gps_s gps_output{_gps_blending.getOutputGpsData()};
const int selected_instance = _gps_blending.getSelectedGps();
// clear device_id if blending
if (_gps_blending.getSelectedGps() == GpsBlending::GPS_MAX_RECEIVERS_BLEND) {
if (selected_instance == GpsBlending::GPS_MAX_RECEIVERS_BLEND) {
gps_output.device_id = 0;
gps_output.position_offset[0] = 0.f;
gps_output.position_offset[1] = 0.f;
gps_output.position_offset[2] = 0.f;
} else if (selected_instance >= 0 && selected_instance < GpsBlending::GPS_MAX_RECEIVERS_BLEND) {
_configuration[selected_instance].position().copyTo(gps_output.position_offset);
}
_vehicle_gps_position_pub.publish(gps_output);
// populate initial SENS_GNSSx configuration slots if necessary
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if ((_configuration[i].device_id() != 0) && (!_configuration[i].configured())) {
_configuration[i].ParametersSave(i);
}
}
}
}
@ -148,6 +186,12 @@ void VehicleGPSPosition::Run()
void VehicleGPSPosition::PrintStatus()
{
PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_blending.getSelectedGps());
for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_configuration[i].device_id() != 0) {
_configuration[i].PrintStatus();
}
}
}
}; // namespace sensors

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>
@ -92,10 +93,11 @@ private:
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

View File

@ -54,6 +54,7 @@ void GpsBlending::update(uint64_t hrt_now_us)
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
if (_gps_state[i].fix_type > best_fix) {
best_fix = _gps_state[i].fix_type;
gps_select_index = i;
}
}
@ -73,15 +74,8 @@ void GpsBlending::update(uint64_t hrt_now_us)
_hgt_offset_m[i] = 0.0;
}
// Only use a secondary instance if the fallback is allowed
if ((_primary_instance > -1)
&& (gps_select_index != _primary_instance)
&& !_fallback_allowed) {
gps_select_index = _primary_instance;
}
_selected_gps = gps_select_index;
_is_new_output_data_available = _gps_updated[gps_select_index];
_is_new_output_data_available = _gps_updated[gps_select_index];
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
_gps_updated[gps_select_index] = false;
@ -109,7 +103,7 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
float raw_dt = 0.f;
if (_gps_state[i].timestamp > _time_prev_us[i]) {
raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]);
raw_dt = 1e-6f * ((int64_t)_gps_state[i].timestamp - (int64_t)_time_prev_us[i]);
}
float present_dt = 0.f;
@ -372,6 +366,14 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
gps_blended_state.vel_e_m_s = 0;
gps_blended_state.vel_d_m_s = 0;
// these will be a sum of weighted values and must start from zero.
gps_blended_state.timestamp = 0;
gps_blended_state.timestamp_sample = 0;
gps_blended_state.vel_m_s = 0;
gps_blended_state.vel_n_m_s = 0;
gps_blended_state.vel_e_m_s = 0;
gps_blended_state.vel_d_m_s = 0;
// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
// Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers
@ -429,11 +431,6 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
gps_blended_state.vel_ned_valid = true;
}
}
// TODO read parameters for individual GPS antenna positions and blend
// Vector3f temp_antenna_offset = _antenna_offset[i];
// temp_antenna_offset *= blend_weights[i];
// _blended_antenna_offset += temp_antenna_offset;
}
/*
@ -500,14 +497,15 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
{
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
// A weighting of 0 will make the offset adjust the slowest, a weighting of 1 will make it adjust with zero filtering
float alpha[GPS_MAX_RECEIVERS_BLEND] {};
float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f);
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
if (_gps_state[i].timestamp > _time_prev_us[i]) {
// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
0.0f, 1.0f);
alpha[i] = constrain(omega_lpf * 1e-6f * (float)((int64_t)_gps_state[i].timestamp - (int64_t)_time_prev_us[i]), 0.f,
1.f);
_time_prev_us[i] = _gps_state[i].timestamp;
}

View File

@ -72,6 +72,7 @@ public:
_gps_updated[instance] = true;
}
}
void setBlendingUseSpeedAccuracy(bool enabled) { _blend_use_spd_acc = enabled; }
void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; }
void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; }
@ -82,6 +83,7 @@ public:
bool isNewOutputDataAvailable() const { return _is_new_output_data_available; }
int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; }
const sensor_gps_s &getOutputGpsData() const
{
if (_selected_gps < GPS_MAX_RECEIVERS_BLEND) {
@ -91,6 +93,7 @@ public:
return _gps_blended_state;
}
}
int getSelectedGps() const { return _selected_gps; }
private:
@ -122,11 +125,12 @@ private:
sensor_gps_s _gps_state[GPS_MAX_RECEIVERS_BLEND] {}; ///< internal state data for the physical GPS
sensor_gps_s _gps_blended_state {};
bool _gps_updated[GPS_MAX_RECEIVERS_BLEND] {};
int _selected_gps{0};
int _np_gps_suitable_for_blending{0};
int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id
bool _fallback_allowed{false};
int _primary_instance{0}; ///< if -1, there is no primary instance and the best receiver is used // TODO: use device_id
bool _fallback_allowed{true};
bool _is_new_output_data_available{false};

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

View File

@ -46,7 +46,7 @@
* @bit 1 use hpos accuracy
* @bit 2 use vpos accuracy
*/
PARAM_DEFINE_INT32(SENS_GPS_MASK, 7);
PARAM_DEFINE_INT32(SENS_GPS_MASK, 0);
/**
* Multi GPS Blending Time Constant
@ -61,22 +61,3 @@ PARAM_DEFINE_INT32(SENS_GPS_MASK, 7);
* @decimal 1
*/
PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
/**
* Multi GPS primary instance
*
* When no blending is active, this defines the preferred GPS receiver instance.
* The GPS selection logic waits until the primary receiver is available to
* send data to the EKF even if a secondary instance is already available.
* The secondary instance is then only used if the primary one times out.
*
* To have an equal priority of all the instances, set this parameter to -1 and
* the best receiver will be used.
*
* This parameter has no effect if blending is active.
*
* @group Sensors
* @min -1
* @max 1
*/
PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);

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>

View File

@ -40,8 +40,8 @@
#include <lib/mathlib/math/WelfordMeanVector.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>
@ -111,8 +111,8 @@ private:
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
calibration::Accelerometer _accel_calibration{};
calibration::Gyroscope _gyro_calibration{};
sensor::calibration::Accelerometer _accel_calibration{};
sensor::calibration::Gyroscope _gyro_calibration{};
sensors::Integrator _accel_integrator{};
sensors::IntegratorConing _gyro_integrator{};

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

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 {

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")};

View File

@ -39,7 +39,7 @@
#include "voted_sensors_update.h"
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/sensor/configuration/Utilities.hpp>
#include <lib/geo/geo.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
@ -88,13 +88,14 @@ void VotedSensorsUpdate::parametersUpdate()
&& (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) {
// find corresponding configured accel priority
int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);
int8_t accel_conf_index = sensor::configuration::FindCurrentConfigurationIndex("ACC", imu.get().accel_device_id);
if (accel_cal_index >= 0) {
if (accel_conf_index >= 0) {
// found matching CAL_ACCx_PRIO
int32_t accel_priority_old = _accel.priority_configured[uorb_index];
_accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index);
_accel.priority_configured[uorb_index] = sensor::configuration::GetConfigurationParamInt32("ACC", "PRIO",
accel_conf_index);
if (accel_priority_old != _accel.priority_configured[uorb_index]) {
if (_accel.priority_configured[uorb_index] == 0) {
@ -111,13 +112,14 @@ void VotedSensorsUpdate::parametersUpdate()
}
// find corresponding configured gyro priority
int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id);
int8_t gyro_conf_index = sensor::configuration::FindCurrentConfigurationIndex("GYRO", imu.get().gyro_device_id);
if (gyro_cal_index >= 0) {
if (gyro_conf_index >= 0) {
// found matching CAL_GYROx_PRIO
int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index);
_gyro.priority_configured[uorb_index] = sensor::configuration::GetConfigurationParamInt32("GYRO", "PRIO",
gyro_conf_index);
if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
if (_gyro.priority_configured[uorb_index] == 0) {