From 36c6e36b9dc67d8ce00153457c671f1de2258a00 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 27 Jan 2020 18:18:04 -0500 Subject: [PATCH] sensors: refactor common corrections and rotation code --- src/modules/sensors/CMakeLists.txt | 2 + .../sensors/sensor_corrections/CMakeLists.txt | 37 ++++ .../sensor_corrections/SensorCorrections.cpp | 170 ++++++++++++++++++ .../sensor_corrections/SensorCorrections.hpp | 99 ++++++++++ src/modules/sensors/sensors.cpp | 4 + .../vehicle_acceleration/CMakeLists.txt | 2 +- .../VehicleAcceleration.cpp | 80 ++------- .../VehicleAcceleration.hpp | 32 ++-- .../vehicle_angular_velocity/CMakeLists.txt | 2 +- .../VehicleAngularVelocity.cpp | 81 ++------- .../VehicleAngularVelocity.hpp | 42 ++--- .../sensors/vehicle_imu/CMakeLists.txt | 2 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 127 ++----------- .../sensors/vehicle_imu/VehicleIMU.hpp | 45 ++--- 14 files changed, 403 insertions(+), 322 deletions(-) create mode 100644 src/modules/sensors/sensor_corrections/CMakeLists.txt create mode 100644 src/modules/sensors/sensor_corrections/SensorCorrections.cpp create mode 100644 src/modules/sensors/sensor_corrections/SensorCorrections.hpp diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index a9b873ae4f..177af3dc9f 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -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) diff --git a/src/modules/sensors/sensor_corrections/CMakeLists.txt b/src/modules/sensors/sensor_corrections/CMakeLists.txt new file mode 100644 index 0000000000..932e2029c5 --- /dev/null +++ b/src/modules/sensors/sensor_corrections/CMakeLists.txt @@ -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 +) diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.cpp b/src/modules/sensors/sensor_corrections/SensorCorrections.cpp new file mode 100644 index 0000000000..f5b51d523b --- /dev/null +++ b/src/modules/sensors/sensor_corrections/SensorCorrections.cpp @@ -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 diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp b/src/modules/sensors/sensor_corrections/SensorCorrections.hpp new file mode 100644 index 0000000000..75881e72f8 --- /dev/null +++ b/src/modules/sensors/sensor_corrections/SensorCorrections.hpp @@ -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 +#include +#include +#include +#include +#include +#include + +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) _param_sens_board_rot, + + (ParamFloat) _param_sens_board_x_off, + (ParamFloat) _param_sens_board_y_off, + (ParamFloat) _param_sens_board_z_off + ) +}; + +} // namespace sensors diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2001a7a775..2f105968cd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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(); } } diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt index 3867dfeab0..ae1ec98d94 100644 --- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -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) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 00c27a311d..7df1ae4e75 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -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 diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 4575384cdb..4617e72a6a 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -33,7 +33,8 @@ #pragma once -#include +#include + #include #include #include @@ -47,10 +48,12 @@ #include #include #include -#include #include #include +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) _param_imu_accel_cutoff, - - (ParamInt) _param_sens_board_rot, - - (ParamFloat) _param_sens_board_x_off, - (ParamFloat) _param_sens_board_y_off, - (ParamFloat) _param_sens_board_z_off - ) - uORB::Publication _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) _param_imu_accel_cutoff + ) }; + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt index df655ded52..8f63f991d5 100644 --- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -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) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 32dd9de0c5..1cab9af024 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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 diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 429b673f99..b1669db492 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -33,7 +33,8 @@ #pragma once -#include +#include + #include #include #include @@ -47,12 +48,14 @@ #include #include #include -#include #include #include #include #include +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) _param_imu_gyro_cutoff, - (ParamFloat) _param_imu_gyro_nf_freq, - (ParamFloat) _param_imu_gyro_nf_bw, - (ParamInt) _param_imu_gyro_rate_max, - - (ParamFloat) _param_imu_dgyro_cutoff, - - (ParamInt) _param_sens_board_rot, - - (ParamFloat) _param_sens_board_x_off, - (ParamFloat) _param_sens_board_y_off, - (ParamFloat) _param_sens_board_z_off - ) - uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; uORB::Publication _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) _param_imu_gyro_cutoff, + (ParamFloat) _param_imu_gyro_nf_freq, + (ParamFloat) _param_imu_gyro_nf_bw, + (ParamInt) _param_imu_gyro_rate_max, + + (ParamFloat) _param_imu_dgyro_cutoff + ) }; + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 36a6edaac6..eafaefa874 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -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) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 743ea310d0..ddad06390f 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -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 diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 528fd4ac81..cd9ec741fb 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -33,23 +33,25 @@ #pragma once -#include -#include +#include + #include #include -#include #include #include +#include #include #include #include #include #include #include -#include #include #include +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) _param_sens_board_rot, - - (ParamFloat) _param_sens_board_x_off, - (ParamFloat) _param_sens_board_y_off, - (ParamFloat) _param_sens_board_z_off - ) - uORB::PublicationMulti _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