Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add GetRate() to ADIS classes (#3864)

Browse Source 
The angular rate is treated somewhat like an angle during calibration,
but the datasheet says it's angular rate. The variables were renamed to
make this clearer.
This commit is contained in:
Tyler Veness
2022-01-04 22:26:23 -08:00
committed by GitHub
parent 05d66f862d
commit 22c4da152e
12 changed files with 516 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
View File

@@ -133,6 +133,11 @@ class ADIS16448_IMU : public nt::NTSendable,
*/
units::degree_t GetAngle() const;
/**
* Returns the yaw axis angular rate in degrees per second (CCW positive).
*/
units::degrees_per_second_t GetRate() const;
/**
* Returns the accumulated gyro angle in the X axis.
*/
@@ -148,6 +153,21 @@ class ADIS16448_IMU : public nt::NTSendable,
*/
units::degree_t GetGyroAngleZ() const;
/**
* Returns the angular rate in the X axis.
*/
units::degrees_per_second_t GetGyroRateX() const;
/**
* Returns the angular rate in the Y axis.
*/
units::degrees_per_second_t GetGyroRateY() const;
/**
* Returns the angular rate in the Z axis.
*/
units::degrees_per_second_t GetGyroRateZ() const;
/**
* Returns the acceleration in the X axis.
*/
@@ -259,10 +279,10 @@ class ADIS16448_IMU : public nt::NTSendable,
static constexpr double grav = 9.81;
/** @brief struct to store offset data */
struct offset_data {
double m_accum_gyro_x = 0.0;
double m_accum_gyro_y = 0.0;
double m_accum_gyro_z = 0.0;
struct OffsetData {
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
};
bool SwitchToStandardSPI();
@@ -281,9 +301,9 @@ class ADIS16448_IMU : public nt::NTSendable,
IMUAxis m_yaw_axis;
// Last read values (post-scaling)
double m_gyro_x = 0.0;
double m_gyro_y = 0.0;
double m_gyro_z = 0.0;
double m_gyro_rate_x = 0.0;
double m_gyro_rate_y = 0.0;
double m_gyro_rate_z = 0.0;
double m_accel_x = 0.0;
double m_accel_y = 0.0;
double m_accel_z = 0.0;
@@ -299,11 +319,11 @@ class ADIS16448_IMU : public nt::NTSendable,
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// vector for storing most recent imu values
offset_data* m_offset_buffer = nullptr;
OffsetData* m_offset_buffer = nullptr;
double m_gyro_offset_x = 0.0;
double m_gyro_offset_y = 0.0;
double m_gyro_offset_z = 0.0;
double m_gyro_rate_offset_x = 0.0;
double m_gyro_rate_offset_y = 0.0;
double m_gyro_rate_offset_z = 0.0;
// function to re-init offset buffer
void InitOffsetBuffer(int size);
@@ -313,9 +333,9 @@ class ADIS16448_IMU : public nt::NTSendable,
int m_accum_count = 0;
// Integrated gyro values
double m_integ_gyro_x = 0.0;
double m_integ_gyro_y = 0.0;
double m_integ_gyro_z = 0.0;
double m_integ_gyro_angle_x = 0.0;
double m_integ_gyro_angle_y = 0.0;
double m_integ_gyro_angle_z = 0.0;
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
@@ -344,6 +364,9 @@ class ADIS16448_IMU : public nt::NTSendable,
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

15
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View File

@@ -127,6 +127,11 @@ class ADIS16470_IMU : public nt::NTSendable,
*/
units::degree_t GetAngle() const;
/**
* Returns the yaw axis angular rate in degrees per second (CCW positive).
*/
units::degrees_per_second_t GetRate() const;
/**
* Returns the acceleration in the X axis.
*/
@@ -342,7 +347,12 @@ class ADIS16470_IMU : public nt::NTSendable,
double m_integ_angle = 0.0;
// Instant raw outputs
double m_gyro_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, m_accel_z = 0.0;
double m_gyro_rate_x = 0.0;
double m_gyro_rate_y = 0.0;
double m_gyro_rate_z = 0.0;
double m_accel_x = 0.0;
double m_accel_y = 0.0;
double m_accel_z = 0.0;
// Complementary filter variables
double m_tau = 1.0;
@@ -375,6 +385,9 @@ class ADIS16470_IMU : public nt::NTSendable,
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

24
wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
View File

@@ -48,6 +48,27 @@ class ADIS16448_IMUSim {
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t angularRate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t angularRate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t angularRate);
/**
* Sets the X axis acceleration.
*
@@ -73,6 +94,9 @@ class ADIS16448_IMUSim {
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

24
wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
View File

@@ -48,6 +48,27 @@ class ADIS16470_IMUSim {
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t angularRate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t angularRate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t angularRate);
/**
* Sets the X axis acceleration.
*
@@ -73,6 +94,9 @@ class ADIS16470_IMUSim {
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;