Skip to content

Commit

Permalink
[wpilibc] Use std::atomic in ADIS classes (#6217)
Browse files Browse the repository at this point in the history
  • Loading branch information
@spacey-sooty
spacey-sooty authored Jan 16, 2024
1 parent 5bc942f commit 30965b2
Show file tree
Hide file tree
Showing 4 changed files with 228 additions and 11 deletions.
116 changes: 116 additions & 0 deletions wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
99 changes: 99 additions & 0 deletions wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

#include <cmath>
#include <numbers>
#include <utility>

#include <hal/HAL.h>
#include <wpi/sendable/SendableBuilder.h>
Expand Down Expand Up @@ -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();
Expand Down
13 changes: 7 additions & 6 deletions wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@

#include <stdint.h>

#include <atomic>
#include <thread>

#include <hal/SimDevice.h>
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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<bool> m_thread_active = false;
std::atomic<bool> m_first_run = true;
std::atomic<bool> m_thread_idle = false;
std::atomic<bool> m_start_up_mode = true;

bool m_auto_configured = false;
SPI::Port m_spi_port;
Expand Down
11 changes: 6 additions & 5 deletions wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@

#include <stdint.h>

#include <atomic>
#include <thread>

#include <hal/SimDevice.h>
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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<bool> m_thread_active = false;
std::atomic<bool> m_first_run = true;
std::atomic<bool> m_thread_idle = false;
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time = 0;
Expand Down

0 comments on commit 30965b2

Please sign in to comment.