From 8ac5f7cfc02d20addba80adf5906cc246b6cd660 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 13 Jan 2023 21:05:50 -0500 Subject: [PATCH] consolidate IMU processing and initial sensor_imu_fifo support --- .ci/Jenkinsfile-hardware | 4 +- ROMFS/cannode/init.d/rcS | 6 - ROMFS/px4fmu_common/init.d-posix/rcS | 5 - ROMFS/px4fmu_common/init.d/rcS | 5 - msg/CMakeLists.txt | 1 + msg/SensorImuFifo.msg | 26 + .../common/uORB/SubscriptionCallback.hpp | 11 + .../SITL/init/test/test_imu_filtering | 2 +- src/lib/sensor_calibration/Accelerometer.hpp | 6 + src/modules/gyro_calibration/CMakeLists.txt | 45 - .../gyro_calibration/GyroCalibration.cpp | 351 --- .../gyro_calibration/GyroCalibration.hpp | 114 - src/modules/gyro_calibration/Kconfig | 12 - src/modules/gyro_calibration/parameters.c | 41 - src/modules/sensors/CMakeLists.txt | 22 +- src/modules/sensors/Integrator.hpp | 5 +- src/modules/sensors/Kconfig | 8 +- src/modules/sensors/sensor_params.c | 12 - src/modules/sensors/sensors.cpp | 311 +-- src/modules/sensors/sensors.hpp | 89 +- .../vehicle_acceleration/CMakeLists.txt | 46 - .../VehicleAcceleration.cpp | 271 -- .../imu_accel_parameters.c | 46 - .../vehicle_angular_velocity/CMakeLists.txt | 50 - .../sensors/vehicle_imu/CMakeLists.txt | 15 +- src/modules/sensors/vehicle_imu/IMU.hpp | 127 + .../vehicle_imu/VehicleAcceleration.cpp | 106 + .../VehicleAcceleration.hpp | 52 +- .../VehicleAngularVelocity.cpp | 657 ++--- .../VehicleAngularVelocity.hpp | 61 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 2208 ++++++++++++----- .../sensors/vehicle_imu/VehicleIMU.hpp | 222 +- .../imu_gyro_parameters.c | 1 - .../sensors/vehicle_imu/imu_parameters.c | 26 + .../VehicleMagnetometer.cpp | 26 + .../sensor_params_mag.c | 0 .../sensor_params_flow.c | 0 src/modules/sensors/voted_sensors_update.cpp | 497 ---- src/modules/sensors/voted_sensors_update.h | 186 -- .../microbench/test_microbench_uorb.cpp | 22 +- 40 files changed, 2343 insertions(+), 3352 deletions(-) create mode 100644 msg/SensorImuFifo.msg delete mode 100644 src/modules/gyro_calibration/CMakeLists.txt delete mode 100644 src/modules/gyro_calibration/GyroCalibration.cpp delete mode 100644 src/modules/gyro_calibration/GyroCalibration.hpp delete mode 100644 src/modules/gyro_calibration/Kconfig delete mode 100644 src/modules/gyro_calibration/parameters.c delete mode 100644 src/modules/sensors/vehicle_acceleration/CMakeLists.txt delete mode 100644 src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp delete mode 100644 src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c delete mode 100644 src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_imu/IMU.hpp create mode 100644 src/modules/sensors/vehicle_imu/VehicleAcceleration.cpp rename src/modules/sensors/{vehicle_acceleration => vehicle_imu}/VehicleAcceleration.hpp (63%) rename src/modules/sensors/{vehicle_angular_velocity => vehicle_imu}/VehicleAngularVelocity.cpp (51%) rename src/modules/sensors/{vehicle_angular_velocity => vehicle_imu}/VehicleAngularVelocity.hpp (75%) rename src/modules/sensors/{vehicle_angular_velocity => vehicle_imu}/imu_gyro_parameters.c (99%) rename src/modules/sensors/{ => vehicle_magnetometer}/sensor_params_mag.c (100%) rename src/modules/sensors/{ => vehicle_optical_flow}/sensor_params_flow.c (100%) delete mode 100644 src/modules/sensors/voted_sensors_update.cpp delete mode 100644 src/modules/sensors/voted_sensors_update.h diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index bb1b8f8a73..574d047235 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -744,7 +744,6 @@ void runTests() { resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_IMU_AUTOCAL" --value "0" || true' // disable during testing sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_MAG_AUTOCAL" --value "0" || true' // disable during testing @@ -853,12 +852,11 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_imu_fifo" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true' diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index 0d9da3a10a..a094fe4142 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -96,12 +96,6 @@ then px4flow start -X & fi -if param compare -s IMU_GYRO_CAL_EN 1 -then - gyro_calibration start -fi - - if param compare -s MBE_ENABLE 1 then # conservative mag bias estimation diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 3b7210d89f..4ad9151ec1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -295,11 +295,6 @@ then gyro_fft start fi -if param compare -s IMU_GYRO_CAL_EN 1 -then - gyro_calibration start -fi - # Payload deliverer module if gripper is enabled if param compare -s PD_GRIPPER_EN 1 then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 04a07cfee9..8d652c231b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -453,11 +453,6 @@ else gyro_fft start fi - if param compare -s IMU_GYRO_CAL_EN 1 - then - gyro_calibration start - fi - # Check for px4flow sensor if param compare -s SENS_EN_PX4FLOW 1 then diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 437c5f86ad..23265d219b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -167,6 +167,7 @@ set(msg_files SensorGyroFft.msg SensorGyroFifo.msg SensorHygrometer.msg + SensorImuFifo.msg SensorMag.msg SensorOpticalFlow.msg SensorPreflightMag.msg diff --git a/msg/SensorImuFifo.msg b/msg/SensorImuFifo.msg new file mode 100644 index 0000000000..e232cd735e --- /dev/null +++ b/msg/SensorImuFifo.msg @@ -0,0 +1,26 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 dt # delta time between samples (microseconds) +uint8 samples # number of valid samples + +float32 accel_scale +float32 gyro_scale + +uint8 FIFO_SIZE = 24 + +int16[24] accel_x # gyro in the FRD board frame X-axis +int16[24] accel_y # gyro in the FRD board frame Y-axis +int16[24] accel_z # gyro in the FRD board frame Z-axis + +int16[24] gyro_x # gyro in the FRD board frame X-axis +int16[24] gyro_y # gyro in the FRD board frame Y-axis +int16[24] gyro_z # gyro in the FRD board frame Z-axis + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 942a9fbe17..16de596212 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -61,6 +61,11 @@ public: { } + SubscriptionCallback(ORB_ID id, uint32_t interval_us = 0, uint8_t instance = 0) : + SubscriptionInterval(id, interval_us, instance) + { + } + virtual ~SubscriptionCallback() { unregisterCallback(); @@ -158,6 +163,12 @@ public: { } + SubscriptionCallbackWorkItem(px4::WorkItem *work_item, ORB_ID id, uint8_t instance = 0) : + SubscriptionCallback(id, 0, instance), // interval 0 + _work_item(work_item) + { + } + virtual ~SubscriptionCallbackWorkItem() = default; void call() override diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering index ed16493f15..e29e85275b 100644 --- a/posix-configs/SITL/init/test/test_imu_filtering +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -53,7 +53,7 @@ logger off sensors status listener sensor_gyro -listener sensor_gyro_fifo +listener sensor_imu_fifo listener sensor_gyro_fft perf diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index ef631a00f1..593cca3c99 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -76,6 +76,7 @@ public: const matrix::Dcmf &rotation() const { return _rotation; } const Rotation &rotation_enum() const { return _rotation_enum; } const matrix::Vector3f &scale() const { return _scale; } + const matrix::Vector3f &thermal_offset() const { return _thermal_offset; } // apply offsets and scale // rotate corrected measurements from sensor to body frame @@ -84,6 +85,11 @@ public: return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)}; } + inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const + { + return (_rotation.I() * corrected_data).edivide(_scale) + _thermal_offset + _offset; + } + // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { diff --git a/src/modules/gyro_calibration/CMakeLists.txt b/src/modules/gyro_calibration/CMakeLists.txt deleted file mode 100644 index cc5381d57b..0000000000 --- a/src/modules/gyro_calibration/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2021-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_module( - MODULE modules__gyro_calibration - MAIN gyro_calibration - COMPILE_FLAGS - #-DDEBUG_BUILD - SRCS - GyroCalibration.cpp - GyroCalibration.hpp - DEPENDS - px4_work_queue - sensor_calibration - ) diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp deleted file mode 100644 index 7bca0b6869..0000000000 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ /dev/null @@ -1,351 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021-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 "GyroCalibration.hpp" - -#include - -using namespace time_literals; -using matrix::Vector3f; - -GyroCalibration::GyroCalibration() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) -{ -} - -GyroCalibration::~GyroCalibration() -{ - perf_free(_loop_interval_perf); - perf_free(_calibration_updated_perf); -} - -bool GyroCalibration::init() -{ - ScheduleOnInterval(INTERVAL_US); - return true; -} - -void GyroCalibration::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - perf_count(_loop_interval_perf); - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - - if (armed != _armed) { - if (!_armed && armed) { - // disarmed -> armed: run at minimal rate until disarmed - ScheduleOnInterval(10_s); - - } else if (_armed && !armed) { - // armed -> disarmed: start running again - ScheduleOnInterval(INTERVAL_US); - } - - _armed = armed; - Reset(); - return; - } - - if (_system_calibrating != vehicle_status.calibration_enabled) { - _system_calibrating = vehicle_status.calibration_enabled; - Reset(); - return; - } - } - } - - if (_armed) { - // do nothing if armed - return; - } - - if (_system_calibrating) { - // do nothing if system is calibrating - Reset(); - return; - } - - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - - if (_parameter_update_sub.copy(¶m_update)) { - // minimize updates immediately following parameter changes - _last_calibration_update = param_update.timestamp; - } - - for (auto &cal : _gyro_calibration) { - cal.ParametersUpdate(); - } - - Reset(); - return; - } - - - // collect raw data from all available gyroscopes (sensor_gyro) - for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - sensor_gyro_s sensor_gyro; - - if (_sensor_gyro_subs[gyro].update(&sensor_gyro)) { - if (PX4_ISFINITE(sensor_gyro.temperature)) { - if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) { - PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro], - (double)sensor_gyro.temperature); - - _temperature[gyro] = sensor_gyro.temperature; - - // reset all on any temperature change - Reset(); - return; - } - - } else { - _temperature[gyro] = NAN; - } - - if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) { - _gyro_calibration[gyro].SensorCorrectionsUpdate(); - const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()}; - _gyro_mean[gyro].update(val); - _gyro_last_update[gyro] = sensor_gyro.timestamp; - - } else { - // setting device id, reset all - _gyro_calibration[gyro].set_device_id(sensor_gyro.device_id); - Reset(); - return; - } - } - - if ((_gyro_last_update[gyro] != 0) && (hrt_elapsed_time(&_gyro_last_update[gyro]) > 100_ms)) { - // remove sensor and reset on any timeout - _gyro_calibration[gyro].set_device_id(0); - _gyro_calibration[gyro].Reset(); - - Reset(); - return; - } - } - - - // check all accelerometers for possible movement - for (int accel = 0; accel < _sensor_accel_subs.size(); accel++) { - sensor_accel_s sensor_accel; - - if (_sensor_accel_subs[accel].update(&sensor_accel)) { - const Vector3f acceleration{sensor_accel.x, sensor_accel.y, sensor_accel.z}; - - if ((acceleration - _acceleration[accel]).longerThan(0.5f)) { - // reset all on any change - PX4_DEBUG("accel %d changed, resetting all %.5f", accel, (double)(acceleration - _acceleration[accel]).length()); - - _acceleration[accel] = acceleration; - Reset(); - return; - - } else if (acceleration.longerThan(CONSTANTS_ONE_G * 1.3f)) { - Reset(); - return; - } - } - } - - // update calibrations for all available gyros - if (hrt_elapsed_time(&_last_calibration_update) > 5_s) { - - // check variance again before saving - for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - if (_gyro_calibration[gyro].device_id() != 0) { - - if (!_gyro_mean[gyro].valid() || _gyro_mean[gyro].variance().longerThan(0.001f)) { - // reset all - PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all", - gyro, (double)_gyro_mean[gyro].variance().length()); - Reset(); - return; - } - } - } - - bool calibration_updated = false; - - for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - - if ((_gyro_calibration[gyro].device_id() != 0) - && _gyro_mean[gyro].valid() && (_gyro_mean[gyro].count() > 100) - ) { - - const Vector3f old_offset{_gyro_calibration[gyro].offset()}; - const Vector3f new_offset{_gyro_mean[gyro].mean()}; - - bool change_exceeds_stddev = false; - bool variance_significantly_better = false; - - const Vector3f variance = _gyro_mean[gyro].variance(); - - for (int i = 0; i < 3; i++) { - // check if offset changed by more than 1 standard deviation - if (sq(new_offset(i) - old_offset(i)) > variance(i)) { - change_exceeds_stddev = true; - } - - // check if current variance is significantly better than previous cal - if (variance(i) < 0.1f * _gyro_cal_variance[gyro](i)) { - variance_significantly_better = true; - } - } - - // update if offset changed by more than 1 standard deviation or currently uncalibrated - if ((change_exceeds_stddev || variance_significantly_better || !_gyro_calibration[gyro].calibrated()) - && _gyro_calibration[gyro].set_offset(new_offset) - ) { - - calibration_updated = true; - - _gyro_cal_variance[gyro] = variance; - - PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] %.1f degC", - gyro, _gyro_calibration[gyro].device_id(), - (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), - (double)new_offset(0), (double)new_offset(1), (double)new_offset(2), - (double)_temperature[gyro]); - - perf_count(_calibration_updated_perf); - } - } - } - - // save all calibrations - if (calibration_updated) { - bool param_save = false; - - for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - if (_gyro_calibration[gyro].device_id() != 0) { - if (_gyro_calibration[gyro].ParametersSave(gyro)) { - param_save = true; - } - } - } - - if (param_save) { - param_notify_changes(); - } - - Reset(); - } - } -} - -int GyroCalibration::task_spawn(int argc, char *argv[]) -{ - GyroCalibration *instance = new GyroCalibration(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int GyroCalibration::print_status() -{ - for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - if (_gyro_calibration[gyro].device_id() != 0) { - PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f degC (count %d)\n", - gyro, _gyro_calibration[gyro].device_id(), - (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), - (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), - (double)_temperature[gyro], _gyro_mean[gyro].count()); - } - } - - perf_print_counter(_loop_interval_perf); - perf_print_counter(_calibration_updated_perf); - return 0; -} - -int GyroCalibration::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int GyroCalibration::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Simple online gyroscope calibration. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("gyro_calibration", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int gyro_calibration_main(int argc, char *argv[]) -{ - return GyroCalibration::main(argc, argv); -} diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp deleted file mode 100644 index 53092855d0..0000000000 --- a/src/modules/gyro_calibration/GyroCalibration.hpp +++ /dev/null @@ -1,114 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -class GyroCalibration : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - GyroCalibration(); - ~GyroCalibration() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - - int print_status() override; - -private: - static constexpr hrt_abstime INTERVAL_US = 20000_us; - static constexpr int MAX_SENSORS = 4; - - void Run() override; - - void Reset() - { - for (int gyro = 0; gyro < MAX_SENSORS; gyro++) { - _gyro_mean[gyro].reset(); - _gyro_last_update[gyro] = 0; - } - - _last_calibration_update = hrt_absolute_time(); - } - - // return the square of two floating point numbers - static constexpr float sq(float var) { return var * var; } - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status}; - - uORB::SubscriptionMultiArray _sensor_accel_subs{ORB_ID::sensor_accel}; - uORB::SubscriptionMultiArray _sensor_gyro_subs{ORB_ID::sensor_gyro}; - - calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; - math::WelfordMeanVector _gyro_mean[MAX_SENSORS] {}; - matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {}; - float _temperature[MAX_SENSORS] {}; - hrt_abstime _gyro_last_update[MAX_SENSORS] {}; - - matrix::Vector3f _acceleration[MAX_SENSORS] {}; - - hrt_abstime _last_calibration_update{0}; - - bool _armed{false}; - bool _system_calibrating{false}; - - perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; - perf_counter_t _calibration_updated_perf{perf_alloc(PC_COUNT, MODULE_NAME": calibration updated")}; -}; diff --git a/src/modules/gyro_calibration/Kconfig b/src/modules/gyro_calibration/Kconfig deleted file mode 100644 index 9eb87312c5..0000000000 --- a/src/modules/gyro_calibration/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_GYRO_CALIBRATION - bool "gyro_calibration" - default n - ---help--- - Enable support for gyro_calibration - -menuconfig USER_GYRO_CALIBRATION - bool "gyro_calibration running as userspace module" - default y - depends on BOARD_PROTECTED && MODULES_GYRO_CALIBRATION - ---help--- - Put gyro_calibration in userspace memory diff --git a/src/modules/gyro_calibration/parameters.c b/src/modules/gyro_calibration/parameters.c deleted file mode 100644 index dee9df9b74..0000000000 --- a/src/modules/gyro_calibration/parameters.c +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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. - * - ****************************************************************************/ - -/** -* IMU gyro auto calibration enable. -* -* @boolean -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 1f84dec4db..3cd5cce3ff 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -35,18 +35,12 @@ add_subdirectory(data_validator) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -if(CONFIG_SENSORS_VEHICLE_ACCELERATION) - add_subdirectory(vehicle_acceleration) -endif() - -add_subdirectory(vehicle_imu) - if(CONFIG_SENSORS_VEHICLE_AIR_DATA) add_subdirectory(vehicle_air_data) endif() -if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - add_subdirectory(vehicle_angular_velocity) +if(CONFIG_SENSORS_VEHICLE_IMU) + add_subdirectory(vehicle_imu) endif() if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) @@ -68,8 +62,7 @@ px4_add_module( ${CMAKE_CURRENT_SOURCE_DIR} SRCS sensors.cpp - voted_sensors_update.cpp - voted_sensors_update.h + sensors.hpp Integrator.hpp MODULE_CONFIG module.yaml @@ -78,13 +71,8 @@ px4_add_module( data_validator mathlib sensor_calibration - vehicle_imu ) -if(CONFIG_SENSORS_VEHICLE_ACCELERATION) - target_link_libraries(modules__sensors PRIVATE vehicle_acceleration) -endif() - if(CONFIG_SENSORS_VEHICLE_AIR_DATA) target_link_libraries(modules__sensors PRIVATE vehicle_air_data) endif() @@ -93,8 +81,8 @@ if(CONFIG_SENSORS_VEHICLE_AIRSPEED) target_link_libraries(modules__sensors PRIVATE airspeed) endif() -if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - target_link_libraries(modules__sensors PRIVATE vehicle_angular_velocity) +if(CONFIG_SENSORS_VEHICLE_IMU) + target_link_libraries(modules__sensors PRIVATE vehicle_imu) endif() if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) diff --git a/src/modules/sensors/Integrator.hpp b/src/modules/sensors/Integrator.hpp index b6c26f331e..1b2cd823ef 100644 --- a/src/modules/sensors/Integrator.hpp +++ b/src/modules/sensors/Integrator.hpp @@ -81,6 +81,7 @@ public: * @param reset_interval New reset time interval for the integrator in microseconds. */ void set_reset_interval(uint32_t reset_interval_us) { _reset_interval_min = reset_interval_us * 1e-6f; } + uint32_t reset_interval_us() const { return _reset_interval_min * 1e6f; } /** * Set required samples for reset. This won't reset the integrator. @@ -90,6 +91,8 @@ public: void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; } uint8_t get_reset_samples() const { return _reset_samples_min; } + uint8_t integrated_samples() const { return _integrated_samples; } + /** * Is the Integrator ready to reset? * @@ -142,7 +145,7 @@ protected: matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ float _integral_dt{0}; - float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */ + float _reset_interval_min{0.01f}; /**< the interval after which the content will be published and the integrator reset */ uint8_t _reset_samples_min{1}; uint8_t _integrated_samples{0}; diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index 9319e80633..47579cdee1 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -20,12 +20,8 @@ if MODULES_SENSORS bool "Include vehicle air data" default y - config SENSORS_VEHICLE_ANGULAR_VELOCITY - bool "Include vehicle angular velocity" - default y - - config SENSORS_VEHICLE_ACCELERATION - bool "Include vehicle acceleration" + config SENSORS_VEHICLE_IMU + bool "Include vehicle IMU" default y config SENSORS_VEHICLE_GPS_POSITION diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd93e18dff..b546196cea 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -210,18 +210,6 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); */ PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1); -/** - * Sensors hub IMU mode - * - * @value 0 Disabled - * @value 1 Publish primary IMU selection - * - * @category system - * @reboot_required true - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); - /** * Enable internal barometers * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..a79a73da96 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -43,18 +43,12 @@ #include "sensors.hpp" -Sensors::Sensors(bool hil_enabled) : +Sensors::Sensors() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _hil_enabled(hil_enabled), - _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), - _voted_sensors_update(hil_enabled, _vehicle_imu_sub) + _loop_perf(perf_alloc(PC_ELAPSED, "sensors")) { - _sensor_pub.advertise(); -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - _vehicle_acceleration.Start(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Differential pressure offset */ @@ -63,6 +57,10 @@ Sensors::Sensors(bool hil_enabled) : _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ +#if defined(CONFIG_SENSORS_VEHICLE_IMU) + _vehicle_imu.Start(); +#endif // CONFIG_SENSORS_VEHICLE_IMU + _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); @@ -82,27 +80,15 @@ Sensors::Sensors(bool hil_enabled) : param_find("SYS_CAL_TMAX"); param_find("SYS_CAL_TMIN"); - _sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - parameters_update(); - - InitializeVehicleIMU(); } Sensors::~Sensors() { - // clear all registered callbacks - for (auto &sub : _vehicle_imu_sub) { - sub.unregisterCallback(); - } -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - _vehicle_acceleration.Stop(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - _vehicle_angular_velocity.Stop(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#if defined(CONFIG_SENSORS_VEHICLE_IMU) + _vehicle_imu.Stop(); +#endif // CONFIG_SENSORS_VEHICLE_IMU #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) @@ -140,29 +126,17 @@ Sensors::~Sensors() #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - for (auto &vehicle_imu : _vehicle_imu_list) { - if (vehicle_imu) { - vehicle_imu->Stop(); - delete vehicle_imu; - } - } - perf_free(_loop_perf); } bool Sensors::init() { - _vehicle_imu_sub[0].registerCallback(); - ScheduleNow(); + ScheduleDelayed(100_ms); return true; } int Sensors::parameters_update() { - if (_armed) { - return 0; - } - #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); @@ -175,69 +149,6 @@ int Sensors::parameters_update() param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - _voted_sensors_update.parametersUpdate(); - - // 1. mark all existing sensor calibrations active even if sensor is missing - // this preserves the calibration in the event of a parameter export while the sensor is missing - // 2. ensure calibration slots are active for the number of sensors currently available - // this to done to eliminate differences in the active set of parameters before and after sensor calibration - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - // sensor_accel - { - const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); - - if (device_id_accel != 0) { - calibration::Accelerometer accel_cal(device_id_accel); - } - - uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - - if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { - calibration::Accelerometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); - } - } - - - // sensor_gyro - { - const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); - - if (device_id_gyro != 0) { - calibration::Gyroscope gyro_cal(device_id_gyro); - } - - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - - if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { - calibration::Gyroscope cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); - } - } - - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - // sensor_mag - { - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - - if (device_id_mag != 0) { - calibration::Magnetometer mag_cal(device_id_mag); - } - - uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; - - if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { - calibration::Magnetometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); - } - } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - } - #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) InitializeVehicleAirData(); #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA @@ -367,11 +278,6 @@ void Sensors::diff_pres_poll() void Sensors::adc_poll() { - /* only read if not in HIL mode */ - if (_hil_enabled) { - return; - } - #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL if (_parameters.diff_pres_analog_scale > 0.0f) { @@ -447,41 +353,6 @@ void Sensors::InitializeVehicleGPSPosition() } #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION -void Sensors::InitializeVehicleIMU() -{ - // create a VehicleIMU instance for each accel/gyro pair - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - if (_vehicle_imu_list[i] == nullptr) { - - uORB::Subscription accel_sub{ORB_ID(sensor_accel), i}; - uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i}; - - if (accel_sub.advertised() && gyro_sub.advertised()) { - // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ - // otherwise each VehicleIMU runs in a corresponding INSx WQ - const bool multi_mode = (_param_sens_imu_mode.get() == 0); - const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0; - - VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config); - - if (imu != nullptr) { - // Start VehicleIMU instance and store - if (imu->Start()) { - _vehicle_imu_list[i] = imu; - - } else { - delete imu; - } - } - - } else { - // abort on first failure, try again later - return; - } - } - } -} - #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) void Sensors::InitializeVehicleMagnetometer() { @@ -517,109 +388,21 @@ void Sensors::InitializeVehicleOpticalFlow() void Sensors::Run() { if (should_exit()) { - // clear all registered callbacks - for (auto &sub : _vehicle_imu_sub) { - sub.unregisterCallback(); - } - exit_and_cleanup(); return; } perf_begin(_loop_perf); - // check vehicle status for changes to publication state - if (_vcontrol_mode_sub.updated()) { - vehicle_control_mode_s vcontrol_mode{}; + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { - _armed = vcontrol_mode.flag_armed; - } - } - - // keep adding sensors as long as we are not armed, - // when not adding sensors poll for param updates - if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) { - - bool updated = false; - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - const int n_baro = orb_group_count(ORB_ID(sensor_baro)); - - if (n_baro != _n_baro) { - _n_baro = n_baro; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - const int n_gps = orb_group_count(ORB_ID(sensor_gps)); - - if (n_gps != _n_gps) { - _n_gps = n_gps; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - const int n_mag = orb_group_count(ORB_ID(sensor_mag)); - - if (n_mag != _n_mag) { - _n_mag = n_mag; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow)); - - if (n_optical_flow != _n_optical_flow) { - _n_optical_flow = n_optical_flow; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - - - const int n_accel = orb_group_count(ORB_ID(sensor_accel)); - const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); - - if ((n_accel != _n_accel) || (n_gyro != _n_gyro) || updated) { - _n_accel = n_accel; - _n_gyro = n_gyro; - - parameters_update(); - } - - // sensor device id (not just orb_group_count) must be populated before IMU init can succeed - _voted_sensors_update.initializeSensors(); - InitializeVehicleIMU(); - - _last_config_update = hrt_absolute_time(); - - } else { - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - parameters_update(); - updateParams(); - } - } - - _voted_sensors_update.sensorsPoll(_sensor_combined); - - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { - - _voted_sensors_update.setRelativeTimestamps(_sensor_combined); - _sensor_pub.publish(_sensor_combined); - _sensor_combined_prev_timestamp = _sensor_combined.timestamp; + // update parameters from storage + updateParams(); + parameters_update(); } #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) @@ -628,43 +411,12 @@ void Sensors::Run() diff_pres_poll(); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - // backup schedule as a watchdog timeout - ScheduleDelayed(10_ms); - perf_end(_loop_perf); } int Sensors::task_spawn(int argc, char *argv[]) { - bool hil_enabled = false; - bool error_flag = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'h': - hil_enabled = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return PX4_ERROR; - } - - Sensors *instance = new Sensors(hil_enabled); + Sensors *instance = new Sensors(); if (instance) { _object.store(instance); @@ -687,7 +439,10 @@ int Sensors::task_spawn(int argc, char *argv[]) int Sensors::print_status() { - _voted_sensors_update.printStatus(); +#if defined(CONFIG_SENSORS_VEHICLE_IMU) + PX4_INFO_RAW("\n"); + _vehicle_imu.PrintStatus(); +#endif // CONFIG_SENSORS_VEHICLE_IMU #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) @@ -722,16 +477,6 @@ int Sensors::print_status() #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - PX4_INFO_RAW("\n"); - _vehicle_acceleration.PrintStatus(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - PX4_INFO_RAW("\n"); - _vehicle_angular_velocity.PrintStatus(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY - #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) if (_vehicle_gps_position) { @@ -741,15 +486,6 @@ int Sensors::print_status() #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - PX4_INFO_RAW("\n"); - - for (auto &i : _vehicle_imu_list) { - if (i != nullptr) { - PX4_INFO_RAW("\n"); - i->PrintStatus(); - } - } - return 0; } @@ -787,7 +523,6 @@ It runs in its own thread and polls on the currently selected gyro topic. PRINT_MODULE_USAGE_NAME("sensors", "system"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 102be087b1..4221af654d 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -45,16 +45,7 @@ #include #include #include -#include -#include #include -#include -#include "voted_sensors_update.h" -#include "vehicle_imu/VehicleIMU.hpp" - -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) -# include "vehicle_acceleration/VehicleAcceleration.hpp" -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) # include @@ -70,9 +61,9 @@ # include "vehicle_air_data/VehicleAirData.hpp" #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) -# include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#if defined(CONFIG_SENSORS_VEHICLE_IMU) +# include "vehicle_imu/VehicleIMU.hpp" +#endif // CONFIG_SENSORS_VEHICLE_IMU #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) # include "vehicle_gps_position/VehicleGPSPosition.hpp" @@ -80,8 +71,6 @@ #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) # include "vehicle_magnetometer/VehicleMagnetometer.hpp" -# include -# include #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) @@ -98,7 +87,7 @@ using namespace time_literals; class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - explicit Sensors(bool hil_enabled); + Sensors(); ~Sensors() override; /** @see ModuleBase */ @@ -123,44 +112,13 @@ private: int parameters_update(); void InitializeVehicleAirData(); - void InitializeVehicleGPSPosition(); - - void InitializeVehicleIMU(); - void InitializeVehicleMagnetometer(); - void InitializeVehicleOpticalFlow(); - const bool _hil_enabled; /**< if true, HIL is active */ - perf_counter_t _loop_perf; /**< loop performance counter */ - VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; - - VotedSensorsUpdate _voted_sensors_update; - - sensor_combined_s _sensor_combined{}; - - hrt_abstime _last_config_update{0}; - hrt_abstime _sensor_combined_prev_timestamp{0}; - - uint8_t _n_accel{0}; - uint8_t _n_gyro{0}; - - bool _armed{false}; /**< arming status of the vehicle */ - - uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] { - {this, ORB_ID(vehicle_imu), 0}, - {this, ORB_ID(vehicle_imu), 1}, - {this, ORB_ID(vehicle_imu), 2}, - {this, ORB_ID(vehicle_imu), 3} - }; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; - - uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /** @@ -201,54 +159,46 @@ private: # endif // ADC_AIRSPEED_VOLTAGE_CHANNEL struct Parameters { - float diff_pres_offset_pa; + float diff_pres_offset_pa{0.f}; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - float diff_pres_analog_scale; + float diff_pres_analog_scale {0.f}; #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - int32_t air_cmodel; - float air_tube_length; - float air_tube_diameter_mm; + int32_t air_cmodel{0}; + float air_tube_length{0.f}; + float air_tube_diameter_mm{0.f}; } _parameters{}; /**< local copies of interesting parameters */ struct ParameterHandles { - param_t diff_pres_offset_pa; + param_t diff_pres_offset_pa{PARAM_INVALID}; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_t diff_pres_analog_scale; + param_t diff_pres_analog_scale {PARAM_INVALID}; #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - param_t air_cmodel; - param_t air_tube_length; - param_t air_tube_diameter_mm; + param_t air_cmodel{PARAM_INVALID}; + param_t air_tube_length{PARAM_INVALID}; + param_t air_tube_diameter_mm{PARAM_INVALID}; } _parameter_handles{}; /**< handles for interesting parameters */ #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - VehicleAcceleration _vehicle_acceleration; -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) VehicleAirData *_vehicle_air_data {nullptr}; - uint8_t _n_baro{0}; #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - VehicleAngularVelocity _vehicle_angular_velocity; -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#if defined(CONFIG_SENSORS_VEHICLE_IMU) + VehicleIMU _vehicle_imu {}; +#endif // CONFIG_SENSORS_VEHICLE_IMU #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) VehicleMagnetometer *_vehicle_magnetometer {nullptr}; - uint8_t _n_mag{0}; #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) VehicleGPSPosition *_vehicle_gps_position {nullptr}; - uint8_t _n_gps{0}; #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) VehicleOpticalFlow *_vehicle_optical_flow {nullptr}; - uint8_t _n_optical_flow{0}; #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW DEFINE_PARAMETERS( @@ -259,8 +209,7 @@ private: (ParamBool) _param_sys_has_gps, #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - (ParamInt) _param_sys_has_mag, + (ParamBool) _param_sys_has_mag #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - (ParamBool) _param_sens_imu_mode ) -}; \ No newline at end of file +}; diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt deleted file mode 100644 index 37f3558992..0000000000 --- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 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(vehicle_acceleration - VehicleAcceleration.cpp - VehicleAcceleration.hpp -) - -target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) - -target_link_libraries(vehicle_acceleration - PRIVATE - mathlib - px4_work_queue - sensor_calibration -) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp deleted file mode 100644 index af2f948f3a..0000000000 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-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 "VehicleAcceleration.hpp" - -#include - -#include - -using namespace matrix; - -namespace sensors -{ - -VehicleAcceleration::VehicleAcceleration() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) -{ - _vehicle_acceleration_pub.advertise(); - - CheckAndUpdateFilters(); -} - -VehicleAcceleration::~VehicleAcceleration() -{ - Stop(); -} - -bool VehicleAcceleration::Start() -{ - // force initial updates - ParametersUpdate(true); - - // sensor_selection needed to change the active sensor if the primary stops updating - if (!_sensor_selection_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - if (!SensorSelectionUpdate(true)) { - _sensor_sub.registerCallback(); - } - - return true; -} - -void VehicleAcceleration::Stop() -{ - // clear all registered callbacks - _sensor_sub.unregisterCallback(); - _sensor_selection_sub.unregisterCallback(); - - Deinit(); -} - -void VehicleAcceleration::CheckAndUpdateFilters() -{ - bool sample_rate_changed = false; - - // get sample rate from vehicle_imu_status publication - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - - const float sample_rate_hz = imu_status.get().accel_rate_hz; - - if (imu_status.advertised() && (imu_status.get().timestamp != 0) - && (imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id()) - && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { - - // check if sample rate error is greater than 1% - if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { - PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); - _filter_sample_rate = sample_rate_hz; - sample_rate_changed = true; - - // determine number of sensor samples that will get closest to the desired rate - if (_param_imu_integ_rate.get() > 0) { - const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); - const float sample_interval_avg = 1e6f / sample_rate_hz; - const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, - (float)sensor_accel_s::ORB_QUEUE_LENGTH); - - _sensor_sub.set_required_updates(samples); - - } else { - _sensor_sub.set_required_updates(1); - } - - break; - } - } - } - - // update software low pass filters - if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) { - _lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get()); - _lp_filter.reset(_acceleration_prev); - } -} - -void VehicleAcceleration::SensorBiasUpdate(bool force) -{ - // find corresponding estimated sensor bias - if (_estimator_selector_status_sub.updated()) { - estimator_selector_status_s estimator_selector_status; - - if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { - _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); - } - } - - if (_estimator_sensor_bias_sub.updated() || force) { - estimator_sensor_bias_s bias; - - if (_estimator_sensor_bias_sub.copy(&bias)) { - if (bias.accel_device_id == _calibration.device_id()) { - _bias = Vector3f{bias.accel_bias}; - - } else { - _bias.zero(); - } - } - } -} - -bool VehicleAcceleration::SensorSelectionUpdate(bool force) -{ - if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) { - sensor_selection_s sensor_selection{}; - _sensor_selection_sub.copy(&sensor_selection); - - if ((sensor_selection.accel_device_id != 0) && (_calibration.device_id() != sensor_selection.accel_device_id)) { - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - - const uint32_t device_id = sensor_accel_sub.get().device_id; - - if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0) - && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) { - - if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { - PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id); - - // clear bias and corrections - _bias.zero(); - - _calibration.set_device_id(device_id); - - CheckAndUpdateFilters(); - - return true; - } - } - } - - PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.accel_device_id); - _calibration.set_device_id(0); - } - } - - return false; -} - -void VehicleAcceleration::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - - _calibration.ParametersUpdate(); - - CheckAndUpdateFilters(); - } -} - -void VehicleAcceleration::Run() -{ - // backup schedule - ScheduleDelayed(10_ms); - - // update corrections first to set _selected_sensor - bool selection_updated = SensorSelectionUpdate(); - - _calibration.SensorCorrectionsUpdate(selection_updated); - SensorBiasUpdate(selection_updated); - ParametersUpdate(); - - // require valid sensor sample rate to run - if (!PX4_ISFINITE(_filter_sample_rate)) { - CheckAndUpdateFilters(); - - if (!PX4_ISFINITE(_filter_sample_rate)) { - return; - } - } - - // process all outstanding messages - sensor_accel_s sensor_data; - - while (_sensor_sub.update(&sensor_data)) { - const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z}; - - if (accel_raw.isAllFinite()) { - // Apply calibration and filter - // - calibration offsets, scale factors, and thermal scale (if available) - // - estimated in run bias (if available) - // - biquad low-pass filter - const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias; - const Vector3f accel_filtered = _lp_filter.apply(accel_corrected); - - _acceleration_prev = accel_corrected; - - // publish once all new samples are processed - if (!_sensor_sub.updated()) { - // Publish vehicle_acceleration - vehicle_acceleration_s v_acceleration; - v_acceleration.timestamp_sample = sensor_data.timestamp_sample; - accel_filtered.copyTo(v_acceleration.xyz); - v_acceleration.timestamp = hrt_absolute_time(); - _vehicle_acceleration_pub.publish(v_acceleration); - - return; - } - } - } -} - -void VehicleAcceleration::PrintStatus() -{ - PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32 ", rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]\n", - _calibration.device_id(), (double)_filter_sample_rate, - (double)_bias(0), (double)_bias(1), (double)_bias(2)); - - _calibration.PrintStatus(); -} - -} // namespace sensors diff --git a/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c b/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c deleted file mode 100644 index c4fda14b94..0000000000 --- a/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** -* Low pass filter cutoff frequency for accel -* -* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer. -* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt deleted file mode 100644 index 8ab38d47bd..0000000000 --- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 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(vehicle_angular_velocity - VehicleAngularVelocity.cpp - VehicleAngularVelocity.hpp -) - -target_compile_options(vehicle_angular_velocity - PRIVATE - ${MAX_CUSTOM_OPT_LEVEL} - #-DDEBUG_BUILD -) - -target_link_libraries(vehicle_angular_velocity - PRIVATE - mathlib - px4_work_queue - sensor_calibration -) diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index fe6ddeb8a4..a45b0e4076 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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,12 +34,23 @@ px4_add_library(vehicle_imu VehicleIMU.cpp VehicleIMU.hpp + + VehicleAcceleration.cpp + VehicleAcceleration.hpp + VehicleAngularVelocity.cpp + VehicleAngularVelocity.hpp ) target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL} #-DDEBUG_BUILD + -Wno-error ) -target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration) +target_link_libraries(vehicle_imu + PRIVATE + mathlib + px4_work_queue + sensor_calibration +) diff --git a/src/modules/sensors/vehicle_imu/IMU.hpp b/src/modules/sensors/vehicle_imu/IMU.hpp new file mode 100644 index 0000000000..aec63fa706 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/IMU.hpp @@ -0,0 +1,127 @@ + + +#pragma once + +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace sensors +{ + +struct IMU { + + struct InFlightCalibration { + uint8_t instance{0}; + matrix::Vector3f offset{}; + matrix::Vector3f bias_variance{}; + bool valid{false}; + }; + + struct { + + calibration::Accelerometer calibration{}; + math::WelfordMeanVector raw_mean{}; + + + + + // FIFO only + matrix::Vector3f integral{}; + int16_t last_raw_sample[3] {}; + float fifo_scale{1.f}; + + math::WelfordMean mean_publish_interval_us{}; // only needed for FIFO + + + // redundant for FIFO case + hrt_abstime timestamp_sample_last{0}; + uint32_t device_id{0}; + uint32_t error_count{0}; + math::WelfordMean temperature{}; // redundant for FIFO case + math::WelfordMean mean_sample_interval_us{}; + bool interval_configured{false}; + + + // non-FIFO only + sensors::Integrator integrator{}; + + + + + matrix::Vector3f estimated_bias{}; + matrix::Vector3f estimated_bias_variance{}; + + uint32_t clipping_total[3] {}; // clipping + uint8_t clipping_flags{0}; + + matrix::Vector3f acceleration_prev{}; // acceleration from the previous IMU measurement for vibration metrics + + float vibration_metric{0.f}; // high frequency vibration level in the accelerometer data (m/s/s) + + + InFlightCalibration learned_calibration[3] {}; + + } accel{}; + + struct { + hrt_abstime timestamp_sample_last{0}; + uint32_t device_id{0}; + calibration::Gyroscope calibration{}; + math::WelfordMean mean_publish_interval_us{}; + math::WelfordMean mean_sample_interval_us{}; + math::WelfordMeanVector raw_mean{}; + math::WelfordMean temperature{}; + + uint32_t error_count{0}; + sensors::IntegratorConing integrator{}; + + matrix::Vector3f estimated_bias{}; + matrix::Vector3f estimated_bias_variance{}; + + uint32_t clipping_total[3] {}; // clipping + uint8_t clipping_flags{0}; + + matrix::Vector3f angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics + + float vibration_metric{0.f}; // high frequency vibration level in the gyro data (rad/s) + + float coning_norm_accum{0}; // average IMU delta angle coning correction (rad^2) + float coning_norm_accum_total_time_s{0}; + + bool interval_configured{false}; + + InFlightCalibration learned_calibration[3] {}; + } gyro{}; + + bool primary{false}; + + uORB::PublicationMulti vehicle_imu_pub{ORB_ID(vehicle_imu)}; + uORB::PublicationMulti vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; + + hrt_abstime time_last_status_publication{0}; + + hrt_abstime time_last_valid{0}; + hrt_abstime time_last_invalid{0}; + + // last valid + // last invalid + + + hrt_abstime time_last_move_detect_us{0}; + hrt_abstime time_still_detect_us{0}; + bool at_rest{false}; + + + // noise when still? + +}; + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_imu/VehicleAcceleration.cpp new file mode 100644 index 0000000000..e88f83f619 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/VehicleAcceleration.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2019-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 "VehicleAcceleration.hpp" + +using namespace matrix; + +namespace sensors +{ + +VehicleAcceleration::VehicleAcceleration() : + ModuleParams(nullptr) +{ + _vehicle_acceleration_pub.advertise(); +} + +void VehicleAcceleration::ParametersUpdate() +{ + updateParams(); + + _filter_config_check = true; +} + +void VehicleAcceleration::updateAccel(IMU &imu, const matrix::Vector3f &accel_raw, const float sample_rate_hz) +{ + bool filter_reset = false; + + if ((_selected_sensor_device_id != imu.accel.calibration.device_id()) || _filter_config_check) { + + bool sample_rate_changed = false; + + if (imu.accel.mean_publish_interval_us.valid() && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { + // check if sample rate error is greater than 5% + if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.05f) { + PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); + _filter_sample_rate = sample_rate_hz; + sample_rate_changed = true; + } + } + + // update software low pass filters + if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) { + filter_reset = true; + } + + _selected_sensor_device_id = imu.accel.calibration.device_id(); + _filter_config_check = false; + } + + if (filter_reset) { + _lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get()); + } + + // Apply calibration and filter + // - calibration offsets, scale factors, and thermal scale (if available) + // - estimated in run bias (if available) + // - biquad low-pass filter + const Vector3f accel_corrected = imu.accel.calibration.Correct(accel_raw) - imu.accel.estimated_bias; + + const Vector3f accel_filtered = filter_reset ? _lp_filter.reset(accel_corrected) : _lp_filter.apply(accel_corrected); + + // Publish vehicle_acceleration + vehicle_acceleration_s v_acceleration; + v_acceleration.timestamp_sample = imu.accel.timestamp_sample_last; + accel_filtered.copyTo(v_acceleration.xyz); + v_acceleration.timestamp = hrt_absolute_time(); + _vehicle_acceleration_pub.publish(v_acceleration); +} + +void VehicleAcceleration::PrintStatus() +{ + PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32 ", rate: %.1f Hz\n", + _selected_sensor_device_id, (double)_filter_sample_rate); +} + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_imu/VehicleAcceleration.hpp similarity index 63% rename from src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp rename to src/modules/sensors/vehicle_imu/VehicleAcceleration.hpp index 68c0652082..0e01f37b31 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleAcceleration.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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,22 +33,15 @@ #pragma once -#include +#include "IMU.hpp" + #include #include #include #include #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include #include using namespace time_literals; @@ -56,46 +49,31 @@ using namespace time_literals; namespace sensors { -class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem +class VehicleAcceleration : public ModuleParams { public: VehicleAcceleration(); - ~VehicleAcceleration() override; + ~VehicleAcceleration() override = default; - bool Start(); - void Stop(); + void ParametersUpdate(); void PrintStatus(); + void updateAccel(IMU &imu, const matrix::Vector3f &accel_raw, const float sample_rate_hz); + private: - void Run() override; - - void CheckAndUpdateFilters(); - void ParametersUpdate(bool force = false); - void SensorBiasUpdate(bool force = false); - bool SensorSelectionUpdate(bool force = false); - - static constexpr int MAX_SENSOR_COUNT = 4; - uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; - uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)}; - - calibration::Accelerometer _calibration{}; - - matrix::Vector3f _bias{}; - matrix::Vector3f _acceleration_prev{}; - float _filter_sample_rate{NAN}; + uint32_t _selected_sensor_device_id{0}; - math::LowPassFilter2p _lp_filter{}; + static constexpr float kDefaultAccelSampleFreqHz{800.f}; + static constexpr float kDefaultAccelCutoffFreqHz{30.f}; + math::LowPassFilter2p _lp_filter{kDefaultAccelSampleFreqHz, kDefaultAccelCutoffFreqHz}; + float _filter_sample_rate{kDefaultAccelSampleFreqHz}; + + bool _filter_config_check{true}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_accel_cutoff, diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_imu/VehicleAngularVelocity.cpp similarity index 51% rename from src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp rename to src/modules/sensors/vehicle_imu/VehicleAngularVelocity.cpp index e688a788e9..766fb00011 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleAngularVelocity.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -35,27 +35,24 @@ #include -#include - using namespace matrix; namespace sensors { VehicleAngularVelocity::VehicleAngularVelocity() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) + ModuleParams(nullptr) { + // force initial updates + ParametersUpdate(); + _vehicle_angular_velocity_pub.advertise(); } VehicleAngularVelocity::~VehicleAngularVelocity() { - Stop(); - perf_free(_cycle_perf); perf_free(_filter_reset_perf); - perf_free(_selection_changed_perf); #if !defined(CONSTRAINED_FLASH) delete[] _dynamic_notch_filter_esc_rpm; @@ -68,49 +65,8 @@ VehicleAngularVelocity::~VehicleAngularVelocity() #endif // CONSTRAINED_FLASH } -bool VehicleAngularVelocity::Start() +bool VehicleAngularVelocity::UpdateSampleRate(float sample_rate_hz, float publish_rate_hz) { - // force initial updates - ParametersUpdate(true); - - // sensor_selection needed to change the active sensor if the primary stops updating - if (!_sensor_selection_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - if (!SensorSelectionUpdate(true)) { - ScheduleNow(); - } - - return true; -} - -void VehicleAngularVelocity::Stop() -{ - // clear all registered callbacks - _sensor_sub.unregisterCallback(); - _sensor_gyro_fifo_sub.unregisterCallback(); - _sensor_selection_sub.unregisterCallback(); - - Deinit(); -} - -bool VehicleAngularVelocity::UpdateSampleRate() -{ - float sample_rate_hz = NAN; - float publish_rate_hz = NAN; - - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - - if (imu_status.get().gyro_device_id == _selected_sensor_device_id) { - sample_rate_hz = imu_status.get().gyro_raw_rate_hz; - publish_rate_hz = imu_status.get().gyro_rate_hz; - break; - } - } - // calculate sensor update rate if (PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 10) && (sample_rate_hz < 10'000) && PX4_ISFINITE(publish_rate_hz) && (publish_rate_hz > 0) @@ -135,22 +91,22 @@ bool VehicleAngularVelocity::UpdateSampleRate() const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get(); const float publish_interval_us = 1e6f / publish_rate_hz; - const uint8_t samples = roundf(configured_interval_us / publish_interval_us); + // const uint8_t samples = roundf(configured_interval_us / publish_interval_us); - if (_fifo_available) { - _sensor_gyro_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH)); + // if (_fifo_available) { + // _sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_imu_fifo_s::ORB_QUEUE_LENGTH)); - } else { - _sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH)); - } + // } else { + // _sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH)); + // } // publish interval (constrained 100 Hz - 8 kHz) _publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us * 0.5f)), 125, 10000); } else { - _sensor_sub.set_required_updates(1); - _sensor_gyro_fifo_sub.set_required_updates(1); + //_sensor_sub.set_required_updates(1); + //_sensor_fifo_sub.set_required_updates(1); _publish_interval_min_us = 0; } } @@ -159,12 +115,12 @@ bool VehicleAngularVelocity::UpdateSampleRate() return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0); } -void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us) +void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us, const IMU &imu) { if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) { - const Vector3f angular_velocity_uncalibrated{GetResetAngularVelocity()}; - const Vector3f angular_acceleration_uncalibrated{GetResetAngularAcceleration()}; + const Vector3f angular_velocity_uncalibrated{GetResetAngularVelocity(imu)}; + const Vector3f angular_acceleration_uncalibrated{GetResetAngularAcceleration(imu)}; for (int axis = 0; axis < 3; axis++) { // angular velocity low pass @@ -203,308 +159,141 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us) } } -void VehicleAngularVelocity::SensorBiasUpdate(bool force) +void VehicleAngularVelocity::ParametersUpdate() { - // find corresponding estimated sensor bias - if (_estimator_selector_status_sub.updated()) { - estimator_selector_status_s estimator_selector_status; + const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); + const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); - if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { - _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + updateParams(); + + const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); + const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); + + // IMU_GYRO_RATEMAX + if (_param_imu_gyro_ratemax.get() <= 0) { + const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get(); + _param_imu_gyro_ratemax.reset(); + PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax, + _param_imu_gyro_ratemax.get()); + } + + // constrain IMU_GYRO_RATEMAX 50-10,000 Hz + const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000); + + if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) { + PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax); + _param_imu_gyro_ratemax.set(imu_gyro_ratemax); + _param_imu_gyro_ratemax.commit_no_notification(); + } + + // gyro low pass cutoff frequency changed + for (auto &lp : _lp_filter_velocity) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; } } - if (_estimator_sensor_bias_sub.updated() || force) { - estimator_sensor_bias_s bias; + // gyro notch filter 0 frequency or bandwidth changed + for (auto &nf : _notch_filter0_velocity) { + const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f); + const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f); - if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) { - _bias = Vector3f{bias.gyro_bias}; - - } else { - _bias.zero(); - } - } -} - -bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force) -{ - if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) { - sensor_selection_s sensor_selection{}; - _sensor_selection_sub.copy(&sensor_selection); - - bool selected_device_id_valid = false; - uint32_t device_id = sensor_selection.gyro_device_id; - uint32_t device_id_first_valid_imu = 0; - - // use vehicle_imu_status to do basic sensor selection validation - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - - if (imu_status.advertised() - && (imu_status.get().timestamp != 0) && (time_now_us < imu_status.get().timestamp + 1_s) - && (imu_status.get().gyro_device_id != 0)) { - // vehicle_imu_status gyro valid - - if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id)) { - selected_device_id_valid = true; - } - - // record first valid IMU as a backup option - if (device_id_first_valid_imu == 0) { - device_id_first_valid_imu = imu_status.get().gyro_device_id; - } - } - } - - // if no gyro selected or healthy then use fallback - if ((device_id == 0) || !selected_device_id_valid) { - device_id = device_id_first_valid_imu; - } - - if ((_selected_sensor_device_id != device_id) || force) { - - const bool device_id_valid = (device_id != 0); - - // see if the selected sensor publishes sensor_gyro_fifo - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; - - if (sensor_gyro_fifo_sub.advertised() - && (sensor_gyro_fifo_sub.get().timestamp != 0) - && (sensor_gyro_fifo_sub.get().device_id != 0) - && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) { - - // if no gyro was selected use the first valid sensor_gyro_fifo - if (!device_id_valid) { - device_id = sensor_gyro_fifo_sub.get().device_id; - PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id); - } - - if (sensor_gyro_fifo_sub.get().device_id == device_id) { - if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) { - // make sure non-FIFO sub is unregistered - _sensor_sub.unregisterCallback(); - - _calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id); - - _selected_sensor_device_id = sensor_gyro_fifo_sub.get().device_id; - - _timestamp_sample_last = 0; - _filter_sample_rate_hz = 1.f / (sensor_gyro_fifo_sub.get().dt * 1e-6f); - _update_sample_rate = true; - _reset_filters = true; - _bias.zero(); - _fifo_available = true; - - perf_count(_selection_changed_perf); - PX4_DEBUG("selecting sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id); - return true; - - } else { - PX4_ERR("unable to register callback for sensor_gyro_fifo:%" PRIu8 " %" PRIu32, - i, sensor_gyro_fifo_sub.get().device_id); - } - } - } - } - - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - - if (sensor_gyro_sub.advertised() - && (sensor_gyro_sub.get().timestamp != 0) - && (sensor_gyro_sub.get().device_id != 0) - && (time_now_us < sensor_gyro_sub.get().timestamp + 1_s)) { - - // if no gyro was selected use the first valid sensor_gyro - if (!device_id_valid) { - device_id = sensor_gyro_sub.get().device_id; - PX4_WARN("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); - } - - if (sensor_gyro_sub.get().device_id == device_id) { - if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { - // make sure FIFO sub is unregistered - _sensor_gyro_fifo_sub.unregisterCallback(); - - _calibration.set_device_id(sensor_gyro_sub.get().device_id); - - _selected_sensor_device_id = sensor_gyro_sub.get().device_id; - - _timestamp_sample_last = 0; - _filter_sample_rate_hz = NAN; - _update_sample_rate = true; - _reset_filters = true; - _bias.zero(); - _fifo_available = false; - - perf_count(_selection_changed_perf); - PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id); - return true; - - } else { - PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, - i, sensor_gyro_sub.get().device_id); - } - } - } - } - - if (device_id != 0) { - PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", device_id); - } - - _selected_sensor_device_id = 0; + if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) { + _reset_filters = true; + break; } } - return false; -} + // gyro notch filter 1 frequency or bandwidth changed + for (auto &nf : _notch_filter1_velocity) { + const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f); + const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f); -void VehicleAngularVelocity::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); - const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); - - updateParams(); - - const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); - const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); - - _calibration.ParametersUpdate(); - - // IMU_GYRO_RATEMAX - if (_param_imu_gyro_ratemax.get() <= 0) { - const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get(); - _param_imu_gyro_ratemax.reset(); - PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax, - _param_imu_gyro_ratemax.get()); + if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) { + _reset_filters = true; + break; } + } - // constrain IMU_GYRO_RATEMAX 50-10,000 Hz - const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000); - - if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) { - PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax); - _param_imu_gyro_ratemax.set(imu_gyro_ratemax); - _param_imu_gyro_ratemax.commit_no_notification(); - } - - // gyro low pass cutoff frequency changed - for (auto &lp : _lp_filter_velocity) { - if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { - _reset_filters = true; - break; - } - } - - // gyro notch filter 0 frequency or bandwidth changed - for (auto &nf : _notch_filter0_velocity) { - const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f); - const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f); - - if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) { - _reset_filters = true; - break; - } - } - - // gyro notch filter 1 frequency or bandwidth changed - for (auto &nf : _notch_filter1_velocity) { - const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f); - const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f); - - if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) { - _reset_filters = true; - break; - } - } - - // gyro derivative low pass cutoff changed - for (auto &lp : _lp_filter_acceleration) { - if (fabsf(lp.getCutoffFreq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { - _reset_filters = true; - break; - } + // gyro derivative low pass cutoff changed + for (auto &lp : _lp_filter_acceleration) { + if (fabsf(lp.getCutoffFreq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; } + } #if !defined(CONSTRAINED_FLASH) - if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) { + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) { - const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10); + const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10); - if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) { - delete[] _dynamic_notch_filter_esc_rpm; - _dynamic_notch_filter_esc_rpm = nullptr; - _esc_rpm_harmonics = 0; - } + if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) { + delete[] _dynamic_notch_filter_esc_rpm; + _dynamic_notch_filter_esc_rpm = nullptr; + _esc_rpm_harmonics = 0; + } - if (_dynamic_notch_filter_esc_rpm == nullptr) { + if (_dynamic_notch_filter_esc_rpm == nullptr) { - _dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics]; + _dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics]; - if (_dynamic_notch_filter_esc_rpm) { - _esc_rpm_harmonics = esc_rpm_harmonics; + if (_dynamic_notch_filter_esc_rpm) { + _esc_rpm_harmonics = esc_rpm_harmonics; - if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) { - _dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ESC RPM disable"); - } - - if (_dynamic_notch_filter_esc_rpm_init_perf == nullptr) { - _dynamic_notch_filter_esc_rpm_init_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ESC RPM init"); - } - - if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) { - _dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ESC RPM update"); - } - - } else { - _esc_rpm_harmonics = 0; - - perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); - perf_free(_dynamic_notch_filter_esc_rpm_init_perf); - perf_free(_dynamic_notch_filter_esc_rpm_update_perf); - - _dynamic_notch_filter_esc_rpm_disable_perf = nullptr; - _dynamic_notch_filter_esc_rpm_init_perf = nullptr; - _dynamic_notch_filter_esc_rpm_update_perf = nullptr; + if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) { + _dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ESC RPM disable"); } - } - } else { - DisableDynamicNotchEscRpm(); + if (_dynamic_notch_filter_esc_rpm_init_perf == nullptr) { + _dynamic_notch_filter_esc_rpm_init_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ESC RPM init"); + } + + if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) { + _dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ESC RPM update"); + } + + } else { + _esc_rpm_harmonics = 0; + + perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); + perf_free(_dynamic_notch_filter_esc_rpm_init_perf); + perf_free(_dynamic_notch_filter_esc_rpm_update_perf); + + _dynamic_notch_filter_esc_rpm_disable_perf = nullptr; + _dynamic_notch_filter_esc_rpm_init_perf = nullptr; + _dynamic_notch_filter_esc_rpm_update_perf = nullptr; + } } - if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) { - if (_dynamic_notch_filter_fft_disable_perf == nullptr) { - _dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable"); - _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); - } + } else { + DisableDynamicNotchEscRpm(); + } - } else { - DisableDynamicNotchFFT(); + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) { + if (_dynamic_notch_filter_fft_disable_perf == nullptr) { + _dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable"); + _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); } + } else { + DisableDynamicNotchFFT(); + } + #endif // !CONSTRAINED_FLASH - } } -Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const +Vector3f VehicleAngularVelocity::GetResetAngularVelocity(const IMU &imu) const { if (_last_publish != 0) { // angular velocity filtering is performed on raw unscaled data // start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection) - Vector3f angular_velocity_uncalibrated{_calibration.Uncorrect(_angular_velocity + _bias)}; + Vector3f angular_velocity_uncalibrated{imu.gyro.calibration.Uncorrect(_angular_velocity + imu.gyro.estimated_bias)}; if (angular_velocity_uncalibrated.isAllFinite()) { return angular_velocity_uncalibrated; @@ -514,12 +303,12 @@ Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const return Vector3f{0.f, 0.f, 0.f}; } -Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const +Vector3f VehicleAngularVelocity::GetResetAngularAcceleration(const IMU &imu) const { if (_last_publish != 0) { // angular acceleration filtering is performed on unscaled angular velocity data // start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection) - Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration}; + Vector3f angular_acceleration{imu.gyro.calibration.rotation().I() *_angular_acceleration}; if (angular_acceleration.isAllFinite()) { return angular_acceleration; @@ -781,137 +570,150 @@ float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float inverse_ return angular_acceleration_filtered; } -void VehicleAngularVelocity::Run() +void VehicleAngularVelocity::update(IMU &imu) { - perf_begin(_cycle_perf); - - // backup schedule - ScheduleDelayed(10_ms); - const hrt_abstime time_now_us = hrt_absolute_time(); // update corrections first to set _selected_sensor - const bool selection_updated = SensorSelectionUpdate(time_now_us); + const bool selection_updated = (_selected_sensor_device_id != imu.gyro.calibration.device_id()); - if (selection_updated || _update_sample_rate) { - if (!UpdateSampleRate()) { + if (selection_updated) { + + // TODO: parent register callback + { + // make sure non-FIFO sub is unregistered + + _timestamp_sample_last = 0; + //_filter_sample_rate_hz = 1.f / (sensor_fifo_data.dt * 1e-6f); + _update_sample_rate = true; + _reset_filters = true; + _fifo_available = true; + + PX4_DEBUG("selecting sensor_imu_fifo:%" PRIu8 " %" PRIu32, 0, _selected_sensor_device_id); + } + + if (!UpdateSampleRate(1e6f / imu.gyro.mean_sample_interval_us.mean(), + 1e6f / imu.gyro.mean_publish_interval_us.mean())) { // sensor sample rate required to run perf_end(_cycle_perf); return; } + + _selected_sensor_device_id = imu.gyro.calibration.device_id(); } - ParametersUpdate(); - - _calibration.SensorCorrectionsUpdate(selection_updated); - SensorBiasUpdate(selection_updated); - if (_reset_filters) { - ResetFilters(time_now_us); - - if (_reset_filters) { - // not safe to run until filters configured - perf_end(_cycle_perf); - return; - } + ResetFilters(time_now_us, imu); } UpdateDynamicNotchEscRpm(time_now_us); UpdateDynamicNotchFFT(time_now_us); - if (_fifo_available) { - // process all outstanding fifo messages - sensor_gyro_fifo_s sensor_fifo_data; +} - while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) { - const float inverse_dt_s = 1e6f / sensor_fifo_data.dt; - const int N = sensor_fifo_data.samples; - static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); +void VehicleAngularVelocity::updateSensorImuFifo(IMU &imu, const sensor_imu_fifo_s &sensor_fifo_data) +{ + perf_begin(_cycle_perf); - if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) { - Vector3f angular_velocity_uncalibrated; - Vector3f angular_acceleration_uncalibrated; + const hrt_abstime time_now_us = hrt_absolute_time(); - int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z}; + update(imu); - for (int axis = 0; axis < 3; axis++) { - // copy raw int16 sensor samples to float array for filtering - float data[FIFO_SIZE_MAX]; + UpdateDynamicNotchEscRpm(time_now_us); + UpdateDynamicNotchFFT(time_now_us); - for (int n = 0; n < N; n++) { - data[n] = sensor_fifo_data.scale * raw_data_array[axis][n]; - } + const float inverse_dt_s = 1e6f / sensor_fifo_data.dt; + const int N = sensor_fifo_data.samples; + static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.gyro_x) / sizeof(sensor_fifo_data.gyro_x[0]); - // save last filtered sample - angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data, N); - angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data, N); - } + if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) { + Vector3f angular_velocity_uncalibrated; + Vector3f angular_acceleration_uncalibrated; - // Publish - if (!_sensor_gyro_fifo_sub.updated()) { - if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, - angular_velocity_uncalibrated, - angular_acceleration_uncalibrated)) { + const int16_t *raw_data_array[] {sensor_fifo_data.gyro_x, sensor_fifo_data.gyro_y, sensor_fifo_data.gyro_z}; - perf_end(_cycle_perf); - return; - } - } + for (int axis = 0; axis < 3; axis++) { + // copy raw int16 sensor samples to float array for filtering + float data[FIFO_SIZE_MAX]; + + for (int n = 0; n < N; n++) { + data[n] = sensor_fifo_data.gyro_scale * raw_data_array[axis][n]; } + + // save last filtered sample + angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data, N); + angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data, N); } - } else { - // process all outstanding messages - sensor_gyro_s sensor_data; + // Publish + //if (!_sensor_fifo_sub.updated()) { + if (hrt_elapsed_time(&sensor_fifo_data.timestamp) < 10_ms) { + if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, imu, angular_velocity_uncalibrated, + angular_acceleration_uncalibrated)) { - while (_sensor_sub.update(&sensor_data)) { - if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) { - - if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { - _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; - } - - const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), - 0.00002f, 0.02f); - _timestamp_sample_last = sensor_data.timestamp_sample; - - Vector3f angular_velocity_uncalibrated; - Vector3f angular_acceleration_uncalibrated; - - float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z}; - - for (int axis = 0; axis < 3; axis++) { - // copy sensor sample to float array for filtering - float data[1] {raw_data_array[axis]}; - - // save last filtered sample - angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data); - angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data); - } - - // Publish - if (!_sensor_sub.updated()) { - if (CalibrateAndPublish(sensor_data.timestamp_sample, - angular_velocity_uncalibrated, - angular_acceleration_uncalibrated)) { - - perf_end(_cycle_perf); - return; - } - } + perf_end(_cycle_perf); + return; } } } - // force reselection on timeout - if (time_now_us > _last_publish + 500_ms) { - SensorSelectionUpdate(true); + perf_end(_cycle_perf); +} + +void VehicleAngularVelocity::updateSensorGyro(IMU &imu, const sensor_gyro_s &sensor_data) +{ + perf_begin(_cycle_perf); + + const hrt_abstime time_now_us = hrt_absolute_time(); + + update(imu); + + UpdateDynamicNotchEscRpm(time_now_us); + UpdateDynamicNotchFFT(time_now_us); + + // process all outstanding messages + + if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) { + + if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { + _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; + } + + const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), + 0.00002f, 0.02f); + _timestamp_sample_last = sensor_data.timestamp_sample; + + Vector3f angular_velocity_uncalibrated; + Vector3f angular_acceleration_uncalibrated; + + float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z}; + + for (int axis = 0; axis < 3; axis++) { + // copy sensor sample to float array for filtering + float data[1] {raw_data_array[axis]}; + + // save last filtered sample + angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data); + angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data); + } + + // Publish + //if (!_sensor_sub.updated()) { + if (true) { + if (CalibrateAndPublish(sensor_data.timestamp_sample, imu, angular_velocity_uncalibrated, + angular_acceleration_uncalibrated)) { + + perf_end(_cycle_perf); + return; + } + } } perf_end(_cycle_perf); } bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sample, + const IMU &imu, const Vector3f &angular_velocity_uncalibrated, const Vector3f &angular_acceleration_uncalibrated) { @@ -922,11 +724,11 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sa angular_velocity.timestamp_sample = timestamp_sample; // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias - _angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias; + _angular_velocity = imu.gyro.calibration.Correct(angular_velocity_uncalibrated) - imu.gyro.estimated_bias; _angular_velocity.copyTo(angular_velocity.xyz); // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation - _angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated; + _angular_acceleration = imu.gyro.calibration.rotation() * angular_acceleration_uncalibrated; _angular_acceleration.copyTo(angular_velocity.xyz_derivative); angular_velocity.timestamp = hrt_absolute_time(); @@ -944,16 +746,11 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sa void VehicleAngularVelocity::PrintStatus() { - PX4_INFO_RAW("[vehicle_angular_velocity] selected sensor: %" PRIu32 - ", rate: %.1f Hz %s, estimated bias: [%.5f %.5f %.5f]\n", - _calibration.device_id(), (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "", - (double)_bias(0), (double)_bias(1), (double)_bias(2)); - - _calibration.PrintStatus(); + PX4_INFO_RAW("[vehicle_angular_velocity] selected sensor: %" PRIu32 ", rate: %.1f Hz %s\n", + _selected_sensor_device_id, (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : ""); perf_print_counter(_cycle_perf); perf_print_counter(_filter_reset_perf); - perf_print_counter(_selection_changed_perf); #if !defined(CONSTRAINED_FLASH) perf_print_counter(_dynamic_notch_filter_esc_rpm_disable_perf); perf_print_counter(_dynamic_notch_filter_esc_rpm_init_perf); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_imu/VehicleAngularVelocity.hpp similarity index 75% rename from src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp rename to src/modules/sensors/vehicle_imu/VehicleAngularVelocity.hpp index 8aae8a3c03..6d5bf30b10 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleAngularVelocity.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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,28 +33,24 @@ #pragma once +#include "IMU.hpp" + #include -#include #include #include #include #include #include +#include #include #include #include -#include #include #include -#include #include -#include -#include -#include #include #include -#include -#include +#include #include using namespace time_literals; @@ -62,61 +58,49 @@ using namespace time_literals; namespace sensors { -class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem +class VehicleAngularVelocity : public ModuleParams { public: VehicleAngularVelocity(); ~VehicleAngularVelocity() override; void PrintStatus(); - bool Start(); - void Stop(); + + void ParametersUpdate(); + + // IMU& ? + void updateSensorGyro(IMU &imu, const sensor_gyro_s &sensor_data); + void updateSensorImuFifo(IMU &imu, const sensor_imu_fifo_s &sensor_fifo_data); private: - void Run() override; - bool CalibrateAndPublish(const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity_uncalibrated, - const matrix::Vector3f &angular_acceleration_uncalibrated); + bool CalibrateAndPublish(const hrt_abstime ×tamp_sample, const IMU &imu, + const matrix::Vector3f &angular_velocity_uncalibrated, const matrix::Vector3f &angular_acceleration_uncalibrated); inline float FilterAngularVelocity(int axis, float data[], int N = 1); inline float FilterAngularAcceleration(int axis, float inverse_dt_s, float data[], int N = 1); void DisableDynamicNotchEscRpm(); void DisableDynamicNotchFFT(); - void ParametersUpdate(bool force = false); - void ResetFilters(const hrt_abstime &time_now_us); - void SensorBiasUpdate(bool force = false); - bool SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force = false); + void ResetFilters(const hrt_abstime &time_now_us, const IMU &imu); void UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force = false); void UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force = false); - bool UpdateSampleRate(); + bool UpdateSampleRate(float sample_rate_hz, float publish_rate_hz); + + void update(IMU &imu); // scaled appropriately for current sensor - matrix::Vector3f GetResetAngularVelocity() const; - matrix::Vector3f GetResetAngularAcceleration() const; + matrix::Vector3f GetResetAngularVelocity(const IMU &imu) const; + matrix::Vector3f GetResetAngularAcceleration(const IMU &imu) const; - static constexpr int MAX_SENSOR_COUNT = 4; + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; - uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; - - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; #if !defined(CONSTRAINED_FLASH) uORB::Subscription _esc_status_sub {ORB_ID(esc_status)}; uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; #endif // !CONSTRAINED_FLASH - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; - uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; - - calibration::Gyroscope _calibration{}; - - matrix::Vector3f _bias{}; - matrix::Vector3f _angular_velocity{}; matrix::Vector3f _angular_acceleration{}; @@ -140,7 +124,7 @@ private: FFT = 2, }; - static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; + static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 1_s; // ESC RPM static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]); @@ -179,7 +163,6 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro filter")}; perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")}; - perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")}; DEFINE_PARAMETERS( #if !defined(CONSTRAINED_FLASH) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 8ca2464b5e..bf1659ba10 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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,57 +33,79 @@ #include "VehicleIMU.hpp" -#include -#include +#include +#include #include -#include -#include +#include + +#include using namespace matrix; -using math::constrain; - namespace sensors { -VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, config), - _sensor_accel_sub(ORB_ID(sensor_accel), accel_index), - _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), - _instance(instance) +static constexpr bool clipping(const int16_t sample) { return (sample == INT16_MIN) || (sample == INT16_MAX); } + +static constexpr int clipping(const int16_t samples[], int length) { - _imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); + int clip_count = 0; - _accel_integrator.set_reset_interval(_imu_integration_interval_us); - _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); + for (int n = 0; n < length; n++) { + if (clipping(samples[n])) { + clip_count++; + } + } - _gyro_integrator.set_reset_interval(_imu_integration_interval_us); - _gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH); + return clip_count; +} -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - // currently with lockstep every raw sample needs a corresponding vehicle_imu publication - _sensor_gyro_sub.set_required_updates(1); -#else - // schedule conservatively until the actual accel & gyro rates are known - _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); -#endif +static constexpr int32_t sum(const int16_t samples[], int length) +{ + int32_t sum = 0; - // advertise immediately to ensure consistent ordering - _vehicle_imu_pub.advertise(); - _vehicle_imu_status_pub.advertise(); + for (int n = 0; n < length; n++) { + sum += samples[n]; + } + + return sum; +} + +static matrix::Vector3f AverageFifoAccel(const sensor_imu_fifo_s &sensor_imu_fifo) +{ + const float scale = sensor_imu_fifo.accel_scale / sensor_imu_fifo.samples; + + return matrix::Vector3f{ + scale * sum(sensor_imu_fifo.accel_x, sensor_imu_fifo.samples), + scale * sum(sensor_imu_fifo.accel_y, sensor_imu_fifo.samples), + scale * sum(sensor_imu_fifo.accel_z, sensor_imu_fifo.samples) + }; +} + +static matrix::Vector3f AverageFifoGyro(const sensor_imu_fifo_s &sensor_imu_fifo) +{ + const float scale = sensor_imu_fifo.gyro_scale / sensor_imu_fifo.samples; + + return matrix::Vector3f{ + scale * sum(sensor_imu_fifo.gyro_x, sensor_imu_fifo.samples), + scale * sum(sensor_imu_fifo.gyro_y, sensor_imu_fifo.samples), + scale * sum(sensor_imu_fifo.gyro_z, sensor_imu_fifo.samples) + }; +} + +VehicleIMU::VehicleIMU() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ } VehicleIMU::~VehicleIMU() { Stop(); - perf_free(_accel_generation_gap_perf); - perf_free(_gyro_generation_gap_perf); - - _vehicle_imu_pub.unadvertise(); - _vehicle_imu_status_pub.unadvertise(); + perf_free(_cycle_perf); + perf_free(_selection_changed_perf); } bool VehicleIMU::Start() @@ -91,94 +113,374 @@ bool VehicleIMU::Start() // force initial updates ParametersUpdate(true); - _sensor_gyro_sub.registerCallback(); + // sensor_accel + for (auto &sub : _sensor_accel_subs) { + sub.set_required_updates(sensor_accel_s::ORB_QUEUE_LENGTH); + sub.registerCallback(); + } + + // sensor_gyro + for (auto &sub : _sensor_gyro_subs) { + sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH); + sub.registerCallback(); + } + + // sensor_imu_fifo + for (auto &sub : _sensor_imu_fifo_subs) { + sub.set_required_updates(sensor_imu_fifo_s::ORB_QUEUE_LENGTH); + sub.registerCallback(); + } + ScheduleNow(); + return true; } void VehicleIMU::Stop() { // clear all registered callbacks - _sensor_gyro_sub.unregisterCallback(); + + // sensor_accel + for (auto &sub : _sensor_accel_subs) { + sub.unregisterCallback(); + } + + // sensor_gyro + for (auto &sub : _sensor_gyro_subs) { + sub.unregisterCallback(); + } + + // sensor_imu_fifo + for (auto &sub : _sensor_imu_fifo_subs) { + sub.unregisterCallback(); + } + + ScheduleClear(); Deinit(); } +void VehicleIMU::SensorBiasUpdate() +{ + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + //_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + + // TODO: + // find corresponding primary accel and gyro device id + } + } + + for (int est_bias_instance = 0; est_bias_instance < _estimator_sensor_bias_subs.size(); est_bias_instance++) { + estimator_sensor_bias_s estimator_sensor_bias; + + if (_estimator_sensor_bias_subs[est_bias_instance].update(&estimator_sensor_bias)) { + + for (auto &imu : _imus) { + if (estimator_sensor_bias.accel_bias_valid + && (estimator_sensor_bias.accel_device_id != 0) + && (estimator_sensor_bias.accel_device_id == imu.accel.calibration.device_id()) + ) { + const Vector3f bias{estimator_sensor_bias.accel_bias}; + const Vector3f bias_var{estimator_sensor_bias.accel_bias_variance}; + + if ((bias_var.norm() < imu.accel.estimated_bias_variance.norm()) + || !imu.accel.estimated_bias_variance.longerThan(FLT_EPSILON)) { + + imu.accel.estimated_bias = bias; + imu.accel.estimated_bias_variance = bias_var; + } + + // TODO: store all + imu.accel.learned_calibration[0].instance = est_bias_instance; + imu.accel.learned_calibration[0].offset = imu.accel.calibration.BiasCorrectedSensorOffset(bias); + imu.accel.learned_calibration[0].bias_variance = bias_var; + imu.accel.learned_calibration[0].valid = true; + _accel_cal_available = true; + } + + if (estimator_sensor_bias.gyro_bias_valid + && (estimator_sensor_bias.gyro_device_id != 0) + && (estimator_sensor_bias.gyro_device_id == imu.gyro.calibration.device_id()) + ) { + const Vector3f bias{estimator_sensor_bias.gyro_bias}; + const Vector3f bias_var{estimator_sensor_bias.gyro_bias_variance}; + + if ((bias_var.norm() < imu.gyro.estimated_bias_variance.norm()) + || !imu.gyro.estimated_bias_variance.longerThan(FLT_EPSILON)) { + + imu.gyro.estimated_bias = bias; + imu.gyro.estimated_bias_variance = bias_var; + } + + // TODO: store all + imu.gyro.learned_calibration[0].instance = est_bias_instance; + imu.gyro.learned_calibration[0].offset = imu.gyro.calibration.BiasCorrectedSensorOffset(bias); + imu.gyro.learned_calibration[0].bias_variance = bias_var; + imu.gyro.learned_calibration[0].valid = true; + _gyro_cal_available = true; + } + + } + } + } +} + +bool VehicleIMU::SensorSelectionUpdate(bool force) +{ + // find highest priority + int highest_priority = 0; + int highest_priority_index = -1; + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + const IMU &imu = _imus[i]; + + if (imu.accel.calibration.enabled() && (hrt_elapsed_time(&imu.time_last_valid) < 1_s)) { + if (imu.accel.calibration.priority() > highest_priority) { + highest_priority = imu.accel.calibration.priority(); + highest_priority_index = i; + } + } + } + + // timeout + + // clipping and other health checks + + + // multi-EKF respect selected IMU +#if 0 + + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + //_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + + // TODO: + // find corresponding primary accel and gyro device id + } + } + +#endif + + + + // TODO: + // - timeout based on expected sensor publication rate + + + + if (highest_priority_index >= 0) { + + if (_selected_imu_index != highest_priority_index) { + + for (auto &imu : _imus) { + imu.primary = false; + } + + _selected_imu_index = highest_priority_index; + _selected_accel_device_id = _imus[_selected_imu_index].accel.calibration.device_id(); + _selected_gyro_device_id = _imus[_selected_imu_index].gyro.calibration.device_id(); + + _imus[_selected_imu_index].primary = true; + + PX4_INFO("selected primary IMU %d", _selected_imu_index); + + // TODO: on primary change update callback scheduling + // sub.set_required_updates(1); + + return true; + } + } + + + return false; +} + bool VehicleIMU::ParametersUpdate(bool force) { - bool updated = false; - // Check if parameters have changed if (_parameter_update_sub.updated() || force) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); - const auto imu_integ_rate_prev = _param_imu_integ_rate.get(); + + // if calibration changed clear anything learned in flight + for (auto &imu : _imus) { + const auto accel_calibration_count = imu.accel.calibration.calibration_count(); + const auto gyro_calibration_count = imu.gyro.calibration.calibration_count(); + + imu.accel.calibration.ParametersUpdate(); + imu.gyro.calibration.ParametersUpdate(); + + if (accel_calibration_count != imu.accel.calibration.calibration_count()) { + // if calibration changed reset any existing learned calibration + _accel_cal_available = false; + _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; + + for (auto &learned_cal : imu.accel.learned_calibration) { + for (auto &learned_cal : imu.accel.learned_calibration) { + learned_cal = {}; + } + } + } + + if (gyro_calibration_count != imu.gyro.calibration.calibration_count()) { + // if calibration changed reset any existing learned calibration + _gyro_cal_available = false; + _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; + + for (auto &learned_cal : imu.gyro.learned_calibration) { + learned_cal = {}; + } + } + + } + + + + + // 1. mark all existing sensor calibrations active even if sensor is missing + // this preserves the calibration in the event of a parameter export while the sensor is missing + // 2. ensure calibration slots are active for the number of sensors currently available + // this to done to eliminate differences in the active set of parameters before and after sensor calibration + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + // sensor_accel + { + const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); + + if (device_id_accel != 0) { + calibration::Accelerometer accel_cal(device_id_accel); + } + + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + + if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { + calibration::Accelerometer cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } + } + + + // sensor_gyro + { + const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); + + if (device_id_gyro != 0) { + calibration::Gyroscope gyro_cal(device_id_gyro); + } + + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { + calibration::Gyroscope cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } + } + } + + + + + + // TODO: IMU priority changes +#if 0 + + if (accel_cal_index >= 0) { + // found matching CAL_ACCx_PRIO + int32_t accel_priority_old = _accel.priority_configured[uorb_index]; + + _accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index); + + if (accel_priority_old != _accel.priority_configured[uorb_index]) { + if (_accel.priority_configured[uorb_index] == 0) { + // disabled + _accel.priority[uorb_index] = 0; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old; + _accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast(1), + static_cast(100)); + } + } + } + + if (gyro_cal_index >= 0) { + // found matching CAL_GYROx_PRIO + int32_t gyro_priority_old = _gyro.priority_configured[uorb_index]; + + _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index); + + if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { + if (_gyro.priority_configured[uorb_index] == 0) { + // disabled + _gyro.priority[uorb_index] = 0; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old; + _gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, static_cast(1), + static_cast(100)); + } + } + } + +#endif + updateParams(); - updated = true; + _vehicle_acceleration.ParametersUpdate(); + _vehicle_angular_velocity.ParametersUpdate(); - const auto accel_calibration_count = _accel_calibration.calibration_count(); - const auto gyro_calibration_count = _gyro_calibration.calibration_count(); - _accel_calibration.ParametersUpdate(); - _gyro_calibration.ParametersUpdate(); - - if (accel_calibration_count != _accel_calibration.calibration_count()) { - // if calibration changed reset any existing learned calibration - _accel_cal_available = false; - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - - for (auto &learned_cal : _accel_learned_calibration) { - learned_cal = {}; - } + for (auto &imu : _imus) { + imu.accel.calibration.ParametersUpdate(); + imu.gyro.calibration.ParametersUpdate(); } - if (gyro_calibration_count != _gyro_calibration.calibration_count()) { - // if calibration changed reset any existing learned calibration - _gyro_cal_available = false; - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - - for (auto &learned_cal : _gyro_learned_calibration) { - learned_cal = {}; - } + // IMU_GYRO_RATEMAX + if (_param_imu_gyro_ratemax.get() <= 0) { + const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get(); + _param_imu_gyro_ratemax.reset(); + PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax, + _param_imu_gyro_ratemax.get()); } - // constrain IMU integration time 1-20 milliseconds (50-1000 Hz) - const int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), (int32_t)50, (int32_t)1000); + // constrain IMU_GYRO_RATEMAX 50-10,000 Hz + const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000); - if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { - PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz); - _param_imu_integ_rate.set(imu_integration_rate_hz); - _param_imu_integ_rate.commit_no_notification(); + if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) { + PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax); + _param_imu_gyro_ratemax.set(imu_gyro_ratemax); + _param_imu_gyro_ratemax.commit_no_notification(); } - _imu_integration_interval_us = 1e6f / imu_integration_rate_hz; - if (_param_imu_integ_rate.get() != imu_integ_rate_prev) { - // force update - _update_integrator_config = true; - } + + _last_calibration_update = hrt_absolute_time(); + + + return true; } - return updated; + return false; } void VehicleIMU::Run() { - const hrt_abstime now_us = hrt_absolute_time(); - - const bool parameters_updated = ParametersUpdate(); - - if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { - _sensor_gyro_sub.unregisterCallback(); - ScheduleDelayed(1_s); - return; - } + perf_begin(_cycle_perf); // backup schedule - ScheduleDelayed(_backup_schedule_timeout_us); + ScheduleDelayed(1.5e6 / _param_imu_gyro_ratemax.get()); // backup 150% of expected/desired interval // check vehicle status for changes to armed state if (_vehicle_control_mode_sub.updated()) { @@ -189,621 +491,1060 @@ void VehicleIMU::Run() } } - // reset data gap monitor - _data_gap = false; + const hrt_abstime time_now_us = hrt_absolute_time(); - while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) { - bool updated = false; + const bool parameters_updated = ParametersUpdate(); - bool consume_all_gyro = !_intervals_configured || _data_gap; + SensorSelectionUpdate(); + SensorBiasUpdate(); - // monitor scheduling latency and force catch up with latest gyro if falling behind - if (_intervals_configured && !consume_all_gyro && _sensor_gyro_sub.updated() - && _gyro_publish_latency_mean_us.valid() && _gyro_mean_interval_us.valid() - && (_gyro_publish_latency_mean_us.mean() > _gyro_mean_interval_us.mean()) + for (int sensor_instance = 0; sensor_instance < MAX_SENSOR_COUNT; sensor_instance++) { + UpdateSensorImuFifo(sensor_instance); + UpdateSensorAccel(sensor_instance); + UpdateSensorGyro(sensor_instance); + } + + // TODO: IMU at rest (across all enabled) + // - if still don't reset gyro/accel welford mean, instead use it for calibration + // - delete gyro_calibraiton module + + + // accel integration on raw int16 data + + // sensor_imu_fifo -> imu instance + // sensor_accel + sensor_gyro -> imu instance + // sensors_combined, sensor selection + + + PublishSensorsStatusIMU(); + + + + // TODO: IMU selection + // - timeouts (checked all at once) + // - errors (sensor_accel + sensor_gyro) + // - stuck data + // - sensor variance + // - variance between sensors + // - publish sensor_selection_s + + + + + // TODO: calibration + // - gyro if still + // - post flight estimated + + // TODO: non-FIFO case + + + if (_param_sens_imu_autocal.get() && !parameters_updated) { + + updateGyroCalibration(); + + // TODO: accel uncalibrated + // TODO: gyro uncalibrated + bool accel_uncalibrated = false; // !_accel_calibration.calibrated() + bool gyro_uncalibrated = false; // !_gyro_calibration.calibrated() + + if ((_armed || accel_uncalibrated || gyro_uncalibrated) + && (time_now_us > _in_flight_calibration_check_timestamp_last + 1_s)) { + + //SensorCalibrationUpdate(); + _in_flight_calibration_check_timestamp_last = time_now_us; + + } else if (!_armed) { + //SensorCalibrationSaveAccel(); + //SensorCalibrationSaveGyro(); + } + } + + perf_end(_cycle_perf); +} + +int8_t VehicleIMU::findAccelInstance(uint32_t device_id) +{ + if (device_id == 0) { + return -1; + } + + // look for existing matching + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_imus[i].accel.device_id == device_id) { + return i; + } + } + + // look for empty slot + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_imus[i].accel.device_id == 0) { + _imus[i].accel.device_id = device_id; + return i; + } + } + + return -1; +} + +void VehicleIMU::UpdateSensorImuFifo(uint8_t sensor_instance) +{ + // sensor_imu_fifo + auto &sub = _sensor_imu_fifo_subs[sensor_instance]; + + while (sub.updated()) { + unsigned last_generation = sub.get_last_generation(); + sensor_imu_fifo_s sensor_imu_fifo; + sub.copy(&sensor_imu_fifo); + + int8_t imu_instance = findAccelInstance(sensor_imu_fifo.device_id); + + if (imu_instance == -1) { + return; + } + + IMU &imu = _imus[imu_instance]; + + // publication interval + // sensor output data rate or interval + if (sub.get_last_generation() == last_generation + 1) { + + if ((sensor_imu_fifo.timestamp_sample > imu.accel.timestamp_sample_last) + && (sensor_imu_fifo.timestamp_sample > imu.gyro.timestamp_sample_last) + ) { + + const float interval_us = sensor_imu_fifo.timestamp_sample - imu.accel.timestamp_sample_last; + const float sample_interval_us = interval_us / sensor_imu_fifo.samples; + + imu.accel.mean_publish_interval_us.update(interval_us); + imu.accel.mean_sample_interval_us.update(sample_interval_us); + + imu.gyro.mean_publish_interval_us.update(interval_us); + imu.gyro.mean_sample_interval_us.update(sample_interval_us); + + // check and update sensor rate + if (imu.gyro.mean_publish_interval_us.valid() && + (imu.gyro.mean_publish_interval_us.count() > 1000 || imu.gyro.mean_publish_interval_us.standard_deviation() < 100) + ) { + + // determine number of sensor samples that will get closest to the desired integration interval + const float imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); + const float pub_interval_avg_us = imu.gyro.mean_publish_interval_us.mean(); + + const uint8_t imu_publications = roundf(imu_integration_interval_us / pub_interval_avg_us); + + const float integration_interval_us = roundf(imu_publications * pub_interval_avg_us); + + // TODO: variance + if ((static_cast(imu.gyro.integrator.reset_interval_us()) - integration_interval_us) > + imu.gyro.mean_publish_interval_us.standard_deviation() + // || primary_changed + ) { + + const auto accel_reset_samples_prev = imu.accel.integrator.get_reset_samples(); + const auto gyro_reset_samples_prev = imu.gyro.integrator.get_reset_samples(); + + imu.gyro.integrator.set_reset_interval(integration_interval_us); + imu.accel.integrator.set_reset_interval(integration_interval_us / 2.f); + + // number of samples per publication + int interval_samples = roundf(integration_interval_us / imu.gyro.mean_sample_interval_us.mean()); + imu.gyro.integrator.set_reset_samples(math::max(interval_samples, 1)); + imu.accel.integrator.set_reset_samples(1); + + if (accel_reset_samples_prev != imu.accel.integrator.get_reset_samples() + || gyro_reset_samples_prev != imu.gyro.integrator.get_reset_samples() + || (static_cast(imu.gyro.integrator.reset_interval_us()) - integration_interval_us > 100) + ) { + + if (imu.primary) { + sub.set_required_updates(1); + + } else { + // avg samples per publication + float samples_per_pub = imu.gyro.mean_publish_interval_us.mean() / imu.gyro.mean_sample_interval_us.mean(); + uint8_t pubs_per_integrator_reset = floorf(imu.gyro.integrator.get_reset_samples() / samples_per_pub); + + sub.set_required_updates(math::constrain(pubs_per_integrator_reset, (uint8_t)1, sensor_imu_fifo_s::ORB_QUEUE_LENGTH)); + } + + sub.registerCallback(); + + imu.accel.interval_configured = true; + imu.gyro.interval_configured = true; + + PX4_INFO("IMU FIFO (%" PRIu32 "), publish interval: %.1f us (STD:%.1f us), integrator: %.1f us, %d samples", + imu.gyro.calibration.device_id(), + (double)imu.gyro.mean_publish_interval_us.mean(), (double)imu.gyro.mean_publish_interval_us.standard_deviation(), + (double)imu.gyro.integrator.reset_interval_us(), imu.gyro.integrator.get_reset_samples() + ); + } + + } + } + } + } + + imu.accel.timestamp_sample_last = sensor_imu_fifo.timestamp_sample; + imu.accel.calibration.set_device_id(sensor_imu_fifo.device_id); + imu.accel.calibration.SensorCorrectionsUpdate(); + imu.accel.error_count = sensor_imu_fifo.error_count; + + imu.gyro.timestamp_sample_last = sensor_imu_fifo.timestamp_sample; + imu.gyro.calibration.set_device_id(sensor_imu_fifo.device_id); + imu.gyro.calibration.SensorCorrectionsUpdate(); + imu.gyro.error_count = sensor_imu_fifo.error_count; + + // temperature average + if (PX4_ISFINITE(sensor_imu_fifo.temperature)) { + imu.accel.temperature.update(sensor_imu_fifo.temperature); + imu.gyro.temperature.update(sensor_imu_fifo.temperature); + } + + if (!imu.accel.calibration.enabled() || !imu.gyro.calibration.enabled()) { + return; + } + + const int N = sensor_imu_fifo.samples; + + const float dt_s = sensor_imu_fifo.dt * 1e-6f; + //const float inverse_dt_s = 1e6f / sensor_imu_fifo.dt; + + // integrate accel + for (int n = 0; n < N; n++) { + const Vector3f accel_raw{ + static_cast(sensor_imu_fifo.accel_x[n]) *sensor_imu_fifo.accel_scale, + static_cast(sensor_imu_fifo.accel_y[n]) *sensor_imu_fifo.accel_scale, + static_cast(sensor_imu_fifo.accel_z[n]) *sensor_imu_fifo.accel_scale}; + + imu.accel.integrator.put(accel_raw, dt_s); + } + + const Vector3f accel_raw_avg{AverageFifoAccel(sensor_imu_fifo)}; + imu.accel.raw_mean.update(accel_raw_avg - imu.accel.calibration.thermal_offset()); + + // integrate accel + + // check for scale change + if (fabsf(sensor_imu_fifo.accel_scale - imu.accel.fifo_scale) > FLT_EPSILON) { + // rescale last sample on scale change + const float rescale = imu.accel.fifo_scale / sensor_imu_fifo.accel_scale; + + imu.accel.last_raw_sample[0] = roundf(imu.accel.last_raw_sample[0] * rescale); + imu.accel.last_raw_sample[1] = roundf(imu.accel.last_raw_sample[1] * rescale); + imu.accel.last_raw_sample[2] = roundf(imu.accel.last_raw_sample[2] * rescale); + + imu.accel.fifo_scale = sensor_imu_fifo.accel_scale; + } + + // trapezoidal integration (equally spaced) + const float scale = dt_s * sensor_imu_fifo.accel_scale; + imu.accel.integral(0) += (0.5f * (imu.accel.last_raw_sample[0] + sensor_imu_fifo.accel_x[N - 2]) + + sum(sensor_imu_fifo.accel_x, N - 1)) * scale; + imu.accel.last_raw_sample[0] = sensor_imu_fifo.accel_x[N - 1]; + + imu.accel.integral(1) += (0.5f * (imu.accel.last_raw_sample[1] + sensor_imu_fifo.accel_y[N - 2]) + + sum(sensor_imu_fifo.accel_y, N - 1)) * scale; + imu.accel.last_raw_sample[1] = sensor_imu_fifo.accel_y[N - 1]; + + imu.accel.integral(2) += (0.5f * (imu.accel.last_raw_sample[2] + sensor_imu_fifo.accel_z[N - 2]) + + sum(sensor_imu_fifo.accel_z, N - 1)) * scale; + imu.accel.last_raw_sample[2] = sensor_imu_fifo.accel_z[N - 1]; + + // accel clipping + { + const Vector3f clip_counter{ + (float)clipping(sensor_imu_fifo.accel_x, sensor_imu_fifo.samples), + (float)clipping(sensor_imu_fifo.accel_y, sensor_imu_fifo.samples), + (float)clipping(sensor_imu_fifo.accel_z, sensor_imu_fifo.samples)}; + + if (clip_counter(0) > 0 || clip_counter(1) > 0 || clip_counter(2) > 0) { + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{imu.accel.calibration.rotation() *clip_counter}; + + // round to get reasonble clip counts per axis (after board rotation) + const uint8_t clip_x = roundf(fabsf(clipping(0))); + + if (clip_x > 0) { + imu.accel.clipping_total[0] += clip_x; + imu.accel.clipping_flags |= vehicle_imu_s::CLIPPING_X; + } + + const uint8_t clip_y = roundf(fabsf(clipping(1))); + + if (clip_y > 0) { + imu.accel.clipping_total[1] += clip_y; + imu.accel.clipping_flags |= vehicle_imu_s::CLIPPING_Y; + } + + const uint8_t clip_z = roundf(fabsf(clipping(2))); + + if (clip_z > 0) { + imu.accel.clipping_total[2] += clip_z; + imu.accel.clipping_flags |= vehicle_imu_s::CLIPPING_Z; + } + } + } + + // integrate gyro + for (int n = 0; n < N; n++) { + const Vector3f gyro_raw{ + static_cast(sensor_imu_fifo.gyro_x[n]) *sensor_imu_fifo.gyro_scale, + static_cast(sensor_imu_fifo.gyro_y[n]) *sensor_imu_fifo.gyro_scale, + static_cast(sensor_imu_fifo.gyro_z[n]) *sensor_imu_fifo.gyro_scale}; + + imu.gyro.integrator.put(gyro_raw, dt_s); + } + + imu.gyro.raw_mean.update(AverageFifoGyro(sensor_imu_fifo) - imu.gyro.calibration.thermal_offset()); + + // gyro clipping + { + const Vector3f clip_counter{ + (float)clipping(sensor_imu_fifo.gyro_x, sensor_imu_fifo.samples), + (float)clipping(sensor_imu_fifo.gyro_y, sensor_imu_fifo.samples), + (float)clipping(sensor_imu_fifo.gyro_z, sensor_imu_fifo.samples)}; + + + if (clip_counter(0) > 0 || clip_counter(1) > 0 || clip_counter(2) > 0) { + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{imu.gyro.calibration.rotation() *clip_counter}; + + // round to get reasonble clip counts per axis (after board rotation) + const uint8_t clip_x = roundf(fabsf(clipping(0))); + + if (clip_x > 0) { + imu.gyro.clipping_total[0] += clip_x; + imu.gyro.clipping_flags |= vehicle_imu_s::CLIPPING_X; + } + + const uint8_t clip_y = roundf(fabsf(clipping(1))); + + if (clip_y > 0) { + imu.gyro.clipping_total[1] += clip_y; + imu.gyro.clipping_flags |= vehicle_imu_s::CLIPPING_Y; + } + + const uint8_t clip_z = roundf(fabsf(clipping(2))); + + if (clip_z > 0) { + imu.gyro.clipping_total[2] += clip_z; + imu.gyro.clipping_flags |= vehicle_imu_s::CLIPPING_Z; + } + } + } + + if (imu.accel.integrator.integral_ready() && imu.gyro.integrator.integral_ready()) { + + + // Vector3f delta_velocity{imu.accel.integral * imu.accel.fifo_scale * dt_s}; + // imu.accel.integral.zero(); + // uint16_t delta_velocity_dt = delta_angle_dt; + + // TODO: rescale if changed? + + + + Vector3f delta_angle; + uint16_t delta_angle_dt; + Vector3f delta_velocity; + uint16_t delta_velocity_dt; + + const Vector3f accumulated_coning_corrections = imu.gyro.integrator.accumulated_coning_corrections(); + + if (imu.accel.integrator.reset(delta_velocity, delta_velocity_dt) + && imu.gyro.integrator.reset(delta_angle, delta_angle_dt) + ) { + + // TODO: + // delta_velocity = imu.accel.integral; // TODO + + PublishImu(imu, delta_angle, delta_angle_dt, delta_velocity, delta_velocity_dt); + } + + imu.accel.integral.zero(); + } + + // TODO: call to filter data + if (imu.primary) { + + if (imu.accel.interval_configured && imu.accel.mean_publish_interval_us.valid()) { + float sample_rate_hz = 1e6f / imu.accel.mean_publish_interval_us.mean(); + _vehicle_acceleration.updateAccel(imu, accel_raw_avg, sample_rate_hz); + } + + if (imu.gyro.interval_configured) { + + // TODO: generic update? + //_vehicle_angular_velocity.update(imu)? + // for (int axis = 0; axis < 3; axis++) { + // _vehicle_angular_velocity.updateAxis(axis, raw_data[], length)? + // } + + _vehicle_angular_velocity.updateSensorImuFifo(imu, sensor_imu_fifo); + + // TODO: or do it axis by axis? + // float raw_data_array[] + + // update per axis? + // publish when + if (!sub.updated()) { + // _vehicle_angular_velocity.publish()? + } + } + } + } +} + +void VehicleIMU::UpdateSensorAccel(uint8_t sensor_instance) +{ + // sensor_accel + auto &sub = _sensor_accel_subs[sensor_instance]; + + while (sub.updated()) { + unsigned last_generation = sub.get_last_generation(); + sensor_accel_s sensor_accel; + sub.copy(&sensor_accel); + + int8_t imu_instance = findAccelInstance(sensor_accel.device_id); + + if (imu_instance == -1) { + return; + } + + IMU &imu = _imus[imu_instance]; + + const float interval_us = sensor_accel.timestamp_sample - imu.accel.timestamp_sample_last; + + // publication interval + // sensor output data rate or interval + if (sub.get_last_generation() == last_generation + 1) { + + if (sensor_accel.timestamp_sample > imu.accel.timestamp_sample_last) { + + const float sample_interval_us = interval_us; + + imu.accel.mean_publish_interval_us.update(interval_us); + imu.accel.mean_sample_interval_us.update(sample_interval_us); + } + } + + const float dt_s = math::constrain(interval_us * 1e-6f, 0.0001f, 0.1f); + + imu.accel.timestamp_sample_last = sensor_accel.timestamp_sample; + imu.accel.calibration.set_device_id(sensor_accel.device_id); + imu.accel.calibration.SensorCorrectionsUpdate(); + imu.accel.error_count = sensor_accel.error_count; + + // temperature average + if (PX4_ISFINITE(sensor_accel.temperature)) { + imu.accel.temperature.update(sensor_accel.temperature); + } + + if (!imu.accel.calibration.enabled()) { + return; + } + + // integrate accel + const Vector3f accel_raw{sensor_accel.x, sensor_accel.y, sensor_accel.z}; + imu.accel.integrator.put(accel_raw, dt_s); + + imu.accel.raw_mean.update(accel_raw - imu.accel.calibration.thermal_offset()); + + // TODO: call to filter data + if (imu.primary) { + + if (imu.accel.interval_configured && imu.accel.mean_publish_interval_us.valid()) { + float sample_rate_hz = 1e6f / imu.accel.mean_publish_interval_us.mean(); + _vehicle_acceleration.updateAccel(imu, accel_raw, sample_rate_hz); + } + } + + // don't get ahead of gyro + if (imu.accel.timestamp_sample_last > imu.gyro.timestamp_sample_last + + 0.5f * imu.accel.mean_sample_interval_us.mean() ) { - PX4_DEBUG("%d gyro publish latency %.6f > %.6f us (mean interval)", - _instance, - (double)_gyro_publish_latency_mean_us.mean(), - (double)_gyro_mean_interval_us.mean() - ); + return; + } + } +} - consume_all_gyro = true; +void VehicleIMU::UpdateSensorGyro(uint8_t sensor_instance) +{ + // sensor_gyro + auto &sub = _sensor_gyro_subs[sensor_instance]; + + while (sub.updated()) { + unsigned last_generation = sub.get_last_generation(); + sensor_gyro_s sensor_gyro; + sub.copy(&sensor_gyro); + + int8_t imu_instance = findAccelInstance(sensor_gyro.device_id); + + if (imu_instance == -1) { + return; } - // update gyro until integrator ready and not falling behind - if (!_gyro_integrator.integral_ready() || consume_all_gyro) { - if (UpdateGyro()) { - updated = true; + IMU &imu = _imus[imu_instance]; + + // publication interval + // sensor output data rate or interval + if (sub.get_last_generation() == last_generation + 1) { + + if (sensor_gyro.timestamp_sample > imu.accel.timestamp_sample_last) { + + const float interval_us = sensor_gyro.timestamp_sample - imu.accel.timestamp_sample_last; + const float sample_interval_us = interval_us; + + imu.accel.mean_publish_interval_us.update(interval_us); + imu.accel.mean_sample_interval_us.update(sample_interval_us); + + imu.gyro.mean_publish_interval_us.update(interval_us); + imu.gyro.mean_sample_interval_us.update(sample_interval_us); + + // check and update sensor rate + if (imu.gyro.mean_publish_interval_us.valid() && + (imu.gyro.mean_publish_interval_us.count() > 1000 || imu.gyro.mean_publish_interval_us.standard_deviation() < 100) + ) { + + // determine number of sensor samples that will get closest to the desired integration interval + const float imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); + const float pub_interval_avg_us = imu.gyro.mean_publish_interval_us.mean(); + + const uint8_t imu_publications = roundf(imu_integration_interval_us / pub_interval_avg_us); + + const float integration_interval_us = roundf(imu_publications * pub_interval_avg_us); + + // TODO: variance + if ((static_cast(imu.gyro.integrator.reset_interval_us()) - integration_interval_us) > + imu.gyro.mean_publish_interval_us.standard_deviation() + // || primary_changed + ) { + + const auto accel_reset_samples_prev = imu.accel.integrator.get_reset_samples(); + const auto gyro_reset_samples_prev = imu.gyro.integrator.get_reset_samples(); + + imu.gyro.integrator.set_reset_interval(integration_interval_us); + imu.accel.integrator.set_reset_interval(integration_interval_us / 2.f); + + // number of samples per publication + int interval_samples = roundf(integration_interval_us / imu.gyro.mean_sample_interval_us.mean()); + imu.gyro.integrator.set_reset_samples(math::max(interval_samples, 1)); + imu.accel.integrator.set_reset_samples(1); + + if (accel_reset_samples_prev != imu.accel.integrator.get_reset_samples() + || gyro_reset_samples_prev != imu.gyro.integrator.get_reset_samples() + || (static_cast(imu.gyro.integrator.reset_interval_us()) - integration_interval_us > 100) + ) { + + if (imu.primary) { + sub.set_required_updates(1); + + } else { + // avg samples per publication + float samples_per_pub = imu.gyro.mean_publish_interval_us.mean() / imu.gyro.mean_sample_interval_us.mean(); + uint8_t pubs_per_integrator_reset = floorf(imu.gyro.integrator.get_reset_samples() / samples_per_pub); + + sub.set_required_updates(math::constrain(pubs_per_integrator_reset, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH)); + } + + sub.registerCallback(); + + imu.accel.interval_configured = true; + imu.gyro.interval_configured = true; + + PX4_INFO("IMU FIFO (%" PRIu32 "), publish interval: %.1f us (STD:%.1f us), integrator: %.1f us, %d samples", + imu.gyro.calibration.device_id(), + (double)imu.gyro.mean_publish_interval_us.mean(), (double)imu.gyro.mean_publish_interval_us.standard_deviation(), + (double)imu.gyro.integrator.reset_interval_us(), imu.gyro.integrator.get_reset_samples() + ); + } + + } + } + } + } + + const float dt_s = math::constrain((sensor_gyro.timestamp_sample - imu.gyro.timestamp_sample_last) * 1e-6f, 0.0001f, + 0.1f); + + imu.gyro.timestamp_sample_last = sensor_gyro.timestamp_sample; + imu.gyro.calibration.set_device_id(sensor_gyro.device_id); + imu.gyro.calibration.SensorCorrectionsUpdate(); + imu.gyro.error_count = sensor_gyro.error_count; + + // temperature average + if (PX4_ISFINITE(sensor_gyro.temperature)) { + imu.gyro.temperature.update(sensor_gyro.temperature); + } + + if (!imu.gyro.calibration.enabled()) { + return; + } + + // integrate gyro + const Vector3f gyro_raw{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}; + imu.gyro.integrator.put(gyro_raw, dt_s); + imu.gyro.raw_mean.update(gyro_raw - imu.gyro.calibration.thermal_offset()); + + if (imu.accel.integrator.integral_ready() && imu.gyro.integrator.integral_ready()) { + + // Vector3f delta_velocity{imu.accel.integral * imu.accel.fifo_scale * dt_s}; + // imu.accel.integral.zero(); + // uint16_t delta_velocity_dt = delta_angle_dt; + + // TODO: rescale if changed? + Vector3f delta_angle; + uint16_t delta_angle_dt; + Vector3f delta_velocity; + uint16_t delta_velocity_dt; + + const Vector3f accumulated_coning_corrections = imu.gyro.integrator.accumulated_coning_corrections(); + + if (imu.accel.integrator.reset(delta_velocity, delta_velocity_dt) + && imu.gyro.integrator.reset(delta_angle, delta_angle_dt) + ) { + + PublishImu(imu, delta_angle, delta_angle_dt, delta_velocity, delta_velocity_dt); + } + + imu.accel.integral.zero(); + } + + // TODO: call to filter data + if (imu.primary) { + + if (imu.gyro.interval_configured) { + + // TODO: generic update? + //_vehicle_angular_velocity.update(imu)? + // for (int axis = 0; axis < 3; axis++) { + // _vehicle_angular_velocity.updateAxis(axis, raw_data[], length)? + // } + + _vehicle_angular_velocity.updateSensorGyro(imu, sensor_gyro); + + // TODO: or do it axis by axis? + // float raw_data_array[] + + // update per axis? + // publish when + if (!sub.updated()) { + // _vehicle_angular_velocity.publish()? + } + } + } + } +} + +bool VehicleIMU::PublishImu(sensors::IMU &imu, + const Vector3f &delta_angle, const uint16_t delta_angle_dt, + const Vector3f &delta_velocity, const uint16_t delta_velocity_dt + ) +{ + bool updated = false; + + // TODO: fix + const Vector3f accumulated_coning_corrections = imu.gyro.integrator.accumulated_coning_corrections(); + + if (imu.accel.calibration.enabled() && imu.gyro.calibration.enabled()) { + + // delta angle: apply offsets, scale, and board rotation + const float gyro_dt_s = 1.e-6f * delta_angle_dt; + const Vector3f angular_velocity{imu.gyro.calibration.Correct(delta_angle / gyro_dt_s)}; + + // Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev) + imu.gyro.vibration_metric = 0.99f * imu.gyro.vibration_metric + 0.01f * Vector3f(angular_velocity - + imu.gyro.angular_velocity_prev).norm(); + imu.gyro.angular_velocity_prev = angular_velocity; + + const Vector3f delta_angle_corrected{angular_velocity * gyro_dt_s}; + + // accumulate delta angle coning corrections + imu.gyro.coning_norm_accum += accumulated_coning_corrections.norm() * gyro_dt_s; + imu.gyro.coning_norm_accum_total_time_s += gyro_dt_s; + + // delta velocity: apply offsets, scale, and board rotation + const float accel_dt_s = 1.e-6f * delta_velocity_dt; + const Vector3f acceleration{imu.accel.calibration.Correct(delta_velocity / accel_dt_s)}; + + // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev) + imu.accel.vibration_metric = 0.99f * imu.accel.vibration_metric + 0.01f * Vector3f(acceleration - + imu.accel.acceleration_prev).norm(); + imu.accel.acceleration_prev = acceleration; + + const Vector3f delta_velocity_corrected{acceleration * accel_dt_s}; + + + // at rest determination + static constexpr float GYRO_VIBE_METRIC_MAX = 0.02f; // gyro_vibration_metric * dt * 4.0e4f > is_moving_scaler) + static constexpr float ACCEL_VIBE_METRIC_MAX = 1.2f; // accel_vibration_metric * dt * 2.1e2f > is_moving_scaler + static constexpr float GYRO_NORM_MAX = math::radians(10.f); + + const float acceleration_norm = acceleration.norm(); + const float angular_velocity_norm = angular_velocity.norm(); + + if ((imu.gyro.vibration_metric > GYRO_VIBE_METRIC_MAX) + || (imu.accel.vibration_metric > ACCEL_VIBE_METRIC_MAX) + || (acceleration_norm < 0.5f * CONSTANTS_ONE_G) + || (acceleration_norm > 1.5f * CONSTANTS_ONE_G) + || (angular_velocity_norm > GYRO_NORM_MAX) + || (imu.accel.raw_mean.variance().min() > 0.1f) + || (imu.gyro.raw_mean.variance().min() > 0.1f) + ) { + imu.time_last_move_detect_us = hrt_absolute_time(); + imu.time_still_detect_us = 0; + + } else { + if (imu.time_still_detect_us == 0) { + imu.time_still_detect_us = hrt_absolute_time(); + } + } + + const bool at_rest = (hrt_elapsed_time(&imu.time_last_move_detect_us) > 400_ms) + && (imu.time_still_detect_us != 0) && (hrt_elapsed_time(&imu.time_still_detect_us) > 10_ms); + + if (imu.at_rest != at_rest) { + PX4_DEBUG("IMU at rest %d->%d (accel:%d gyro:%d)", imu.at_rest, at_rest, imu.accel.calibration.device_id(), + imu.gyro.calibration.device_id()); + + imu.at_rest = at_rest; + } + + + // vehicle_imu_status + // publish before vehicle_imu so that error counts are available synchronously if needed + const bool status_publish_interval_exceeded = (hrt_elapsed_time(&imu.time_last_status_publication) >= 100_ms); + + if (imu.accel.raw_mean.valid() && imu.gyro.raw_mean.valid() + && imu.accel.mean_sample_interval_us.valid() && imu.gyro.mean_sample_interval_us.valid() + && status_publish_interval_exceeded + ) { + + vehicle_imu_status_s imu_status{}; + + // Accel + { + imu_status.accel_device_id = imu.accel.calibration.device_id(); + + imu_status.accel_error_count = imu.accel.error_count; + + imu_status.accel_clipping[0] = imu.accel.clipping_total[0]; + imu_status.accel_clipping[1] = imu.accel.clipping_total[1]; + imu_status.accel_clipping[2] = imu.accel.clipping_total[2]; + + imu_status.accel_rate_hz = 1e6f / imu.accel.mean_publish_interval_us.mean(); + imu_status.accel_raw_rate_hz = 1e6f / imu.accel.mean_sample_interval_us.mean(); + + // accel mean and variance + const Dcmf &R = imu.accel.calibration.rotation(); + Vector3f(R * imu.accel.raw_mean.mean()).copyTo(imu_status.mean_accel); + + // variance from R * COV * R^T + const Matrix3f cov = R * imu.accel.raw_mean.covariance() * R.transpose(); + cov.diag().copyTo(imu_status.var_accel); + + imu_status.accel_vibration_metric = imu.accel.vibration_metric; + + // temperature + if (imu.accel.temperature.valid()) { + imu_status.temperature_accel = imu.accel.temperature.mean(); + + } else { + imu_status.temperature_accel = NAN; + } + } + + // Gyro + { + imu_status.gyro_device_id = imu.gyro.calibration.device_id(); + + imu_status.gyro_error_count = imu.gyro.error_count; + + imu_status.gyro_clipping[0] = imu.gyro.clipping_total[0]; + imu_status.gyro_clipping[1] = imu.gyro.clipping_total[1]; + imu_status.gyro_clipping[2] = imu.gyro.clipping_total[2]; + + imu_status.gyro_rate_hz = 1e6f / imu.gyro.mean_publish_interval_us.mean(); + imu_status.gyro_raw_rate_hz = 1e6f / imu.gyro.mean_sample_interval_us.mean(); + + // gyro mean and variance + const Dcmf &R = imu.gyro.calibration.rotation(); + Vector3f(R * imu.gyro.raw_mean.mean()).copyTo(imu_status.mean_gyro); + + // variance from R * COV * R^T + const Matrix3f cov = R * imu.gyro.raw_mean.covariance() * R.transpose(); + cov.diag().copyTo(imu_status.var_gyro); + + imu_status.gyro_vibration_metric = imu.gyro.vibration_metric; + + // Gyro delta angle coning metric = length of coning corrections averaged since last status publication + imu_status.delta_angle_coning_metric = imu.gyro.coning_norm_accum / imu.gyro.coning_norm_accum_total_time_s; + imu.gyro.coning_norm_accum = 0; + imu.gyro.coning_norm_accum_total_time_s = 0; + + // temperature + if (imu.gyro.temperature.valid()) { + imu_status.temperature_gyro = imu.gyro.temperature.mean(); + + } else { + imu_status.temperature_gyro = NAN; + } + } + + // publish + imu_status.timestamp = hrt_absolute_time(); + imu.vehicle_imu_status_pub.publish(imu_status); + + imu.time_last_status_publication = imu_status.timestamp; + + if (status_publish_interval_exceeded && !at_rest) { + imu.accel.raw_mean.reset(); + imu.gyro.raw_mean.reset(); + } + } + + // publish vehicle_imu + vehicle_imu_s vehicle_imu{}; + vehicle_imu.timestamp_sample = imu.gyro.timestamp_sample_last; + vehicle_imu.accel_device_id = imu.accel.calibration.device_id(); + vehicle_imu.gyro_device_id = imu.gyro.calibration.device_id(); + delta_angle_corrected.copyTo(vehicle_imu.delta_angle); + delta_velocity_corrected.copyTo(vehicle_imu.delta_velocity); + vehicle_imu.delta_angle_dt = delta_angle_dt; + vehicle_imu.delta_velocity_dt = delta_velocity_dt; + vehicle_imu.delta_angle_clipping = imu.gyro.clipping_flags; + vehicle_imu.delta_velocity_clipping = imu.accel.clipping_flags; + vehicle_imu.accel_calibration_count = imu.accel.calibration.calibration_count(); + vehicle_imu.gyro_calibration_count = imu.gyro.calibration.calibration_count(); + vehicle_imu.timestamp = hrt_absolute_time(); + imu.vehicle_imu_pub.publish(vehicle_imu); + + + + if (delta_angle_corrected.isAllFinite() && delta_velocity_corrected.isAllFinite()) { + imu.time_last_valid = vehicle_imu.timestamp; + + } else { + imu.time_last_invalid = vehicle_imu.timestamp; + } + + +#if 0 + _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, + imu_status.accel_error_count, _accel.priority[uorb_index]); + + _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, + imu_status.gyro_error_count, _gyro.priority[uorb_index]); + + + // find the best sensor + int accel_best_index = _accel.last_best_vote; + int gyro_best_index = _gyro.last_best_vote; + + if (!_parameter_update) { + // update current accel/gyro selection, skipped on cycles where parameters update + _accel.voter.get_best(time_now_us, &accel_best_index); + _gyro.voter.get_best(time_now_us, &gyro_best_index); + + if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { + // use sensor_selection to find best + if (_sensor_selection_sub.update(&_selection)) { + // reset inconsistency checks against primary + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + _accel_diff[sensor_index].zero(); + _gyro_diff[sensor_index].zero(); + } + } + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { + accel_best_index = i; + } + + if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { + gyro_best_index = i; + } + } + + } else { + // use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published + checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel); + checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro); } } - // update accel until integrator ready and caught up to gyro - while (_sensor_accel_sub.updated() - && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap - || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us))) - ) { + if (!sensor_data.advertised[i] && imu_sub.advertised()) { + sensor_data.advertised[i] = true; + sensor_data.priority[i] = DEFAULT_PRIORITY; + sensor_data.priority_configured[i] = DEFAULT_PRIORITY; - if (UpdateAccel()) { - updated = true; + if (i > 0) { + /* the first always exists, but for each further sensor, add a new validator */ + if (sensor_data.voter.add_new_validator()) { + added = true; + + } else { + PX4_ERR("failed to add validator for sensor %i", i); + } } } - // reconfigure integrators if calculated sensor intervals have changed - if (_update_integrator_config || !_intervals_configured) { - UpdateIntegratorConfiguration(); + + +#endif + + if (imu.primary) { + + + + // publish sensor_combined + sensor_combined_s sensor_combined{}; + + sensor_combined.timestamp = vehicle_imu.timestamp_sample; + sensor_combined.accelerometer_m_s2[0] = acceleration(0); + sensor_combined.accelerometer_m_s2[1] = acceleration(1); + sensor_combined.accelerometer_m_s2[2] = acceleration(2); + sensor_combined.accelerometer_integral_dt = vehicle_imu.delta_velocity_dt; + sensor_combined.accelerometer_clipping = vehicle_imu.delta_velocity_clipping; + sensor_combined.gyro_rad[0] = angular_velocity(0); + sensor_combined.gyro_rad[1] = angular_velocity(1); + sensor_combined.gyro_rad[2] = angular_velocity(2); + sensor_combined.gyro_integral_dt = vehicle_imu.delta_angle_dt; + sensor_combined.gyro_clipping = vehicle_imu.delta_angle_clipping; + sensor_combined.accel_calibration_count = vehicle_imu.accel_calibration_count; + sensor_combined.gyro_calibration_count = vehicle_imu.gyro_calibration_count; + + sensor_combined.accelerometer_timestamp_relative = (int32_t)((int64_t)imu.accel.timestamp_sample_last - + (int64_t)imu.gyro.timestamp_sample_last); + + _sensor_combined_pub.publish(sensor_combined); } - // check for additional updates and that we're fully caught up before publishing - if ((consume_all_gyro || _data_gap) && _sensor_gyro_sub.updated()) { - continue; - } - // publish if both accel & gyro integrators are ready - if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { - if (Publish()) { - break; - } - } - // finish if there are no more updates, but didn't publish - if (!updated) { + + + + // reset clipping flags + imu.gyro.clipping_flags = 0; + imu.accel.clipping_flags = 0; + + // record gyro publication latency and integrated samples + //_gyro_publish_latency_mean_us.update(imu.timestamp - _gyro_timestamp_last); + //_gyro_update_latency_mean_us.update(imu.timestamp - _gyro_timestamp_sample_last); + + updated = true; + } + + return updated; +} + +void VehicleIMU::PublishSensorsStatusIMU() +{ + // publish sensors_status_imu + sensors_status_imu_s status{}; + + for (const auto &imu : _imus) { + if (imu.primary) { + status.accel_device_id_primary = imu.accel.calibration.device_id(); + status.gyro_device_id_primary = imu.gyro.calibration.device_id(); break; } } - if (_param_sens_imu_autocal.get() && !parameters_updated) { - if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated()) - && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) { + static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof( + sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); + static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( + sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); - SensorCalibrationUpdate(); - _in_flight_calibration_check_timestamp_last = now_us; - } else if (!_armed) { - SensorCalibrationSaveAccel(); - SensorCalibrationSaveGyro(); + { + // accel diff + Vector3f accel_mean{}; + Vector3f accel_all[MAX_SENSOR_COUNT] {}; + uint8_t accel_count = 0; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + const auto &accel = _imus[sensor_index].accel; + + if ((accel.calibration.device_id() != 0) && accel.calibration.enabled()) { + accel_count++; + accel_all[sensor_index] = accel.acceleration_prev; + accel_mean += accel_all[sensor_index]; + } + } + + if (accel_count > 0) { + accel_mean /= accel_count; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + const auto &accel = _imus[sensor_index].accel; + + if ((accel.calibration.device_id() != 0) && accel.calibration.enabled()) { + _accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean); + } + } } } -} -bool VehicleIMU::UpdateAccel() -{ - bool updated = false; + { + // gyro diff + Vector3f gyro_mean {}; + Vector3f gyro_all[MAX_SENSOR_COUNT] {}; + uint8_t gyro_count = 0; - // integrate queued accel - sensor_accel_s accel; + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + const auto &gyro = _imus[sensor_index].gyro; - if (_sensor_accel_sub.update(&accel)) { - if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) { - _data_gap = true; - perf_count(_accel_generation_gap_perf); + if ((gyro.calibration.device_id() != 0) && gyro.calibration.enabled()) { + gyro_count++; + gyro_all[sensor_index] = gyro.angular_velocity_prev; + gyro_mean += gyro_all[sensor_index]; + } + } - } else { - // collect sample interval average for filters - if (accel.timestamp_sample > _accel_timestamp_sample_last) { - if (_accel_timestamp_sample_last != 0) { - const float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last; + if (gyro_count > 0) { + gyro_mean /= gyro_count; - _accel_mean_interval_us.update(interval_us); - _accel_fifo_mean_interval_us.update(interval_us / math::max(accel.samples, (uint8_t)1)); + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + const auto &gyro = _imus[sensor_index].gyro; - // check measured interval periodically - if (_accel_mean_interval_us.valid() && (_accel_mean_interval_us.count() % 10 == 0)) { - - const float interval_mean = _accel_mean_interval_us.mean(); - - // update sample rate if previously invalid or changed by more than 1 standard deviation - const bool diff_exceeds_stddev = sq(interval_mean - _accel_interval_us) > _accel_mean_interval_us.variance(); - - if (!PX4_ISFINITE(_accel_interval_us) || diff_exceeds_stddev) { - // update integrator configuration if interval has changed by more than 10% - _update_integrator_config = true; - } + if ((gyro.calibration.device_id() != 0) && gyro.calibration.enabled()) { + _gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean); } } - - } else { - PX4_ERR("%d - accel %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64, - _instance, accel.device_id, accel.timestamp_sample, _accel_timestamp_sample_last); - } - - if (accel.timestamp < accel.timestamp_sample) { - PX4_ERR("%d - accel %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")", - _instance, accel.device_id, accel.timestamp, accel.timestamp_sample); - } - } - - _accel_last_generation = _sensor_accel_sub.get_last_generation(); - - _accel_calibration.set_device_id(accel.device_id); - - if (accel.error_count != _status.accel_error_count) { - _publish_status = true; - _status.accel_error_count = accel.error_count; - } - - - // temperature average - if (PX4_ISFINITE(accel.temperature)) { - if ((_accel_temperature_sum_count == 0) || !PX4_ISFINITE(_accel_temperature_sum)) { - _accel_temperature_sum = accel.temperature; - _accel_temperature_sum_count = 1; - - } else { - _accel_temperature_sum += accel.temperature; - _accel_temperature_sum_count += 1; - } - } - - - const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f; - _accel_timestamp_sample_last = accel.timestamp_sample; - - const Vector3f accel_raw{accel.x, accel.y, accel.z}; - _raw_accel_mean.update(accel_raw); - _accel_integrator.put(accel_raw, dt); - - updated = true; - - if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { - // rotate sensor clip counts into vehicle body frame - const Vector3f clipping{_accel_calibration.rotation() * - Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; - - // round to get reasonble clip counts per axis (after board rotation) - const uint8_t clip_x = roundf(fabsf(clipping(0))); - const uint8_t clip_y = roundf(fabsf(clipping(1))); - const uint8_t clip_z = roundf(fabsf(clipping(2))); - - _status.accel_clipping[0] += clip_x; - _status.accel_clipping[1] += clip_y; - _status.accel_clipping[2] += clip_z; - - if (clip_x > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; - } - - if (clip_y > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; - } - - if (clip_z > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; - } - - _publish_status = true; - - if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) { - // start notifying the user periodically if there's significant continuous clipping - const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; - - if (clipping_total > _last_accel_clipping_notify_total_count + 1000) { - mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance); - /* EVENT - * @description Land now, and check the vehicle setup. - * Clipping can lead to fly-aways. - */ - events::send(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical, - "Accel {1} clipping, not safe to fly!", _instance); - _last_accel_clipping_notify_time = accel.timestamp_sample; - _last_accel_clipping_notify_total_count = clipping_total; - } } } } - return updated; -} -bool VehicleIMU::UpdateGyro() -{ - bool updated = false; - // integrate queued gyro - sensor_gyro_s gyro; + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + const auto ¤t_imu = _imus[i]; - if (_sensor_gyro_sub.update(&gyro)) { - if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { - _data_gap = true; - perf_count(_gyro_generation_gap_perf); - - } else { - // collect sample interval average for filters - if (gyro.timestamp_sample > _gyro_timestamp_sample_last) { - if (_gyro_timestamp_sample_last != 0) { - - const float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last; - - _gyro_mean_interval_us.update(interval_us); - _gyro_fifo_mean_interval_us.update(interval_us / math::max(gyro.samples, (uint8_t)1)); - - // check measured interval periodically - if (_gyro_mean_interval_us.valid() && (_gyro_mean_interval_us.count() % 10 == 0)) { - const float interval_mean = _gyro_mean_interval_us.mean(); - - // update sample rate if previously invalid or changed by more than 1 standard deviation - const bool diff_exceeds_stddev = sq(interval_mean - _gyro_interval_us) > _gyro_mean_interval_us.variance(); - - if (!PX4_ISFINITE(_gyro_interval_us) || diff_exceeds_stddev) { - // update integrator configuration if interval has changed by more than 10% - _update_integrator_config = true; - } - } - } - - } else { - PX4_ERR("%d - gyro %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64, - _instance, gyro.device_id, gyro.timestamp_sample, _gyro_timestamp_sample_last); - } - - if (gyro.timestamp < gyro.timestamp_sample) { - PX4_ERR("%d - gyro %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")", - _instance, gyro.device_id, gyro.timestamp, gyro.timestamp_sample); - } + if ((current_imu.accel.calibration.device_id() != 0) && current_imu.accel.calibration.enabled()) { + status.accel_device_ids[i] = current_imu.accel.calibration.device_id(); + status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm(); + status.accel_healthy[i] = true; // TODO (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); + status.accel_priority[i] = current_imu.accel.calibration.priority(); } - _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); - - const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; - - _gyro_timestamp_sample_last = gyro.timestamp_sample; - _gyro_timestamp_last = gyro.timestamp; - - _gyro_calibration.set_device_id(gyro.device_id); - - if (gyro.error_count != _status.gyro_error_count) { - _publish_status = true; - _status.gyro_error_count = gyro.error_count; - } - - // temperature average - if (PX4_ISFINITE(gyro.temperature)) { - if ((_gyro_temperature_sum_count == 0) || !PX4_ISFINITE(_gyro_temperature_sum)) { - _gyro_temperature_sum = gyro.temperature; - _gyro_temperature_sum_count = 1; - - } else { - _gyro_temperature_sum += gyro.temperature; - _gyro_temperature_sum_count += 1; - } - } - - const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; - _raw_gyro_mean.update(gyro_raw); - _gyro_integrator.put(gyro_raw, dt); - - updated = true; - - if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) { - // rotate sensor clip counts into vehicle body frame - const Vector3f clipping{_gyro_calibration.rotation() * - Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}}; - - // round to get reasonble clip counts per axis (after board rotation) - const uint8_t clip_x = roundf(fabsf(clipping(0))); - const uint8_t clip_y = roundf(fabsf(clipping(1))); - const uint8_t clip_z = roundf(fabsf(clipping(2))); - - _status.gyro_clipping[0] += clip_x; - _status.gyro_clipping[1] += clip_y; - _status.gyro_clipping[2] += clip_z; - - if (clip_x > 0) { - _delta_angle_clipping |= vehicle_imu_s::CLIPPING_X; - } - - if (clip_y > 0) { - _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y; - } - - if (clip_z > 0) { - _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z; - } - - _publish_status = true; - - if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) { - // start notifying the user periodically if there's significant continuous clipping - const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2]; - - if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) { - mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance); - /* EVENT - * @description Land now, and check the vehicle setup. - * Clipping can lead to fly-aways. - */ - events::send(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical, - "Gyro {1} clipping, not safe to fly!", _instance); - _last_gyro_clipping_notify_time = gyro.timestamp_sample; - _last_gyro_clipping_notify_total_count = clipping_total; - } - } + if ((current_imu.gyro.calibration.device_id() != 0) && current_imu.gyro.calibration.enabled()) { + status.gyro_device_ids[i] = current_imu.gyro.calibration.device_id(); + status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm(); + status.gyro_healthy[i] = true; // TODO (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); + status.gyro_priority[i] = current_imu.gyro.calibration.priority(); } } - return updated; -} - -bool VehicleIMU::Publish() -{ - bool updated = false; - - vehicle_imu_s imu; - Vector3f delta_angle; - Vector3f delta_velocity; - - const Vector3f accumulated_coning_corrections = _gyro_integrator.accumulated_coning_corrections(); - - if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt) - && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) { - - if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { - - // delta angle: apply offsets, scale, and board rotation - _gyro_calibration.SensorCorrectionsUpdate(); - const float gyro_dt_s = 1.e-6f * imu.delta_angle_dt; - const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle / gyro_dt_s)}; - UpdateGyroVibrationMetrics(angular_velocity); - const Vector3f delta_angle_corrected{angular_velocity * gyro_dt_s}; - - // accumulate delta angle coning corrections - _coning_norm_accum += accumulated_coning_corrections.norm() * gyro_dt_s; - _coning_norm_accum_total_time_s += gyro_dt_s; - - - // delta velocity: apply offsets, scale, and board rotation - _accel_calibration.SensorCorrectionsUpdate(); - const float accel_dt_s = 1.e-6f * imu.delta_velocity_dt; - const Vector3f acceleration{_accel_calibration.Correct(delta_velocity / accel_dt_s)}; - UpdateAccelVibrationMetrics(acceleration); - const Vector3f delta_velocity_corrected{acceleration * accel_dt_s}; - - // vehicle_imu_status - // publish before vehicle_imu so that error counts are available synchronously if needed - const bool status_publish_interval_exceeded = (hrt_elapsed_time(&_status.timestamp) >= kIMUStatusPublishingInterval); - - if (_raw_accel_mean.valid() && _raw_gyro_mean.valid() - && _accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid() - && (_publish_status || status_publish_interval_exceeded) - ) { - - // Accel - { - _status.accel_device_id = _accel_calibration.device_id(); - - _status.accel_rate_hz = 1e6f / _accel_mean_interval_us.mean(); - _status.accel_raw_rate_hz = 1e6f / _accel_fifo_mean_interval_us.mean(); // FIFO - - // accel mean and variance - const Dcmf &R = _accel_calibration.rotation(); - Vector3f(R * _raw_accel_mean.mean()).copyTo(_status.mean_accel); - - // variance from R * COV * R^T - const Matrix3f cov = R * _raw_accel_mean.covariance() * R.transpose(); - cov.diag().copyTo(_status.var_accel); - - // temperature - if ((_accel_temperature_sum_count > 0) && PX4_ISFINITE(_accel_temperature_sum)) { - _status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count; - - } else { - _status.temperature_accel = NAN; - } - } - - // Gyro - { - _status.gyro_device_id = _gyro_calibration.device_id(); - - _status.gyro_rate_hz = 1e6f / _gyro_mean_interval_us.mean(); - _status.gyro_raw_rate_hz = 1e6f / _gyro_fifo_mean_interval_us.mean(); // FIFO - - // gyro mean and variance - const Dcmf &R = _gyro_calibration.rotation(); - Vector3f(R * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro); - - // variance from R * COV * R^T - const Matrix3f cov = R * _raw_gyro_mean.covariance() * R.transpose(); - cov.diag().copyTo(_status.var_gyro); - - - // Gyro delta angle coning metric = length of coning corrections averaged since last status publication - _status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s; - _coning_norm_accum = 0; - _coning_norm_accum_total_time_s = 0; - - // temperature - if ((_gyro_temperature_sum_count > 0) && PX4_ISFINITE(_gyro_temperature_sum)) { - _status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count; - - } else { - _status.temperature_gyro = NAN; - } - } - - // publish - _status.timestamp = hrt_absolute_time(); - _vehicle_imu_status_pub.publish(_status); - - _publish_status = false; - - if (status_publish_interval_exceeded) { - _raw_accel_mean.reset(); - _accel_temperature_sum = NAN; - _accel_temperature_sum_count = 0; - - _raw_gyro_mean.reset(); - _gyro_temperature_sum = NAN; - _gyro_temperature_sum_count = 0; - } - } - - // publish vehicle_imu - imu.timestamp_sample = _gyro_timestamp_sample_last; - imu.accel_device_id = _accel_calibration.device_id(); - imu.gyro_device_id = _gyro_calibration.device_id(); - delta_angle_corrected.copyTo(imu.delta_angle); - delta_velocity_corrected.copyTo(imu.delta_velocity); - imu.delta_velocity_clipping = _delta_velocity_clipping; - imu.accel_calibration_count = _accel_calibration.calibration_count(); - imu.gyro_calibration_count = _gyro_calibration.calibration_count(); - imu.timestamp = hrt_absolute_time(); - _vehicle_imu_pub.publish(imu); - - // reset clip counts - _delta_velocity_clipping = 0; - - // record gyro publication latency and integrated samples - _gyro_publish_latency_mean_us.update(imu.timestamp - _gyro_timestamp_last); - _gyro_update_latency_mean_us.update(imu.timestamp - _gyro_timestamp_sample_last); - - updated = true; - } - } - - return updated; -} - -void VehicleIMU::UpdateIntegratorConfiguration() -{ - if (_accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid()) { - - const float accel_interval_us = _accel_mean_interval_us.mean(); - const float gyro_interval_us = _gyro_mean_interval_us.mean(); - - // determine number of sensor samples that will get closest to the desired integration interval - uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us)); - - // if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options - if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { - gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us / 2) * 2); - } - - uint32_t integration_interval_us = roundf(gyro_integral_samples * gyro_interval_us); - - // accel follows gyro as closely as possible - uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / accel_interval_us)); - - // let the gyro set the configuration and scheduling - // relaxed minimum integration time required - _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * accel_interval_us)); - _accel_integrator.set_reset_samples(accel_integral_samples); - - _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * gyro_interval_us)); - _gyro_integrator.set_reset_samples(gyro_integral_samples); - - _backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * accel_interval_us, - sensor_gyro_s::ORB_QUEUE_LENGTH * gyro_interval_us) / 2, 1000, 20000); - - // gyro: find largest integer multiple of gyro_integral_samples - for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { - if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { - gyro_integral_samples /= 2; - } - - if (gyro_integral_samples % n == 0) { - _sensor_gyro_sub.set_required_updates(n); - _sensor_gyro_sub.registerCallback(); - - _intervals_configured = true; - _update_integrator_config = false; - - PX4_DEBUG("accel (%" PRIu32 "), gyro (%" PRIu32 "), accel samples: %" PRIu8 ", gyro samples: %" PRIu8 - ", accel interval: %.1f, gyro interval: %.1f sub samples: %d", - _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples, - (double)accel_interval_us, (double)_gyro_interval_us, n); - - _accel_interval_us = accel_interval_us; - _gyro_interval_us = gyro_interval_us; - - break; - } - } - } -} - -void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration) -{ - // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev) - _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric - + 0.01f * Vector3f(acceleration - _acceleration_prev).norm(); - - _acceleration_prev = acceleration; -} - -void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity) -{ - // Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev) - _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric - + 0.01f * Vector3f(angular_velocity - _angular_velocity_prev).norm(); - - _angular_velocity_prev = angular_velocity; -} - -void VehicleIMU::PrintStatus() -{ - PX4_INFO_RAW("[vehicle_imu] %" PRIu8 " - Accel: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro: %" PRIu32 - ", interval: %.1f us (SD %.1f us)\n", - _instance, - _accel_calibration.device_id(), (double)_accel_mean_interval_us.mean(), - (double)_accel_mean_interval_us.standard_deviation(), - _gyro_calibration.device_id(), (double)_gyro_mean_interval_us.mean(), - (double)_gyro_mean_interval_us.standard_deviation()); - -#if defined(DEBUG_BUILD) - PX4_INFO_RAW("[vehicle_imu] %" PRIu8 - " - gyro update sample latency: %.1f us (SD %.1f us), publish latency %.1f us (SD %.1f us)\n", - _instance, - (double)_gyro_update_latency_mean_us.mean(), (double)_gyro_update_latency_mean_us.standard_deviation(), - (double)_gyro_publish_latency_mean_us.mean(), (double)_gyro_publish_latency_mean_us.standard_deviation() - ); -#endif // DEBUG_BUILD - - perf_print_counter(_accel_generation_gap_perf); - perf_print_counter(_gyro_generation_gap_perf); - - _accel_calibration.PrintStatus(); - _gyro_calibration.PrintStatus(); -} - -void VehicleIMU::SensorCalibrationUpdate() -{ - for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { - estimator_sensor_bias_s estimator_sensor_bias; - - if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias) - && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) { - - // find corresponding accel bias - if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable - && (estimator_sensor_bias.accel_device_id != 0) - && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) { - - const Vector3f bias{estimator_sensor_bias.accel_bias}; - - _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); - _accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance}; - _accel_learned_calibration[i].valid = true; - _accel_cal_available = true; - } - - // find corresponding gyro calibration - if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable - && (estimator_sensor_bias.gyro_device_id != 0) - && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) { - - const Vector3f bias{estimator_sensor_bias.gyro_bias}; - - _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); - _gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance}; - _gyro_learned_calibration[i].valid = true; - _gyro_cal_available = true; - } - } - } + status.timestamp = hrt_absolute_time(); + _sensors_status_imu_pub.publish(status); + } +#if 0 void VehicleIMU::SensorCalibrationSaveAccel() { if (_accel_cal_available) { @@ -907,5 +1648,176 @@ void VehicleIMU::SensorCalibrationSaveGyro() _gyro_cal_available = false; } } +#endif + +void VehicleIMU::updateGyroCalibration() +{ + // update calibrations for all available gyros + if (hrt_elapsed_time(&_last_calibration_update) > 5_s) { + bool calibration_updated = false; + int imu_index = 0; + + for (auto &imu : _imus) { + + if (imu.gyro.calibration.enabled() + && (imu.gyro.calibration.device_id() != 0) + && imu.at_rest + && (hrt_elapsed_time(&imu.time_last_move_detect_us) > 5_s) + && (hrt_elapsed_time(&imu.time_last_valid) < 1_s) + && imu.gyro.raw_mean.valid() && (imu.gyro.raw_mean.count() > 100) + && !imu.gyro.raw_mean.variance().longerThan(0.001f) + ) { + // update calibration + const Vector3f old_offset{imu.gyro.calibration.offset()}; + const Vector3f new_offset{imu.gyro.raw_mean.mean()}; + + bool change_exceeds_stddev = false; + bool variance_significantly_better = false; + + const Vector3f variance = imu.gyro.raw_mean.variance(); + + for (int i = 0; i < 3; i++) { + // check if offset changed by more than 1 standard deviation + if (sq(new_offset(i) - old_offset(i)) > variance(i)) { + change_exceeds_stddev = true; + } + + // check if current variance is significantly better than previous cal + // if (variance(i) < 0.1f * _gyro_cal_variance[gyro](i)) { + // variance_significantly_better = true; + // } + } + + // update if offset changed by more than 1 standard deviation or currently uncalibrated + if ((change_exceeds_stddev || !imu.gyro.calibration.calibrated()) + && imu.gyro.calibration.set_offset(new_offset) + ) { + + //_gyro_cal_variance[gyro] = variance; + + PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]", + imu_index, imu.gyro.calibration.device_id(), + (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), + (double)new_offset(0), (double)new_offset(1), (double)new_offset(2)); + + if (imu.gyro.calibration.ParametersSave(imu_index)) { + + } + + calibration_updated = true; + } + } + + imu_index++; + } + + // save all calibrations + if (calibration_updated) { + param_notify_changes(); + _last_calibration_update = hrt_absolute_time(); + } + } +} + +void VehicleIMU::PrintStatus() +{ + +#if 0 + PX4_INFO_RAW("selected gyro: %" PRIu32 " (%" PRIu8 ")\n", _selection.gyro_device_id, _gyro.last_best_vote); + _gyro.voter.print(); + + PX4_INFO_RAW("\n"); + PX4_INFO_RAW("selected accel: %" PRIu32 " (%" PRIu8 ")\n", _selection.accel_device_id, _accel.last_best_vote); + _accel.voter.print(); +#endif + + + + + // for (unsigned i = 0; i < dimensions; i++) { + // + // } + + // print accel + int accel_index = 0; + + for (auto &imu : _imus) { + if (imu.accel.calibration.device_id() != 0) { + PX4_INFO_RAW("accel #%u (%d), prio: %" PRIi32 "\n", accel_index++, imu.accel.calibration.device_id(), + imu.accel.calibration.priority()); + + for (unsigned axis = 0; axis < 3; axis++) { + PX4_INFO_RAW("\tval: %8.4f, mean: %8.4f VAR: %8.6f\n", (double)imu.accel.acceleration_prev(axis), + (double)imu.accel.raw_mean.mean()(axis), (double)imu.accel.raw_mean.variance()(axis)); + } + } + } + + // print gyro + int gyro_index = 0; + + for (auto &imu : _imus) { + if (imu.gyro.calibration.device_id() != 0) { + PX4_INFO_RAW("gyro #%u (%d), prio: %" PRIi32 "\n", gyro_index++, imu.gyro.calibration.device_id(), + imu.gyro.calibration.priority()); + + for (unsigned axis = 0; axis < 3; axis++) { + PX4_INFO_RAW("\tval: %8.4f, mean: %8.4f VAR: %8.6f\n", (double)imu.gyro.angular_velocity_prev(axis), + (double)imu.gyro.raw_mean.mean()(axis), (double)imu.gyro.raw_mean.variance()(axis)); + } + } + } + + + + // PX4_INFO_RAW("[vehicle_angular_velocity] selected sensor: %" PRIu32 + // ", rate: %.1f Hz %s, estimated bias: [%.5f %.5f %.5f]\n", + // _gyro_calibration.device_id(), (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "", + // (double)_gyro_bias(0), (double)_gyro_bias(1), (double)_gyro_bias(2)); + + // _gyro_calibration.PrintStatus(); + + // uint32_t imu_fifo_index = 0; + + // for (auto &s : _sensor_imu_fifo_subs) { + // if (s.mean_sample_interval_us.mean() > 0.f) { + // PX4_INFO_RAW("[vehicle_imu] IMU FIFO (%" PRIu32 "), publish interval: %.1f us (STD:%.1f us)\n", + // imu_fifo_index++, (double)s.mean_sample_interval_us.mean(), (double)s.mean_sample_interval_us.standard_deviation()); + // } + // } + + for (auto &imu : _imus) { + if (imu.accel.calibration.device_id() != 0) { + PX4_INFO_RAW("[vehicle_imu] IMU (%" PRIu32 "), pub:%.1f us (STD:%.1f us), sample:%.1f us (STD:%.1f us) %s\n", + imu.accel.calibration.device_id(), + (double)imu.accel.mean_publish_interval_us.mean(), (double)imu.accel.mean_publish_interval_us.standard_deviation(), + (double)imu.accel.mean_sample_interval_us.mean(), (double)imu.accel.mean_sample_interval_us.standard_deviation(), + imu.primary ? "*" : ""); + } + } + + + + + + // PX4_INFO_RAW("[vehicle_imu_fifo] %" PRIu8 " - IMU: %" PRIu32 ", interval: %.1f us (SD %.1f us)\n", + // _instance, _accel_calibration.device_id(), (double)_interval_us, (double)sqrtf(_interval_best_variance)); + + // PX4_INFO_RAW("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s", + // (double)_update_latency_mean.mean()(0), + // (double)_update_latency_mean.mean()(1), + // (double)(_interval_us * 1e-6f)); + + // perf_print_counter(_imu_generation_gap_perf); + + // _accel_calibration.PrintStatus(); + // _gyro_calibration.PrintStatus(); + + _vehicle_acceleration.PrintStatus(); + _vehicle_angular_velocity.PrintStatus(); + + perf_print_counter(_cycle_perf); + perf_print_counter(_selection_changed_perf); +} } // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index ab06dbf03e..d166f71e65 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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,30 +33,41 @@ #pragma once -#include +#include "IMU.hpp" +#include "VehicleAcceleration.hpp" +#include "VehicleAngularVelocity.hpp" + +#include #include -#include -#include + #include -#include -#include -#include +#include +#include +#include #include #include #include #include + +// publications +#include #include +#include +#include +#include + +// subscriptions #include -#include #include +#include +#include #include #include #include #include +#include #include -#include -#include using namespace time_literals; @@ -66,140 +77,129 @@ namespace sensors class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem { public: - VehicleIMU() = delete; - VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); - + VehicleIMU(); ~VehicleIMU() override; + void PrintStatus(); bool Start(); void Stop(); - void PrintStatus(); - private: - bool ParametersUpdate(bool force = false); - bool Publish(); void Run() override; - bool UpdateAccel(); - bool UpdateGyro(); + bool ParametersUpdate(bool force = false); - void UpdateIntegratorConfiguration(); + int8_t findAccelInstance(uint32_t device_id); - inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration); - inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); + bool PublishImu(sensors::IMU &imu, const matrix::Vector3f &delta_angle, const uint16_t delta_angle_dt, + const matrix::Vector3f &delta_velocity, const uint16_t delta_velocity_dt); + void PublishSensorsStatusIMU(); + + void SensorBiasUpdate(); + bool SensorSelectionUpdate(bool force = false); + void UpdateSensorImuFifo(uint8_t sensor_instance); + void UpdateSensorAccel(uint8_t sensor_instance); + void UpdateSensorGyro(uint8_t sensor_instance); void SensorCalibrationUpdate(); void SensorCalibrationSaveAccel(); void SensorCalibrationSaveGyro(); + void updateGyroCalibration(); + // return the square of two floating point numbers static constexpr float sq(float var) { return var * var; } - uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; - uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; + static constexpr int MAX_SENSOR_COUNT = 4; - static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - // Used to check, save and use learned magnetometer biases + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::SubscriptionMultiArray _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias}; - - uORB::Subscription _sensor_accel_sub; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; - + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - calibration::Accelerometer _accel_calibration{}; - calibration::Gyroscope _gyro_calibration{}; - - sensors::Integrator _accel_integrator{}; - sensors::IntegratorConing _gyro_integrator{}; - - uint32_t _imu_integration_interval_us{5000}; - - hrt_abstime _accel_timestamp_sample_last{0}; - hrt_abstime _gyro_timestamp_sample_last{0}; - hrt_abstime _gyro_timestamp_last{0}; - - math::WelfordMeanVector _raw_accel_mean{}; - math::WelfordMeanVector _raw_gyro_mean{}; - - math::WelfordMean _accel_mean_interval_us{}; - math::WelfordMean _accel_fifo_mean_interval_us{}; - - math::WelfordMean _gyro_mean_interval_us{}; - math::WelfordMean _gyro_fifo_mean_interval_us{}; - - math::WelfordMean _gyro_update_latency_mean_us{}; - math::WelfordMean _gyro_publish_latency_mean_us{}; - - float _accel_interval_us{NAN}; - float _gyro_interval_us{NAN}; - - unsigned _accel_last_generation{0}; - unsigned _gyro_last_generation{0}; - - float _accel_temperature_sum{NAN}; - float _gyro_temperature_sum{NAN}; - - int _accel_temperature_sum_count{0}; - int _gyro_temperature_sum_count{0}; - - matrix::Vector3f _acceleration_prev{}; // acceleration from the previous IMU measurement for vibration metrics - matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics - - vehicle_imu_status_s _status{}; - - float _coning_norm_accum{0}; - float _coning_norm_accum_total_time_s{0}; - - uint8_t _delta_angle_clipping{0}; - uint8_t _delta_velocity_clipping{0}; - - hrt_abstime _last_accel_clipping_notify_time{0}; - hrt_abstime _last_gyro_clipping_notify_time{0}; - - uint64_t _last_accel_clipping_notify_total_count{0}; - uint64_t _last_gyro_clipping_notify_total_count{0}; - - orb_advert_t _mavlink_log_pub{nullptr}; - - uint32_t _backup_schedule_timeout_us{20000}; - - bool _data_gap{false}; - bool _update_integrator_config{true}; - bool _intervals_configured{false}; - bool _publish_status{true}; - - const uint8_t _instance; - - bool _armed{false}; - - bool _accel_cal_available{false}; - bool _gyro_cal_available{false}; - - struct InFlightCalibration { - matrix::Vector3f offset{}; - matrix::Vector3f bias_variance{}; - bool valid{false}; + uORB::SubscriptionCallbackWorkItem _sensor_imu_fifo_subs[MAX_SENSOR_COUNT] { + {this, ORB_ID::sensor_imu_fifo, 0}, + {this, ORB_ID::sensor_imu_fifo, 1}, + {this, ORB_ID::sensor_imu_fifo, 2}, + {this, ORB_ID::sensor_imu_fifo, 3} }; - InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; - InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; + uORB::SubscriptionCallbackWorkItem _sensor_accel_subs[MAX_SENSOR_COUNT] { + {this, ORB_ID::sensor_accel, 0}, + {this, ORB_ID::sensor_accel, 1}, + {this, ORB_ID::sensor_accel, 2}, + {this, ORB_ID::sensor_accel, 3} + }; + + uORB::SubscriptionCallbackWorkItem _sensor_gyro_subs[MAX_SENSOR_COUNT] { + {this, ORB_ID::sensor_gyro, 0}, + {this, ORB_ID::sensor_gyro, 1}, + {this, ORB_ID::sensor_gyro, 2}, + {this, ORB_ID::sensor_gyro, 3} + }; + + IMU _imus[MAX_SENSOR_COUNT] {}; + + + + // struct SensorData { + // DataValidatorGroup voter{1}; + // unsigned int last_failover_count{0}; + // int32_t priority[MAX_SENSOR_COUNT] {}; + // int32_t priority_configured[MAX_SENSOR_COUNT] {}; + // uint8_t last_best_vote{0}; /**< index of the latest best vote */ + // uint8_t subscription_count{0}; + // bool advertised[MAX_SENSOR_COUNT] {false, false, false}; + // }; + + // SensorData _accel{}; + // SensorData _gyro{}; + + hrt_abstime _last_error_message{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */ + matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ + + + + // TODO: + uORB::Publication _sensor_combined_pub{ORB_ID(sensor_combined)}; // legacy + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + uORB::Publication _sensors_status_imu_pub{ORB_ID(sensors_status_imu)}; + + VehicleAcceleration _vehicle_acceleration{}; + VehicleAngularVelocity _vehicle_angular_velocity{}; + + uint32_t _selected_accel_device_id{0}; + uint32_t _selected_gyro_device_id{0}; + int8_t _selected_imu_index{-1}; + + + // calibration + hrt_abstime _last_calibration_update{0}; static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s}; hrt_abstime _in_flight_calibration_check_timestamp_last{0}; + bool _accel_cal_available{false}; + bool _gyro_cal_available{false}; - perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; - perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; + + bool _armed{false}; + + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": IMU update")}; + perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU selection changed")}; DEFINE_PARAMETERS( + (ParamBool) _param_sens_imu_autocal, + (ParamBool) _param_sens_imu_mode, (ParamInt) _param_imu_integ_rate, - (ParamBool) _param_sens_imu_autocal + (ParamInt) _param_imu_gyro_ratemax ) + }; } // namespace sensors diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_imu/imu_gyro_parameters.c similarity index 99% rename from src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c rename to src/modules/sensors/vehicle_imu/imu_gyro_parameters.c index cb5f320bc3..4db63a12c1 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_imu/imu_gyro_parameters.c @@ -99,7 +99,6 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); - /** * Low pass filter cutoff frequency for gyro * diff --git a/src/modules/sensors/vehicle_imu/imu_parameters.c b/src/modules/sensors/vehicle_imu/imu_parameters.c index ebe8a465a6..0aeb5bc06e 100644 --- a/src/modules/sensors/vehicle_imu/imu_parameters.c +++ b/src/modules/sensors/vehicle_imu/imu_parameters.c @@ -31,6 +31,20 @@ * ****************************************************************************/ +/** +* Low pass filter cutoff frequency for accel +* +* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); + /** * IMU integration rate. * @@ -60,3 +74,15 @@ PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200); * @group Sensors */ PARAM_DEFINE_INT32(SENS_IMU_AUTOCAL, 1); + +/** + * Sensors hub IMU mode + * + * @value 0 Disabled + * @value 1 Publish primary IMU selection + * + * @category system + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 374f2025e8..e0c3b3442a 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -111,6 +111,31 @@ bool VehicleMagnetometer::ParametersUpdate(bool force) _param_cal_mag_sides.commit(); } + + // 1. mark all existing sensor calibrations active even if sensor is missing + // this preserves the calibration in the event of a parameter export while the sensor is missing + // 2. ensure calibration slots are active for the number of sensors currently available + // this to done to eliminate differences in the active set of parameters before and after sensor calibration + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + // sensor_mag + { + uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); + + if (device_id_mag != 0) { + calibration::Magnetometer mag_cal(device_id_mag); + } + + uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; + + if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { + calibration::Magnetometer cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } + } + } + + // Mag compensation type MagCompensationType mag_comp_typ = static_cast(_param_mag_comp_typ.get()); @@ -143,6 +168,7 @@ bool VehicleMagnetometer::ParametersUpdate(bool force) if (!_armed) { + bool calibration_updated = false; // update mag priority (CAL_MAGx_PRIO) diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/vehicle_magnetometer/sensor_params_mag.c similarity index 100% rename from src/modules/sensors/sensor_params_mag.c rename to src/modules/sensors/vehicle_magnetometer/sensor_params_mag.c diff --git a/src/modules/sensors/sensor_params_flow.c b/src/modules/sensors/vehicle_optical_flow/sensor_params_flow.c similarity index 100% rename from src/modules/sensors/sensor_params_flow.c rename to src/modules/sensors/vehicle_optical_flow/sensor_params_flow.c diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp deleted file mode 100644 index 3e89e18644..0000000000 --- a/src/modules/sensors/voted_sensors_update.cpp +++ /dev/null @@ -1,497 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-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. - * - ****************************************************************************/ - -/** - * @file voted_sensors_update.cpp - * - * @author Beat Kueng - */ - -#include "voted_sensors_update.h" - -#include -#include -#include -#include - -using namespace sensors; -using namespace matrix; -using namespace time_literals; - -VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, - uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) : - ModuleParams(nullptr), - _vehicle_imu_sub(vehicle_imu_sub), - _hil_enabled(hil_enabled) -{ - _sensor_selection_pub.advertise(); - _sensors_status_imu_pub.advertise(); - - if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit - _gyro.voter.set_timeout(500000); - _accel.voter.set_timeout(500000); - } - - initializeSensors(); - - parametersUpdate(); -} - -void VotedSensorsUpdate::initializeSensors() -{ - initSensorClass(_gyro, MAX_SENSOR_COUNT); - initSensorClass(_accel, MAX_SENSOR_COUNT); -} - -void VotedSensorsUpdate::parametersUpdate() -{ - _parameter_update = true; - - updateParams(); - - // run through all IMUs - for (uint8_t uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { - uORB::SubscriptionData imu{ORB_ID(vehicle_imu), uorb_index}; - imu.update(); - - if (imu.advertised() && (imu.get().timestamp != 0) - && (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); - - if (accel_cal_index >= 0) { - // found matching CAL_ACCx_PRIO - int32_t accel_priority_old = _accel.priority_configured[uorb_index]; - - _accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index); - - if (accel_priority_old != _accel.priority_configured[uorb_index]) { - if (_accel.priority_configured[uorb_index] == 0) { - // disabled - _accel.priority[uorb_index] = 0; - - } else { - // change relative priority to incorporate any sensor faults - int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old; - _accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast(1), - static_cast(100)); - } - } - } - - // find corresponding configured gyro priority - int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id); - - if (gyro_cal_index >= 0) { - // found matching CAL_GYROx_PRIO - int32_t gyro_priority_old = _gyro.priority_configured[uorb_index]; - - _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index); - - if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { - if (_gyro.priority_configured[uorb_index] == 0) { - // disabled - _gyro.priority[uorb_index] = 0; - - } else { - // change relative priority to incorporate any sensor faults - int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old; - _gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, static_cast(1), - static_cast(100)); - } - } - } - } - } -} - -void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) -{ - const hrt_abstime time_now_us = hrt_absolute_time(); - - for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { - vehicle_imu_s imu_report; - - if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0) - && _vehicle_imu_sub[uorb_index].update(&imu_report)) { - - // copy corresponding vehicle_imu_status for accel & gyro error counts - vehicle_imu_status_s imu_status{}; - _vehicle_imu_status_subs[uorb_index].copy(&imu_status); - - _accel_device_id[uorb_index] = imu_report.accel_device_id; - _gyro_device_id[uorb_index] = imu_report.gyro_device_id; - - // convert the delta velocities to an equivalent acceleration - const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt; - Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv; - - - // convert the delta angles to an equivalent angular rate - const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt; - Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv; - - _last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample; - _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); - _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); - _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); - _last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt; - _last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping; - _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); - _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); - _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); - _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; - _last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping; - _last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count; - _last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count; - - _last_accel_timestamp[uorb_index] = imu_report.timestamp_sample; - - _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, - imu_status.accel_error_count, _accel.priority[uorb_index]); - - _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, - imu_status.gyro_error_count, _gyro.priority[uorb_index]); - } - } - - // find the best sensor - int accel_best_index = _accel.last_best_vote; - int gyro_best_index = _gyro.last_best_vote; - - if (!_parameter_update) { - // update current accel/gyro selection, skipped on cycles where parameters update - _accel.voter.get_best(time_now_us, &accel_best_index); - _gyro.voter.get_best(time_now_us, &gyro_best_index); - - if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { - // use sensor_selection to find best - if (_sensor_selection_sub.update(&_selection)) { - // reset inconsistency checks against primary - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - _accel_diff[sensor_index].zero(); - _gyro_diff[sensor_index].zero(); - } - } - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { - accel_best_index = i; - } - - if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { - gyro_best_index = i; - } - } - - } else { - // use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published - checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel); - checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro); - } - } - - // write data for the best sensor to output variables - if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0) - && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) { - - raw.timestamp = _last_sensor_data[gyro_best_index].timestamp; - memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, - sizeof(raw.accelerometer_m_s2)); - memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); - - raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; - raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; - raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count; - - raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; - raw.gyro_clipping = _last_sensor_data[gyro_best_index].gyro_clipping; - raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count; - - if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { - _accel.last_best_vote = (uint8_t)accel_best_index; - _selection.accel_device_id = _accel_device_id[accel_best_index]; - _selection_changed = true; - } - - if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) { - _gyro.last_best_vote = (uint8_t)gyro_best_index; - _selection.gyro_device_id = _gyro_device_id[gyro_best_index]; - _selection_changed = true; - - // clear all registered callbacks - for (auto &sub : _vehicle_imu_sub) { - sub.unregisterCallback(); - } - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - vehicle_imu_s report{}; - - if (_vehicle_imu_sub[i].copy(&report)) { - if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) { - _vehicle_imu_sub[i].registerCallback(); - } - } - } - } - } - - // publish sensor selection if changed - if (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) { - if (_selection_changed) { - // don't publish until selected IDs are valid - if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) { - _selection.timestamp = hrt_absolute_time(); - _sensor_selection_pub.publish(_selection); - _selection_changed = false; - } - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - _accel_diff[sensor_index].zero(); - _gyro_diff[sensor_index].zero(); - } - } - } -} - -bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, - events::px4::enums::sensor_type_t sensor_type) -{ - if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { - - uint32_t flags = sensor.voter.failover_state(); - int failover_index = sensor.voter.failover_index(); - - if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { - if (failover_index != -1) { - // we switched due to a non-critical reason. No need to panic. - PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index); - } - - } else { - if (failover_index != -1) { - - const hrt_abstime now = hrt_absolute_time(); - - if (now - _last_error_message > 3_s) { - mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!\t", - sensor_name, - failover_index, - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); - - events::px4::enums::sensor_failover_reason_t failover_reason{}; - - if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; } - - if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; } - - if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; } - - /* EVENT - * @description - * Land immediately and check the system. - */ - events::send( - events::ID("sensor_failover"), events::Log::Emergency, "{1} sensor #{2} failure: {3}", sensor_type, failover_index, - failover_reason); - - _last_error_message = now; - } - - // reduce priority of failed sensor to the minimum - sensor.priority[failover_index] = 1; - } - } - - sensor.last_failover_count = sensor.voter.failover_count(); - return true; - } - - return false; -} - -void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max) -{ - bool added = false; - int max_sensor_index = -1; - - for (unsigned i = 0; i < sensor_count_max; i++) { - - max_sensor_index = i; - - if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) { - sensor_data.advertised[i] = true; - sensor_data.priority[i] = DEFAULT_PRIORITY; - sensor_data.priority_configured[i] = DEFAULT_PRIORITY; - - if (i > 0) { - /* the first always exists, but for each further sensor, add a new validator */ - if (sensor_data.voter.add_new_validator()) { - added = true; - - } else { - PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i); - } - } - } - } - - // never decrease the sensor count, as we could end up with mismatching validators - if (max_sensor_index + 1 > sensor_data.subscription_count) { - sensor_data.subscription_count = max_sensor_index + 1; - } - - if (added) { - // force parameter refresh if anything was added - parametersUpdate(); - } -} - -void VotedSensorsUpdate::printStatus() -{ - PX4_INFO_RAW("selected gyro: %" PRIu32 " (%" PRIu8 ")\n", _selection.gyro_device_id, _gyro.last_best_vote); - _gyro.voter.print(); - - PX4_INFO_RAW("\n"); - PX4_INFO_RAW("selected accel: %" PRIu32 " (%" PRIu8 ")\n", _selection.accel_device_id, _accel.last_best_vote); - _accel.voter.print(); -} - -void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) -{ - imuPoll(raw); - - calcAccelInconsistency(); - calcGyroInconsistency(); - - sensors_status_imu_s status{}; - status.accel_device_id_primary = _selection.accel_device_id; - status.gyro_device_id_primary = _selection.gyro_device_id; - - static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof( - sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); - static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( - sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) { - status.accel_device_ids[i] = _accel_device_id[i]; - status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm(); - status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); - status.accel_priority[i] = _accel.voter.get_sensor_priority(i); - } - - if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) { - status.gyro_device_ids[i] = _gyro_device_id[i]; - status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm(); - status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); - status.gyro_priority[i] = _gyro.voter.get_sensor_priority(i); - } - } - - - status.timestamp = hrt_absolute_time(); - _sensors_status_imu_pub.publish(status); - - if (_parameter_update) { - // reset - _parameter_update = false; - } -} - -void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) -{ - if (_last_accel_timestamp[_accel.last_best_vote]) { - raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - - (int64_t)raw.timestamp); - } -} - -void VotedSensorsUpdate::calcAccelInconsistency() -{ - Vector3f accel_mean{}; - Vector3f accel_all[MAX_SENSOR_COUNT] {}; - uint8_t accel_count = 0; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { - accel_count++; - accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2}; - accel_mean += accel_all[sensor_index]; - } - } - - if (accel_count > 0) { - accel_mean /= accel_count; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { - _accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean); - } - } - } -} - -void VotedSensorsUpdate::calcGyroInconsistency() -{ - Vector3f gyro_mean{}; - Vector3f gyro_all[MAX_SENSOR_COUNT] {}; - uint8_t gyro_count = 0; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { - gyro_count++; - gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad}; - gyro_mean += gyro_all[sensor_index]; - } - } - - if (gyro_count > 0) { - gyro_mean /= gyro_count; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { - _gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean); - } - } - } -} diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h deleted file mode 100644 index 9e8d40a6e9..0000000000 --- a/src/modules/sensors/voted_sensors_update.h +++ /dev/null @@ -1,186 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-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 - -/** - * @file voted_sensors_update.h - * - * @author Beat Kueng - */ - -#include "data_validator/DataValidator.hpp" -#include "data_validator/DataValidatorGroup.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sensors -{ - -static constexpr uint8_t MAX_SENSOR_COUNT = 4; - -/** - ** class VotedSensorsUpdate - * - * Handling of sensor updates with voting - */ -class VotedSensorsUpdate : public ModuleParams -{ -public: - /** - * @param parameters parameter values. These do not have to be initialized when constructing this object. - * Only when calling init(), they have to be initialized. - */ - VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]); - - /** - * This tries to find new sensor instances. This is called from init(), then it can be called periodically. - */ - void initializeSensors(); - - void printStatus(); - - /** - * call this whenever parameters got updated. Make sure to have initializeSensors() called at least - * once before calling this. - */ - void parametersUpdate(); - - /** - * read new sensor data - */ - void sensorsPoll(sensor_combined_s &raw); - - /** - * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, - * so that the data can be published. - */ - void setRelativeTimestamps(sensor_combined_s &raw); - -private: - - static constexpr uint8_t DEFAULT_PRIORITY = 50; - - struct SensorData { - SensorData() = delete; - explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {} - - uORB::Subscription subscription[MAX_SENSOR_COUNT]; /**< raw sensor data subscription */ - DataValidatorGroup voter{1}; - unsigned int last_failover_count{0}; - int32_t priority[MAX_SENSOR_COUNT] {}; - int32_t priority_configured[MAX_SENSOR_COUNT] {}; - uint8_t last_best_vote{0}; /**< index of the latest best vote */ - uint8_t subscription_count{0}; - bool advertised[MAX_SENSOR_COUNT] {false, false, false}; - }; - - void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max); - - /** - * Poll IMU for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void imuPoll(sensor_combined_s &raw); - - /** - * Check & handle failover of a sensor - * @return true if a switch occured (could be for a non-critical reason) - */ - bool checkFailover(SensorData &sensor, const char *sensor_name, events::px4::enums::sensor_type_t sensor_type); - - /** - * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors - */ - void calcAccelInconsistency(); - - /** - * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors - */ - void calcGyroInconsistency(); - - SensorData _accel{ORB_ID::sensor_accel}; - SensorData _gyro{ORB_ID::sensor_gyro}; - - hrt_abstime _last_error_message{0}; - orb_advert_t _mavlink_log_pub{nullptr}; - - uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ - uORB::Publication _sensors_status_imu_pub{ORB_ID(sensors_status_imu)}; - - uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[MAX_SENSOR_COUNT]; - uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; - - uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - - sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {}; /**< latest sensor data from all sensors instances */ - - const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ - - bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ - - matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */ - matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ - - uint32_t _accel_device_id[MAX_SENSOR_COUNT] {}; /**< accel driver device id for each uorb instance */ - uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {}; /**< gyro driver device id for each uorb instance */ - - uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {}; /**< latest full timestamp */ - - sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ - - bool _parameter_update{false}; - - DEFINE_PARAMETERS( - (ParamBool) _param_sens_imu_mode - ) -}; - -} /* namespace sensors */ diff --git a/src/systemcmds/microbench/test_microbench_uorb.cpp b/src/systemcmds/microbench/test_microbench_uorb.cpp index b724c4a0ba..c21f3cc1bb 100644 --- a/src/systemcmds/microbench/test_microbench_uorb.cpp +++ b/src/systemcmds/microbench/test_microbench_uorb.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2018-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 @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include @@ -108,7 +108,7 @@ private: failsafe_flags_s status; vehicle_local_position_s lpos; sensor_gyro_s gyro; - sensor_gyro_fifo_s gyro_fifo; + sensor_imu_fifo_s imu_fifo; }; bool MicroBenchORB::run_tests() @@ -139,7 +139,7 @@ void MicroBenchORB::reset() gyro.timestamp = rand(); - gyro_fifo.timestamp = rand(); + imu_fifo.timestamp = rand(); } ut_declare_test_c(test_microbench_uorb, MicroBenchORB) @@ -149,7 +149,7 @@ bool MicroBenchORB::time_px4_uorb() int fd_status = orb_subscribe(ORB_ID(failsafe_flags)); int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position)); int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro)); - int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo)); + int fd_imu_fifo = orb_subscribe(ORB_ID(sensor_imu_fifo)); int ret = 0; bool updated = false; @@ -169,8 +169,8 @@ bool MicroBenchORB::time_px4_uorb() printf("\n"); - PERF("orb_check sensor_gyro_fifo", ret = orb_check(fd_gyro_fifo, &updated), 100); - PERF("orb_copy sensor_gyro_fifo", ret = orb_copy(ORB_ID(sensor_gyro_fifo), fd_gyro_fifo, &gyro_fifo), 100); + PERF("orb_check sensor_imu_fifo", ret = orb_check(fd_imu_fifo, &updated), 100); + PERF("orb_copy sensor_imu_fifo", ret = orb_copy(ORB_ID(sensor_imu_fifo), fd_imu_fifo, &imu_fifo), 100); printf("\n"); @@ -189,7 +189,7 @@ bool MicroBenchORB::time_px4_uorb() orb_unsubscribe(fd_status); orb_unsubscribe(fd_lpos); orb_unsubscribe(fd_gyro); - orb_unsubscribe(fd_gyro_fifo); + orb_unsubscribe(fd_imu_fifo); return true; } @@ -238,9 +238,9 @@ bool MicroBenchORB::time_px4_uorb_direct() { printf("\n"); - uORB::Subscription sens_gyro_fifo0{ORB_ID(sensor_gyro_fifo), 0}; - PERF("uORB::Subscription orb_check sensor_gyro_fifo:0", ret = sens_gyro_fifo0.updated(), 100); - PERF("uORB::Subscription orb_copy sensor_gyro_fifo:0", ret = sens_gyro_fifo0.copy(&gyro_fifo), 100); + uORB::Subscription sens_imu_fifo0{ORB_ID(sensor_imu_fifo), 0}; + PERF("uORB::Subscription orb_check sensor_imu_fifo:0", ret = sens_imu_fifo0.updated(), 100); + PERF("uORB::Subscription orb_copy sensor_imu_fifo:0", ret = sens_imu_fifo0.copy(&imu_fifo), 100); } return true;