dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors: refactor common corrections and rotation code

This commit is contained in:
Daniel Agar 2020-01-27 18:18:04 -05:00
parent ca96b6b0ea
commit 36c6e36b9d
14 changed files with 403 additions and 322 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,6 +31,8 @@
#
############################################################################
add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_velocity, imu}
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_imu)

37
src/modules/sensors/sensor_corrections/CMakeLists.txt Normal file
View File

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

170
src/modules/sensors/sensor_corrections/SensorCorrections.cpp Normal file
View File

@ -0,0 +1,170 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "SensorCorrections.hpp"
using namespace matrix;
using namespace time_literals;
using math::radians;
namespace sensors
{
SensorCorrections::SensorCorrections(ModuleParams *parent, SensorType type) :
ModuleParams(parent),
_type(type)
{
}
void SensorCorrections::set_device_id(uint32_t device_id)
{
if (_device_id != device_id) {
_device_id = device_id;
SensorCorrectionsUpdate(true);
}
}
const char *SensorCorrections::SensorString() const
{
switch (_type) {
case SensorType::Accelerometer:
return "ACC";
case SensorType::Gyroscope:
return "GYRO";
}
return nullptr;
}
void SensorCorrections::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections;
if (_sensor_correction_sub.copy(&corrections)) {
// selected sensor has changed, find updated index
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
switch (_type) {
case SensorType::Accelerometer:
if (corrections.accel_device_ids[i] == _device_id) {
_corrections_selected_instance = i;
}
break;
case SensorType::Gyroscope:
if (corrections.gyro_device_ids[i] == _device_id) {
_corrections_selected_instance = i;
}
break;
}
}
}
switch (_type) {
case SensorType::Accelerometer:
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.accel_offset_0};
_scale = Vector3f{corrections.accel_scale_0};
return;
case 1:
_offset = Vector3f{corrections.accel_offset_1};
_scale = Vector3f{corrections.accel_scale_1};
return;
case 2:
_offset = Vector3f{corrections.accel_offset_2};
_scale = Vector3f{corrections.accel_scale_2};
return;
}
break;
case SensorType::Gyroscope:
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.gyro_offset_0};
_scale = Vector3f{corrections.gyro_scale_0};
return;
case 1:
_offset = Vector3f{corrections.gyro_offset_1};
_scale = Vector3f{corrections.gyro_scale_1};
return;
case 2:
_offset = Vector3f{corrections.gyro_offset_2};
_scale = Vector3f{corrections.gyro_scale_2};
return;
}
break;
}
}
}
}
void SensorCorrections::ParametersUpdate()
{
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
// get transformation matrix from sensor/board to body frame
_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
}
void SensorCorrections::PrintStatus()
{
if (_offset.norm() > 0.f) {
PX4_INFO("%s %d offset: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_offset(0), (double)_offset(1),
(double)_offset(2));
}
if (fabsf(_scale.norm_squared() - 3.f) > FLT_EPSILON) {
PX4_INFO("%s %d scale: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_scale(0), (double)_scale(1),
(double)_scale(2));
}
}
} // namespace sensors

99
src/modules/sensors/sensor_corrections/SensorCorrections.hpp Normal file
View File

@ -0,0 +1,99 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/conversion/rotation.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
namespace sensors
{
class SensorCorrections : public ModuleParams
{
public:
enum class SensorType : uint8_t {
Accelerometer,
Gyroscope,
};
SensorCorrections() = delete;
SensorCorrections(ModuleParams *parent, SensorType type);
~SensorCorrections() override = default;
void PrintStatus();
void set_device_id(uint32_t device_id);
uint32_t get_device_id() const { return _device_id; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
matrix::Vector3f Correct(const matrix::Vector3f &data) const { return _board_rotation * matrix::Vector3f{(data - _offset).emult(_scale)}; }
void ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
private:
static constexpr int MAX_SENSOR_COUNT = 3;
const char *SensorString() const;
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
matrix::Dcmf _board_rotation;
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
uint32_t _device_id{0};
int8_t _corrections_selected_instance{-1};
const SensorType _type;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
};
} // namespace sensors

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

@ -567,11 +567,15 @@ int Sensors::print_status()
PX4_INFO("Airspeed status:");
_airspeed_validator.print();
PX4_INFO_RAW("\n");
_vehicle_acceleration.PrintStatus();
PX4_INFO_RAW("\n");
_vehicle_angular_velocity.PrintStatus();
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
PX4_INFO_RAW("\n");
i->PrintStatus();
}
}

