From 30965b20cff60690485327df96eaa97abd7c2555 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Tue, 16 Jan 2024 14:42:19 +0800 Subject: [PATCH] [wpilibc] Use std::atomic in ADIS classes (#6217) --- wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp | 116 ++++++++++++++++++ wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 99 +++++++++++++++ .../main/native/include/frc/ADIS16448_IMU.h | 13 +- .../main/native/include/frc/ADIS16470_IMU.h | 11 +- 4 files changed, 228 insertions(+), 11 deletions(-) diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index c9d5190fd39..a9950ae2dbe 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -154,6 +154,122 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, m_connected = true; } +ADIS16448_IMU::ADIS16448_IMU(ADIS16448_IMU&& other) + : m_reset_in{std::move(other.m_reset_in)}, + m_status_led{std::move(other.m_status_led)}, + m_yaw_axis{std::move(other.m_yaw_axis)}, + m_gyro_rate_x{std::move(other.m_gyro_rate_x)}, + m_gyro_rate_y{std::move(other.m_gyro_rate_y)}, + m_gyro_rate_z{std::move(other.m_gyro_rate_z)}, + m_accel_x{std::move(other.m_accel_x)}, + m_accel_y{std::move(other.m_accel_y)}, + m_accel_z{std::move(other.m_accel_z)}, + m_mag_x{std::move(other.m_mag_x)}, + m_mag_y{std::move(other.m_mag_y)}, + m_mag_z{std::move(other.m_mag_z)}, + m_baro{std::move(other.m_baro)}, + m_temp{std::move(other.m_temp)}, + m_tau{std::move(other.m_tau)}, + m_dt{std::move(other.m_dt)}, + m_alpha{std::move(other.m_alpha)}, + m_compAngleX{std::move(other.m_compAngleX)}, + m_compAngleY{std::move(other.m_compAngleY)}, + m_accelAngleX{std::move(other.m_accelAngleX)}, + m_accelAngleY{std::move(other.m_accelAngleY)}, + m_offset_buffer{other.m_offset_buffer}, + m_gyro_rate_offset_x{std::move(other.m_gyro_rate_offset_x)}, + m_gyro_rate_offset_y{std::move(other.m_gyro_rate_offset_y)}, + m_gyro_rate_offset_z{std::move(other.m_gyro_rate_offset_z)}, + m_avg_size{std::move(other.m_avg_size)}, + m_accum_count{std::move(other.m_accum_count)}, + m_integ_gyro_angle_x{std::move(other.m_integ_gyro_angle_x)}, + m_integ_gyro_angle_y{std::move(other.m_integ_gyro_angle_y)}, + m_integ_gyro_angle_z{std::move(other.m_integ_gyro_angle_z)}, + m_thread_active{other.m_thread_active.load()}, + m_first_run{other.m_first_run.load()}, + m_thread_idle{other.m_thread_idle.load()}, + m_start_up_mode{other.m_start_up_mode.load()}, + m_auto_configured{std::move(other.m_auto_configured)}, + m_spi_port{std::move(other.m_spi_port)}, + m_calibration_time{std::move(other.m_calibration_time)}, + m_spi{std::move(other.m_spi)}, + m_auto_interrupt{std::move(other.m_auto_interrupt)}, + m_connected{std::move(other.m_connected)}, + m_acquire_task{std::move(other.m_acquire_task)}, + m_simDevice{std::move(other.m_simDevice)}, + m_simConnected{std::move(other.m_simConnected)}, + m_simGyroAngleX{std::move(other.m_simGyroAngleX)}, + m_simGyroAngleY{std::move(other.m_simGyroAngleZ)}, + m_simGyroAngleZ{std::move(other.m_simGyroAngleZ)}, + m_simGyroRateX{std::move(other.m_simGyroRateX)}, + m_simGyroRateY{std::move(other.m_simGyroRateY)}, + m_simGyroRateZ{std::move(other.m_simGyroRateZ)}, + m_simAccelX{std::move(other.m_simAccelX)}, + m_simAccelY{std::move(other.m_simAccelY)}, + m_simAccelZ{std::move(other.m_simAccelZ)}, + m_mutex{std::move(other.m_mutex)} {} + +ADIS16448_IMU& ADIS16448_IMU::operator=(ADIS16448_IMU&& other) { + if (this == &other) { + return *this; + } + + std::swap(this->m_reset_in, other.m_reset_in); + std::swap(this->m_status_led, other.m_status_led); + std::swap(this->m_yaw_axis, other.m_yaw_axis); + std::swap(this->m_gyro_rate_x, other.m_gyro_rate_x); + std::swap(this->m_gyro_rate_y, other.m_gyro_rate_y); + std::swap(this->m_gyro_rate_z, other.m_gyro_rate_z); + std::swap(this->m_accel_x, other.m_accel_x); + std::swap(this->m_accel_y, other.m_accel_y); + std::swap(this->m_accel_z, other.m_accel_z); + std::swap(this->m_mag_x, other.m_mag_x); + std::swap(this->m_mag_y, other.m_mag_y); + std::swap(this->m_mag_z, other.m_mag_z); + std::swap(this->m_baro, other.m_baro); + std::swap(this->m_temp, other.m_temp); + std::swap(this->m_tau, other.m_tau); + std::swap(this->m_dt, other.m_dt); + std::swap(this->m_alpha, other.m_alpha); + std::swap(this->m_compAngleX, other.m_compAngleX); + std::swap(this->m_compAngleY, other.m_compAngleY); + std::swap(this->m_accelAngleX, other.m_accelAngleX); + std::swap(this->m_accelAngleY, other.m_accelAngleY); + std::swap(this->m_offset_buffer, other.m_offset_buffer); + std::swap(this->m_gyro_rate_offset_x, other.m_gyro_rate_offset_x); + std::swap(this->m_gyro_rate_offset_y, other.m_gyro_rate_offset_y); + std::swap(this->m_gyro_rate_offset_z, other.m_gyro_rate_offset_z); + std::swap(this->m_avg_size, other.m_avg_size); + std::swap(this->m_accum_count, other.m_accum_count); + std::swap(this->m_integ_gyro_angle_x, other.m_integ_gyro_angle_x); + std::swap(this->m_integ_gyro_angle_y, other.m_integ_gyro_angle_y); + std::swap(this->m_integ_gyro_angle_z, other.m_integ_gyro_angle_z); + this->m_thread_active = other.m_thread_active.load(); + this->m_first_run = other.m_first_run.load(); + this->m_thread_idle = other.m_thread_idle.load(); + this->m_start_up_mode = other.m_start_up_mode.load(); + std::swap(this->m_auto_configured, other.m_auto_configured); + std::swap(this->m_spi_port, other.m_spi_port); + std::swap(this->m_calibration_time, other.m_calibration_time); + std::swap(this->m_spi, other.m_spi); + std::swap(this->m_auto_interrupt, other.m_auto_interrupt); + std::swap(this->m_connected, other.m_connected); + std::swap(this->m_acquire_task, other.m_acquire_task); + std::swap(this->m_simDevice, other.m_simDevice); + std::swap(this->m_simConnected, other.m_simConnected); + std::swap(this->m_simGyroAngleX, other.m_simGyroAngleX); + std::swap(this->m_simGyroAngleY, other.m_simGyroAngleY); + std::swap(this->m_simGyroAngleZ, other.m_simGyroAngleZ); + std::swap(this->m_simGyroRateX, other.m_simGyroRateX); + std::swap(this->m_simGyroRateY, other.m_simGyroRateY); + std::swap(this->m_simGyroRateZ, other.m_simGyroRateZ); + std::swap(this->m_simAccelX, other.m_simAccelX); + std::swap(this->m_simAccelY, other.m_simAccelY); + std::swap(this->m_simAccelZ, other.m_simAccelZ); + std::swap(this->m_mutex, other.m_mutex); + return *this; +} + bool ADIS16448_IMU::IsConnected() const { if (m_simConnected) { return m_simConnected.Get(); diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 0b11f847a87..6f562e1b6ed 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -172,6 +173,104 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, m_connected = true; } +ADIS16470_IMU::ADIS16470_IMU(ADIS16470_IMU&& other) + : m_yaw_axis{std::move(other.m_yaw_axis)}, + m_pitch_axis{std::move(other.m_pitch_axis)}, + m_roll_axis{std::move(other.m_roll_axis)}, + m_reset_in{std::move(other.m_reset_in)}, + m_status_led{std::move(other.m_status_led)}, + m_integ_angle_x{std::move(other.m_integ_angle_x)}, + m_integ_angle_y{std::move(other.m_integ_angle_y)}, + m_integ_angle_z{std::move(other.m_integ_angle_z)}, + m_gyro_rate_x{std::move(other.m_gyro_rate_x)}, + m_gyro_rate_y{std::move(other.m_gyro_rate_y)}, + m_gyro_rate_z{std::move(other.m_gyro_rate_z)}, + m_accel_x{std::move(other.m_accel_x)}, + m_accel_y{std::move(other.m_accel_y)}, + m_accel_z{std::move(other.m_accel_z)}, + m_tau{std::move(other.m_tau)}, + m_dt{std::move(other.m_dt)}, + m_alpha{std::move(other.m_alpha)}, + m_compAngleX{std::move(other.m_compAngleX)}, + m_compAngleY{std::move(other.m_compAngleY)}, + m_accelAngleX{std::move(other.m_accelAngleX)}, + m_accelAngleY{std::move(other.m_accelAngleY)}, + m_thread_active{other.m_thread_active.load()}, + m_first_run{other.m_first_run.load()}, + m_thread_idle{other.m_thread_idle.load()}, + m_auto_configured{std::move(other.m_auto_configured)}, + m_spi_port{std::move(other.m_spi_port)}, + m_calibration_time{std::move(other.m_calibration_time)}, + m_spi{std::move(other.m_spi)}, + m_auto_interrupt{std::move(other.m_auto_interrupt)}, + m_scaled_sample_rate{std::move(other.m_scaled_sample_rate)}, + m_connected{std::move(other.m_connected)}, + m_acquire_task{std::move(other.m_acquire_task)}, + m_simDevice{std::move(other.m_simDevice)}, + m_simConnected{std::move(other.m_simConnected)}, + m_simGyroAngleX{std::move(other.m_simGyroAngleX)}, + m_simGyroAngleY{std::move(other.m_simGyroAngleZ)}, + m_simGyroAngleZ{std::move(other.m_simGyroAngleZ)}, + m_simGyroRateX{std::move(other.m_simGyroRateX)}, + m_simGyroRateY{std::move(other.m_simGyroRateY)}, + m_simGyroRateZ{std::move(other.m_simGyroRateZ)}, + m_simAccelX{std::move(other.m_simAccelX)}, + m_simAccelY{std::move(other.m_simAccelY)}, + m_simAccelZ{std::move(other.m_simAccelZ)}, + m_mutex{std::move(other.m_mutex)} {} + +ADIS16470_IMU& ADIS16470_IMU::operator=(ADIS16470_IMU&& other) { + if (this == &other) { + return *this; + } + + std::swap(this->m_yaw_axis, other.m_yaw_axis); + std::swap(this->m_pitch_axis, other.m_pitch_axis); + std::swap(this->m_roll_axis, other.m_roll_axis); + std::swap(this->m_reset_in, other.m_reset_in); + std::swap(this->m_status_led, other.m_status_led); + std::swap(this->m_integ_angle_x, other.m_integ_angle_x); + std::swap(this->m_integ_angle_y, other.m_integ_angle_y); + std::swap(this->m_integ_angle_z, other.m_integ_angle_z); + std::swap(this->m_gyro_rate_x, other.m_gyro_rate_x); + std::swap(this->m_gyro_rate_y, other.m_gyro_rate_y); + std::swap(this->m_gyro_rate_z, other.m_gyro_rate_z); + std::swap(this->m_accel_x, other.m_accel_x); + std::swap(this->m_accel_y, other.m_accel_y); + std::swap(this->m_accel_z, other.m_accel_z); + std::swap(this->m_tau, other.m_tau); + std::swap(this->m_dt, other.m_dt); + std::swap(this->m_alpha, other.m_alpha); + std::swap(this->m_compAngleX, other.m_compAngleX); + std::swap(this->m_compAngleY, other.m_compAngleY); + std::swap(this->m_accelAngleX, other.m_accelAngleX); + std::swap(this->m_accelAngleY, other.m_accelAngleY); + this->m_thread_active = other.m_thread_active.load(); + this->m_first_run = other.m_first_run.load(); + this->m_thread_idle = other.m_thread_idle.load(); + std::swap(this->m_auto_configured, other.m_auto_configured); + std::swap(this->m_spi_port, other.m_spi_port); + std::swap(this->m_calibration_time, other.m_calibration_time); + std::swap(this->m_spi, other.m_spi); + std::swap(this->m_auto_interrupt, other.m_auto_interrupt); + std::swap(this->m_scaled_sample_rate, other.m_scaled_sample_rate); + std::swap(this->m_connected, other.m_connected); + std::swap(this->m_acquire_task, other.m_acquire_task); + std::swap(this->m_simDevice, other.m_simDevice); + std::swap(this->m_simConnected, other.m_simConnected); + std::swap(this->m_simGyroAngleX, other.m_simGyroAngleX); + std::swap(this->m_simGyroAngleY, other.m_simGyroAngleZ); + std::swap(this->m_simGyroAngleZ, other.m_simGyroAngleZ); + std::swap(this->m_simGyroRateX, other.m_simGyroRateX); + std::swap(this->m_simGyroRateY, other.m_simGyroRateY); + std::swap(this->m_simGyroRateZ, other.m_simGyroRateZ); + std::swap(this->m_simAccelX, other.m_simAccelX); + std::swap(this->m_simAccelY, other.m_simAccelY); + std::swap(this->m_simAccelZ, other.m_simAccelZ); + std::swap(this->m_mutex, other.m_mutex); + return *this; +} + bool ADIS16470_IMU::IsConnected() const { if (m_simConnected) { return m_simConnected.Get(); diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 3e7120f3b2d..9fedb59559e 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -15,6 +15,7 @@ #include +#include #include #include @@ -114,8 +115,8 @@ class ADIS16448_IMU : public wpi::Sendable, ~ADIS16448_IMU() override; - ADIS16448_IMU(ADIS16448_IMU&&) = default; - ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default; + ADIS16448_IMU(ADIS16448_IMU&&); + ADIS16448_IMU& operator=(ADIS16448_IMU&&); /** * Initialize the IMU. @@ -414,10 +415,10 @@ class ADIS16448_IMU : public wpi::Sendable, double CompFilterProcess(double compAngle, double accelAngle, double omega); // State and resource variables - volatile bool m_thread_active = false; - volatile bool m_first_run = true; - volatile bool m_thread_idle = false; - volatile bool m_start_up_mode = true; + std::atomic m_thread_active = false; + std::atomic m_first_run = true; + std::atomic m_thread_idle = false; + std::atomic m_start_up_mode = true; bool m_auto_configured = false; SPI::Port m_spi_port; diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index c5ee7e6a6cd..54078298756 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -15,6 +15,7 @@ #include +#include #include #include @@ -143,8 +144,8 @@ class ADIS16470_IMU : public wpi::Sendable, ~ADIS16470_IMU() override; - ADIS16470_IMU(ADIS16470_IMU&&) = default; - ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default; + ADIS16470_IMU(ADIS16470_IMU&& other); + ADIS16470_IMU& operator=(ADIS16470_IMU&& other); /** * Configures the decimation rate of the IMU. @@ -500,9 +501,9 @@ class ADIS16470_IMU : public wpi::Sendable, double CompFilterProcess(double compAngle, double accelAngle, double omega); // State and resource variables - volatile bool m_thread_active = false; - volatile bool m_first_run = true; - volatile bool m_thread_idle = false; + std::atomic m_thread_active = false; + std::atomic m_first_run = true; + std::atomic m_thread_idle = false; bool m_auto_configured = false; SPI::Port m_spi_port; uint16_t m_calibration_time = 0;