From dcc87299b0d1d43735aa69105a08538017903966 Mon Sep 17 00:00:00 2001 From: Ville Juven Date: Tue, 7 May 2024 10:54:12 +0300 Subject: [PATCH] Revert "px4_platform_common/atomic.h: Use IRQ locks only when REALLY needed" This reverts commit cfa78c8ac5ae892ae02aa22ffa2bc15eb1dccc62. --- .../include/px4_platform_common/atomic.h | 116 +++++--- .../px4_platform_common/atomic_from_isr.h | 266 ------------------ .../analog_devices/adis16448/ADIS16448.hpp | 4 +- .../analog_devices/adis16470/ADIS16470.hpp | 4 +- src/drivers/imu/bosch/bmi055/BMI055.hpp | 3 +- src/drivers/imu/bosch/bmi088/BMI088.hpp | 3 +- src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp | 3 +- .../imu/invensense/icm20602/ICM20602.hpp | 4 +- .../imu/invensense/icm20608g/ICM20608G.hpp | 4 +- .../imu/invensense/icm20649/ICM20649.hpp | 4 +- .../imu/invensense/icm20689/ICM20689.hpp | 4 +- .../imu/invensense/icm20948/ICM20948.hpp | 4 +- .../imu/invensense/icm40609d/ICM40609D.hpp | 4 +- .../imu/invensense/icm42605/ICM42605.hpp | 4 +- .../imu/invensense/icm42670p/ICM42670P.hpp | 4 +- .../imu/invensense/icm42688p/ICM42688P.hpp | 4 +- .../imu/invensense/mpu6000/MPU6000.hpp | 4 +- .../imu/invensense/mpu6500/MPU6500.hpp | 4 +- .../imu/invensense/mpu9250/MPU9250.hpp | 4 +- .../imu/invensense/mpu9250/MPU9250_I2C.hpp | 4 +- 20 files changed, 103 insertions(+), 348 deletions(-) delete mode 100644 platforms/common/include/px4_platform_common/atomic_from_isr.h diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index 6a4e00694d4d..e3e12a59c032 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -58,7 +58,9 @@ #include #include -#include +#if defined(__PX4_NUTTX) +# include +#endif // __PX4_NUTTX namespace px4 { @@ -73,31 +75,26 @@ class atomic // IRQ handlers. This might not be required everywhere though. static_assert(__atomic_always_lock_free(sizeof(T), 0), "atomic is not lock-free for the given type T"); #endif // __PX4_POSIX - atomic() - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - px4_sem_init(&_lock, 0, 1); - } - } - explicit atomic(T value) : _value(value) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - px4_sem_init(&_lock, 0, 1); - } - } + + atomic() = default; + explicit atomic(T value) : _value(value) {} /** * Atomically read the current value */ inline T load() const { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T val = _value; - release_lock(); + leave_critical_section(flags); return val; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); } } @@ -107,12 +104,16 @@ class atomic */ inline void store(T value) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); _value = value; - release_lock(); + leave_critical_section(flags); - } else { + } else +#endif // __PX4_NUTTX + { __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); } } @@ -123,14 +124,18 @@ class atomic */ inline T fetch_add(T num) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T ret = _value; _value += num; - release_lock(); + leave_critical_section(flags); return ret; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); } } @@ -141,14 +146,18 @@ class atomic */ inline T fetch_sub(T num) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T ret = _value; _value -= num; - release_lock(); + leave_critical_section(flags); return ret; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); } } @@ -159,14 +168,18 @@ class atomic */ inline T fetch_and(T num) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T val = _value; _value &= num; - release_lock(); + leave_critical_section(flags); return val; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); } } @@ -177,14 +190,18 @@ class atomic */ inline T fetch_xor(T num) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T val = _value; _value ^= num; - release_lock(); + leave_critical_section(flags); return val; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); } } @@ -195,14 +212,18 @@ class atomic */ inline T fetch_or(T num) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T val = _value; _value |= num; - release_lock(); + leave_critical_section(flags); return val; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); } } @@ -213,14 +234,18 @@ class atomic */ inline T fetch_nand(T num) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); T ret = _value; _value = ~(_value & num); - release_lock(); + leave_critical_section(flags); return ret; - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); } } @@ -235,31 +260,31 @@ class atomic */ inline bool compare_exchange(T *expected, T desired) { +#if defined(__PX4_NUTTX) + if (!__atomic_always_lock_free(sizeof(T), 0)) { - take_lock(); + irqstate_t flags = enter_critical_section(); if (_value == *expected) { _value = desired; - release_lock(); + leave_critical_section(flags); return true; } else { *expected = _value; - release_lock(); + leave_critical_section(flags); return false; } - } else { + } else +#endif // __PX4_NUTTX + { return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); } } private: T _value {}; - - inline void take_lock() const { do {} while (px4_sem_wait(&_lock) != 0); } - inline void release_lock() const { px4_sem_post(&_lock); } - mutable px4_sem_t _lock; }; using atomic_int = atomic; @@ -279,4 +304,3 @@ using atomic_bool = atomic; } /* namespace px4 */ #endif /* __cplusplus */ - diff --git a/platforms/common/include/px4_platform_common/atomic_from_isr.h b/platforms/common/include/px4_platform_common/atomic_from_isr.h deleted file mode 100644 index 191e969dfdf1..000000000000 --- a/platforms/common/include/px4_platform_common/atomic_from_isr.h +++ /dev/null @@ -1,266 +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. - * - ****************************************************************************/ - -/** - * @file atomic_from_isr.h - * - * Provides atomic integers and counters. Each method is executed atomically and thus - * can be used to prevent data races and add memory synchronization between threads. - * - * In addition to the atomicity, each method serves as a memory barrier (sequential - * consistent ordering). This means all operations that happen before and could - * potentially have visible side-effects in other threads will happen before - * the method is executed. - * - * The implementation uses the built-in methods from GCC (supported by Clang as well). - * @see https://gcc.gnu.org/onlinedocs/gcc/_005f_005fatomic-Builtins.html. - * - * @note: on ARM, the instructions LDREX and STREX might be emitted. To ensure correct - * behavior, the exclusive monitor needs to be cleared on a task switch (via CLREX). - * This happens automatically e.g. on ARMv7-M as part of an exception entry or exit - * sequence. - */ - -#pragma once - -#ifdef __cplusplus - -#include -#include - - -#if !defined(__PX4_NUTTX) - -/* For non-NuttX targets forward this to the generic atomic implementation */ -#include "atomic.h" - -namespace px4 -{ - -template -using atomic_from_isr = atomic; - -} - -#else - -/* For NuttX targets, implement an IRQ safe way for atomic data */ -#include - -namespace px4 -{ - -template -class atomic_from_isr -{ -public: - - atomic_from_isr() = default; - explicit atomic_from_isr(T value) : _value(value) {} - - /** - * Atomically read the current value - */ - inline T load() const - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T val = _value; - leave_critical_section(flags); - return val; - - } else { - return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomically store a value - */ - inline void store(T value) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - _value = value; - leave_critical_section(flags); - - } else { - __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomically add a number and return the previous value. - * @return value prior to the addition - */ - inline T fetch_add(T num) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T ret = _value; - _value += num; - leave_critical_section(flags); - return ret; - - } else { - return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomically substract a number and return the previous value. - * @return value prior to the substraction - */ - inline T fetch_sub(T num) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T ret = _value; - _value -= num; - leave_critical_section(flags); - return ret; - - } else { - return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomic AND with a number - * @return value prior to the operation - */ - inline T fetch_and(T num) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T val = _value; - _value &= num; - leave_critical_section(flags); - return val; - - } else { - return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomic XOR with a number - * @return value prior to the operation - */ - inline T fetch_xor(T num) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T val = _value; - _value ^= num; - leave_critical_section(flags); - return val; - - } else { - return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomic OR with a number - * @return value prior to the operation - */ - inline T fetch_or(T num) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T val = _value; - _value |= num; - leave_critical_section(flags); - return val; - - } else { - return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomic NAND (~(_value & num)) with a number - * @return value prior to the operation - */ - inline T fetch_nand(T num) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - T ret = _value; - _value = ~(_value & num); - leave_critical_section(flags); - return ret; - - } else { - return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); - } - } - - /** - * Atomic compare and exchange operation. - * This compares the contents of _value with the contents of *expected. If - * equal, the operation is a read-modify-write operation that writes desired - * into _value. If they are not equal, the operation is a read and the current - * contents of _value are written into *expected. - * @return If desired is written into _value then true is returned - */ - inline bool compare_exchange(T *expected, T desired) - { - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); - - if (_value == *expected) { - _value = desired; - leave_critical_section(flags); - return true; - - } else { - *expected = _value; - leave_critical_section(flags); - return false; - } - - } else { - return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); - } - } - -private: - T _value {}; -}; - -} /* namespace px4 */ - -#endif /* __PX4_NUTTX */ -#endif /* __cplusplus */ diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp index 0c17bdfd4d43..a28f6f270654 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include using namespace Analog_Devices_ADIS16448; @@ -114,7 +114,7 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp index a0a94056295f..40c6fab14f9f 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace Analog_Devices_ADIS16470; @@ -107,7 +107,7 @@ class ADIS16470 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; bool _self_test_passed{false}; diff --git a/src/drivers/imu/bosch/bmi055/BMI055.hpp b/src/drivers/imu/bosch/bmi055/BMI055.hpp index 6cc8617cb5b7..f4fad2772ea2 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055.hpp @@ -36,7 +36,6 @@ #include #include #include -#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -67,7 +66,7 @@ class BMI055 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp index 7008f7cee243..fa6cc223701a 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp @@ -36,7 +36,6 @@ #include #include #include -#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -67,7 +66,7 @@ class BMI088 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp index 25068c1c9245..6c1734a059db 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp @@ -36,7 +36,6 @@ #include #include #include -#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -73,7 +72,7 @@ class BMI088 : public device::I2C, public I2CSPIDriver int _total_failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index 8838cd48c34e..3855ce12ea93 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20602; @@ -140,7 +140,7 @@ class ICM20602 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index bd11f45bcf4d..9608eeb816ac 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20608G; @@ -138,7 +138,7 @@ class ICM20608G : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index 43aaa1c632c7..531f0be988ef 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20649; @@ -152,7 +152,7 @@ class ICM20649 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index 206b57ed5cff..df3b8a734e90 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20689; @@ -138,7 +138,7 @@ class ICM20689 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index c9e53deab785..103a03fe573e 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include "ICM20948_AK09916.hpp" @@ -169,7 +169,7 @@ class ICM20948 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index f3f47677539e..ab6fcfa1f27b 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM40609D; @@ -146,7 +146,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp index d4bee687721c..98ad916d90dc 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42605; @@ -146,7 +146,7 @@ class ICM42605 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp index ec497d67ea72..362418f7aea3 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42670P; @@ -150,7 +150,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index 4ad0d1beebce..67f456cdc795 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42688P; @@ -164,7 +164,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index ff8df4f409d7..2d5180013a22 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU6000; @@ -140,7 +140,7 @@ class MPU6000 : public device::SPI, public I2CSPIDriver FIFO::DATA _fifo_sample_last_new_accel{}; uint32_t _fifo_accel_samples_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 712790ec3266..7686db5114e5 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU6500; @@ -138,7 +138,7 @@ class MPU6500 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index c0d26521d21b..369d2ec6e7cb 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include "MPU9250_AK8963.hpp" @@ -150,7 +150,7 @@ class MPU9250 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp index d6aa3bdde105..7af10d0b6076 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU9250; @@ -136,7 +136,7 @@ class MPU9250_I2C : public device::I2C, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic_from_isr _drdy_timestamp_sample{0}; + px4::atomic _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false};