2
src/modules/sensors/vehicle_acceleration/CMakeLists.txt
View File

@ -35,4 +35,4 @@ px4_add_library(vehicle_acceleration
VehicleAcceleration.cpp
VehicleAcceleration.hpp
)
target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)
target_link_libraries(vehicle_acceleration PRIVATE sensor_corrections px4_work_queue)

80
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -37,11 +37,14 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
namespace sensors
{
VehicleAcceleration::VehicleAcceleration() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_corrections(this, SensorCorrections::SensorType::Accelerometer)
{
_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
}
@ -135,46 +138,6 @@ void VehicleAcceleration::SensorBiasUpdate(bool force)
}
}
void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.accel_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.accel_offset_0};
_scale = Vector3f{corrections.accel_scale_0};
break;
case 1:
_offset = Vector3f{corrections.accel_offset_1};
_scale = Vector3f{corrections.accel_scale_1};
break;
case 2:
_offset = Vector3f{corrections.accel_offset_2};
_scale = Vector3f{corrections.accel_scale_2};
break;
default:
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
bool VehicleAcceleration::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
@ -201,11 +164,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
_corrections.set_device_id(report.device_id);
// reset sample rate monitor
_sample_rate_incorrect_count = 0;
@ -234,16 +194,7 @@ void VehicleAcceleration::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
_corrections.ParametersUpdate();
}
}
@ -252,7 +203,7 @@ void VehicleAcceleration::Run()
// update corrections first to set _selected_sensor
bool selection_updated = SensorSelectionUpdate();
SensorCorrectionsUpdate(selection_updated);
_corrections.SensorCorrectionsUpdate(selection_updated);
SensorBiasUpdate(selection_updated);
ParametersUpdate();
@ -281,14 +232,8 @@ void VehicleAcceleration::Run()
sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
if (!sensor_updated) {
// apply offsets and scale
Vector3f accel{(accel_filtered - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// correct for in-run bias errors
accel -= _bias;
const Vector3f accel = _corrections.Correct(accel_filtered) - _bias;
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
@ -308,9 +253,10 @@ void VehicleAcceleration::PrintStatus()
{
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter.get_cutoff_freq());
_corrections.PrintStatus();
}
} // namespace sensors

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

@ -33,7 +33,8 @@
#pragma once
#include <lib/conversion/rotation.h>
#include <sensor_corrections/SensorCorrections.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
@ -47,10 +48,12 @@
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_acceleration.h>
namespace sensors
{
class VehicleAcceleration : public ModuleParams, public px4::WorkItem
{
public:
@ -69,26 +72,14 @@ private:
void CheckFilters();
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
@ -97,13 +88,11 @@ private:
{this, ORB_ID(sensor_accel), 2}
};
SensorCorrections _corrections;
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
matrix::Dcmf _board_rotation;
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
@ -119,5 +108,10 @@ private:
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
)
};
} // namespace sensors

2
src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
View File

@ -35,4 +35,4 @@ px4_add_library(vehicle_angular_velocity
VehicleAngularVelocity.cpp
VehicleAngularVelocity.hpp
)
target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue)
target_link_libraries(vehicle_angular_velocity PRIVATE sensor_corrections px4_work_queue)

81
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -37,11 +37,14 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
namespace sensors
{
VehicleAngularVelocity::VehicleAngularVelocity() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_corrections(this, SensorCorrections::SensorType::Gyroscope)
{
_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
@ -150,46 +153,6 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
}
}
void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.gyro_offset_0};
_scale = Vector3f{corrections.gyro_scale_0};
break;
case 1:
_offset = Vector3f{corrections.gyro_offset_1};
_scale = Vector3f{corrections.gyro_scale_1};
break;
case 2:
_offset = Vector3f{corrections.gyro_offset_2};
_scale = Vector3f{corrections.gyro_scale_2};
break;
default:
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
@ -216,11 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
_corrections.set_device_id(report.device_id);
// reset sample rate monitor
_sample_rate_incorrect_count = 0;
@ -249,16 +209,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
_corrections.ParametersUpdate();
}
}
@ -267,7 +218,7 @@ void VehicleAngularVelocity::Run()
// update corrections first to set _selected_sensor
bool selection_updated = SensorSelectionUpdate();
SensorCorrectionsUpdate(selection_updated);
_corrections.SensorCorrectionsUpdate(selection_updated);
SensorBiasUpdate(selection_updated);
ParametersUpdate();
@ -290,10 +241,10 @@ void VehicleAngularVelocity::Run()
_timestamp_sample_prev = sensor_data.timestamp_sample;
// get the sensor data and correct for thermal errors (apply offsets and scale)
Vector3f angular_velocity_raw{(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z} - _offset).emult(_scale)};
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// rotate corrected measurements from sensor to body frame
angular_velocity_raw = _board_rotation * angular_velocity_raw;
// correct for in-run bias errors
Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
// correct for in-run bias errors
angular_velocity_raw -= _bias;
@ -352,14 +303,10 @@ void VehicleAngularVelocity::PrintStatus()
{
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter_velocity.get_cutoff_freq());
if (_notch_filter_velocity.getNotchFreq() > 0.0f) {
PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter_velocity.getNotchFreq(),
(double)_notch_filter_velocity.getBandwidth());
}
_corrections.PrintStatus();
}
} // namespace sensors

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

@ -33,7 +33,8 @@
#pragma once
#include <lib/conversion/rotation.h>
#include <sensor_corrections/SensorCorrections.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
@ -47,12 +48,14 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
namespace sensors
{
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
{
public:
@ -71,32 +74,15 @@ private:
void CheckFilters();
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff,
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
@ -105,13 +91,11 @@ private:
{this, ORB_ID(sensor_gyro), 2}
};
SensorCorrections _corrections;
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
matrix::Dcmf _board_rotation;
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f};
matrix::Vector3f _angular_velocity_prev{0.f, 0.f, 0.f};
@ -134,5 +118,15 @@ private:
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
)
};
} // namespace sensors

2
src/modules/sensors/vehicle_imu/CMakeLists.txt
View File

@ -35,4 +35,4 @@ px4_add_library(vehicle_imu
VehicleIMU.cpp
VehicleIMU.hpp
)
target_link_libraries(vehicle_imu PRIVATE px4_work_queue)
target_link_libraries(vehicle_imu PRIVATE sensor_corrections px4_work_queue)

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

@ -37,20 +37,23 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
namespace sensors
{
VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index),
_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index)
_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index),
_accel_corrections(this, SensorCorrections::SensorType::Accelerometer),
_gyro_corrections(this, SensorCorrections::SensorType::Gyroscope)
{
}
VehicleIMU::~VehicleIMU()
{
Stop();
delete _name;
}
bool VehicleIMU::Start()
@ -70,81 +73,6 @@ void VehicleIMU::Stop()
_sensor_gyro_integrated_sub.unregisterCallback();
}
void VehicleIMU::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
if (_sensor_correction_sub.copy(&corrections)) {
// accel
if (_accel_device_id > 0) {
if ((_corrections_selected_accel_instance < 0) || force) {
_corrections_selected_accel_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.accel_device_ids[i] == _accel_device_id) {
_corrections_selected_accel_instance = i;
}
}
}
switch (_corrections_selected_accel_instance) {
case 0:
_accel_offset = Vector3f{corrections.accel_offset_0};
_accel_scale = Vector3f{corrections.accel_scale_0};
break;
case 1:
_accel_offset = Vector3f{corrections.accel_offset_1};
_accel_scale = Vector3f{corrections.accel_scale_1};
break;
case 2:
_accel_offset = Vector3f{corrections.accel_offset_2};
_accel_scale = Vector3f{corrections.accel_scale_2};
break;
default:
_accel_offset = Vector3f{0.f, 0.f, 0.f};
_accel_scale = Vector3f{1.f, 1.f, 1.f};
}
}
// gyro
if (_gyro_device_id > 0) {
if ((_corrections_selected_gyro_instance < 0) || force) {
_corrections_selected_gyro_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _gyro_device_id) {
_corrections_selected_gyro_instance = i;
}
}
}
switch (_corrections_selected_gyro_instance) {
case 0:
_gyro_offset = Vector3f{corrections.gyro_offset_0};
_gyro_scale = Vector3f{corrections.gyro_scale_0};
break;
case 1:
_gyro_offset = Vector3f{corrections.gyro_offset_1};
_gyro_scale = Vector3f{corrections.gyro_scale_1};
break;
case 2:
_gyro_offset = Vector3f{corrections.gyro_offset_2};
_gyro_scale = Vector3f{corrections.gyro_scale_2};
break;
default:
_gyro_offset = Vector3f{0.f, 0.f, 0.f};
_gyro_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
}
}
void VehicleIMU::ParametersUpdate(bool force)
{
// Check if parameters have changed
@ -155,16 +83,8 @@ void VehicleIMU::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
_accel_corrections.ParametersUpdate();
_gyro_corrections.ParametersUpdate();
}
}
@ -173,39 +93,26 @@ void VehicleIMU::Run()
if (_sensor_accel_integrated_sub.updated() && _sensor_gyro_integrated_sub.updated()) {
sensor_accel_integrated_s accel;
_sensor_accel_integrated_sub.copy(&accel);
_accel_device_id = accel.device_id;
_accel_corrections.set_device_id(accel.device_id);
sensor_gyro_integrated_s gyro;
_sensor_gyro_integrated_sub.copy(&gyro);
_gyro_device_id = gyro.device_id;
_gyro_corrections.set_device_id(gyro.device_id);
SensorCorrectionsUpdate();
ParametersUpdate();
_accel_corrections.SensorCorrectionsUpdate();
_gyro_corrections.SensorCorrectionsUpdate();
// delta angle: apply offsets, scale, and board rotation
Vector3f delta_angle{gyro.delta_angle};
const float gyro_dt = 1.e-6f * gyro.dt;
// apply offsets
delta_angle = delta_angle - (_gyro_offset * gyro_dt);
// apply scale
delta_angle = delta_angle.emult(_gyro_scale);
// apply board rotation
delta_angle = _board_rotation * delta_angle;
Vector3f delta_angle = _gyro_corrections.Correct(Vector3f{gyro.delta_angle} * gyro_dt) / gyro_dt;
// delta velocity: apply offsets, scale, and board rotation
Vector3f delta_velocity{accel.delta_velocity};
const float accel_dt = 1.e-6f * accel.dt;
// apply offsets
delta_velocity = delta_velocity - (_accel_offset * accel_dt);
// apply scale
delta_velocity = delta_velocity.emult(_accel_scale);
// apply board rotation
delta_velocity = _board_rotation * delta_velocity;
Vector3f delta_velocity = _accel_corrections.Correct(Vector3f{accel.delta_velocity} * accel_dt) / accel_dt;
// publich vehicle_imu
vehicle_imu_s imu;
imu.timestamp_sample = accel.timestamp_sample;
imu.accel_device_id = accel.device_id;
imu.gyro_device_id = gyro.device_id;
@ -224,5 +131,9 @@ void VehicleIMU::Run()
void VehicleIMU::PrintStatus()
{
PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_device_id, _gyro_device_id);
PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_corrections.get_device_id(), _gyro_corrections.get_device_id());
_accel_corrections.PrintStatus();
_gyro_corrections.PrintStatus();
}
} // namespace sensors

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

@ -33,23 +33,25 @@
#pragma once
#include <containers/List.hpp>
#include <lib/conversion/rotation.h>
#include <sensor_corrections/SensorCorrections.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/vehicle_imu.h>
namespace sensors
{
class VehicleIMU : public ModuleParams, public px4::WorkItem
{
public:
@ -64,41 +66,16 @@ public:
void PrintStatus();
private:
void ParametersUpdate(bool force = false);
void Run() override;
void ParametersUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub;
uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub;
matrix::Dcmf _board_rotation;
matrix::Vector3f _accel_offset{0.f, 0.f, 0.f};
matrix::Vector3f _gyro_offset{0.f, 0.f, 0.f};
matrix::Vector3f _accel_scale{1.f, 1.f, 1.f};
matrix::Vector3f _gyro_scale{1.f, 1.f, 1.f};
char *_name{nullptr};
int8_t _corrections_selected_accel_instance{-1};
int8_t _corrections_selected_gyro_instance{-1};
uint32_t _accel_device_id{0};
uint32_t _gyro_device_id{0};
SensorCorrections _accel_corrections;
SensorCorrections _gyro_corrections;
};
} // namespace sensors