11 #include "frc/smartdashboard/SendableBase.h"
12 #include "frc/smartdashboard/SendableBuilder.h"
15 #include "frc/SerialPort.h"
16 #include "frc/PIDSource.h"
17 #include "frc/Timer.h"
18 #include "frc/interfaces/Gyro.h"
19 #include "ITimestampedDataSubscriber.h"
20 #include "networktables/NetworkTableEntry.h"
23 #include <hal/SimDevice.h>
26 class ContinuousAngleTracker;
27 class InertialDataIntegrator;
33 class AHRS :
public frc::SendableBase,
34 public frc::ErrorBase,
35 public frc::PIDSource,
71 friend class AHRSInternal;
72 AHRSInternal * ahrs_internal;
77 volatile float compass_heading;
78 volatile float world_linear_accel_x;
79 volatile float world_linear_accel_y;
80 volatile float world_linear_accel_z;
81 volatile float mpu_temp_c;
82 volatile float fused_heading;
83 volatile float altitude;
84 volatile float baro_pressure;
85 volatile bool is_moving;
86 volatile bool is_rotating;
87 volatile float baro_sensor_temp_c;
88 volatile bool altitude_valid;
89 volatile bool is_magnetometer_calibrated;
90 volatile bool magnetic_disturbance;
91 volatile float quaternionW;
92 volatile float quaternionX;
93 volatile float quaternionY;
94 volatile float quaternionZ;
98 float displacement[3];
102 volatile int16_t raw_gyro_x;
103 volatile int16_t raw_gyro_y;
104 volatile int16_t raw_gyro_z;
105 volatile int16_t raw_accel_x;
106 volatile int16_t raw_accel_y;
107 volatile int16_t raw_accel_z;
108 volatile int16_t cal_mag_x;
109 volatile int16_t cal_mag_y;
110 volatile int16_t cal_mag_z;
113 volatile uint8_t update_rate_hz;
114 volatile int16_t accel_fsr_g;
115 volatile int16_t gyro_fsr_dps;
116 volatile int16_t capability_flags;
117 volatile uint8_t op_status;
118 volatile int16_t sensor_status;
119 volatile uint8_t cal_status;
120 volatile uint8_t selftest_status;
123 volatile uint8_t board_type;
124 volatile uint8_t hw_rev;
125 volatile uint8_t fw_ver_major;
126 volatile uint8_t fw_ver_minor;
128 long last_sensor_timestamp;
129 double last_update_time;
131 InertialDataIntegrator *integrator;
132 ContinuousAngleTracker *yaw_angle_tracker;
133 OffsetTracker * yaw_offset_tracker;
139 hal::SimDevice m_simDevice;
141 #define MAX_NUM_CALLBACKS 3
142 ITimestampedDataSubscriber *callbacks[MAX_NUM_CALLBACKS];
143 void *callback_contexts[MAX_NUM_CALLBACKS];
145 bool enable_boardlevel_yawreset;
146 double last_yawreset_request_timestamp;
147 double last_yawreset_while_calibrating_request_timestamp;
148 uint32_t successive_suppressed_yawreset_request_count;
149 bool disconnect_startupcalibration_recovery_pending;
150 bool logging_enabled;
153 AHRS(frc::SPI::Port spi_port_id);
154 AHRS(frc::I2C::Port i2c_port_id);
155 AHRS(frc::SerialPort::Port serial_port_id);
157 AHRS(frc::SPI::Port spi_port_id, uint8_t update_rate_hz);
158 AHRS(frc::SPI::Port spi_port_id, uint32_t spi_bitrate, uint8_t update_rate_hz);
160 AHRS(frc::I2C::Port i2c_port_id, uint8_t update_rate_hz);
191 int update_rate_hz,
bool is_moving );
199 double GetRate()
const override;
202 void Reset()
override;
217 bool RegisterCallback( ITimestampedDataSubscriber *callback,
void *callback_context);
234 void SPIInit( frc::SPI::Port spi_port_id, uint32_t bitrate, uint8_t update_rate_hz );
235 void I2CInit( frc::I2C::Port i2c_port_id, uint8_t update_rate_hz );
236 void SerialInit(frc::SerialPort::Port serial_port_id,
AHRS::SerialDataType data_type, uint8_t update_rate_hz);
237 void commonInit( uint8_t update_rate_hz );
238 static int ThreadFunc(IIOProvider *io_provider);
241 void InitSendable(frc::SendableBuilder& builder)
override;
246 uint8_t GetActualUpdateRateInternal(uint8_t update_rate);
248 nt::NetworkTableEntry m_valueEntry;
Class providing an "Attitude and Heading Reference System" (AHRS) interface to a navX-sensor.
Definition: AHRS.h:36
void EnableLogging(bool enable)
Enables or disables logging (via Console I/O) of AHRS library internal behaviors, including events su...
Definition: AHRS.cpp:1024
bool IsAltitudeValid()
Indicates whether the current altitude (and barometric pressure) data is valid.
Definition: AHRS.cpp:791
void UpdateDisplacement(float accel_x_g, float accel_y_g, int update_rate_hz, bool is_moving)
Each time new linear acceleration samples are received, this function should be invoked.
Definition: AHRS.cpp:936
void SetAngleAdjustment(double angle)
Sets an amount of angle to be automatically added before returning a angle from the getAngle() method...
Definition: AHRS.cpp:1273
float GetRawGyroZ()
Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec).
Definition: AHRS.cpp:1334
bool IsCalibrating()
Returns true if the sensor is currently performing automatic gyro/accelerometer calibration.
Definition: AHRS.cpp:619
double GetUpdateCount()
Returns the count of valid updates which have been received from the sensor.
Definition: AHRS.cpp:657
double GetByteCount()
Returns the count in bytes of data received from the sensor.
Definition: AHRS.cpp:647
float GetRawMagZ()
Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla).
Definition: AHRS.cpp:1414
float GetVelocityY()
Returns the velocity (in meters/sec) of the Y axis [Experimental].
Definition: AHRS.cpp:961
SerialDataType
navX-Sensor support Serial Data types
Definition: AHRS.h:59
@ kProcessedData
(default): 6 and 9-axis processed data
Definition: AHRS.h:63
@ kRawData
unprocessed data from each individual sensor
Definition: AHRS.h:67
float GetDisplacementY()
Returns the displacement (in meters) of the Y axis since resetDisplacement() was last invoked [Experi...
Definition: AHRS.cpp:1001
float GetDisplacementZ()
Returns the displacement (in meters) of the Z axis since resetDisplacement() was last invoked [Experi...
Definition: AHRS.cpp:1015
float GetPressure()
Returns the current barometric pressure (in millibar) [navX Aero only].
Definition: AHRS.cpp:1425
float GetYaw()
Returns the current yaw value (in degrees, from -180 to 180) reported by the sensor.
Definition: AHRS.cpp:525
float GetFusedHeading()
Returns the "fused" (9-axis) heading.
Definition: AHRS.cpp:811
int GetActualUpdateRate()
Returns the navX-Model device's currently configured update rate.
Definition: AHRS.cpp:1587
bool IsMoving()
Indicates if the sensor is currently detecting motion, based upon the X and Y-axis world linear accel...
Definition: AHRS.cpp:730
float GetWorldLinearAccelZ()
Returns the current linear acceleration in the Z-axis (in G).
Definition: AHRS.cpp:717
float GetBarometricPressure()
Returns the current barometric pressure, based upon calibrated readings from the onboard pressure sen...
Definition: AHRS.cpp:759
void Calibrate() override
Gyro interface implementation.
Definition: AHRS.cpp:1618
float GetVelocityZ()
Returns the velocity (in meters/sec) of the Z axis [Experimental].
Definition: AHRS.cpp:973
float GetTempC()
Returns the current temperature (in degrees centigrade) reported by the sensor's gyro/accelerometer c...
Definition: AHRS.cpp:1440
bool DeregisterCallback(ITimestampedDataSubscriber *callback)
Deregisters a previously registered callback interface.
Definition: AHRS.cpp:1558
bool IsRotating()
Indicates if the sensor is currently detecting yaw rotation, based upon whether the change in yaw ove...
Definition: AHRS.cpp:746
bool IsConnected()
Indicates whether the sensor is currently connected to the host computer.
Definition: AHRS.cpp:633
float GetRawGyroY()
Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec).
Definition: AHRS.cpp:1322
float GetRawAccelZ()
Returns the current raw (unprocessed) Z-axis acceleration rate (in G).
Definition: AHRS.cpp:1373
float GetRoll()
Returns the current roll value (in degrees, from -180 to 180) reported by the sensor.
Definition: AHRS.cpp:511
float GetRawMagX()
Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla).
Definition: AHRS.cpp:1388
float GetRawMagY()
Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla).
Definition: AHRS.cpp:1401
float GetPitch()
Returns the current pitch value (in degrees, from -180 to 180) reported by the sensor.
Definition: AHRS.cpp:501
float GetQuaternionW()
Returns the imaginary portion (W) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:860
float GetQuaternionX()
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:875
BoardAxis
Enumeration of all board axes.
Definition: AHRS.h:40
@ kBoardAxisZ
Board Z (up/down) Axis.
Definition: AHRS.h:46
@ kBoardAxisX
Board X (left/right) Axis.
Definition: AHRS.h:42
@ kBoardAxisY
Board Y (forward/reverse) Axis.
Definition: AHRS.h:44
void EnableBoardlevelYawReset(bool enable)
Enables or disables board-level yaw zero (reset) requests.
Definition: AHRS.cpp:1044
long GetLastSensorTimestamp()
Returns the sensor timestamp corresponding to the last sample retrieved from the sensor.
Definition: AHRS.cpp:670
bool IsMagneticDisturbance()
Indicates whether the current magnetic field strength diverges from the calibrated value for the eart...
Definition: AHRS.cpp:825
float GetRawGyroX()
Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec).
Definition: AHRS.cpp:1310
float GetDisplacementX()
Returns the displacement (in meters) of the X axis since resetDisplacement() was last invoked [Experi...
Definition: AHRS.cpp:987
AHRS(frc::SPI::Port spi_port_id)
Constructs the AHRS class using SPI communication and the default update rate.
Definition: AHRS.cpp:462
float GetRawAccelX()
Returns the current raw (unprocessed) X-axis acceleration rate (in G).
Definition: AHRS.cpp:1347
void Reset() override
Reset the Yaw gyro.
Definition: AHRS.cpp:1296
int16_t GetAccelFullScaleRangeG()
Returns the sensor full scale range (in G) of the X, Y and X-axis accelerometers.
Definition: AHRS.cpp:1074
int GetRequestedUpdateRate()
Returns the currently requested update rate.
Definition: AHRS.cpp:1613
double GetRate() const override
Return the rate of rotation of the yaw (Z-axis) gyro, in degrees per second.
Definition: AHRS.cpp:1254
bool IsMagnetometerCalibrated()
Indicates whether the magnetometer has been calibrated.
Definition: AHRS.cpp:841
void ResetDisplacement()
Zeros the displacement integration variables.
Definition: AHRS.cpp:919
float GetRawAccelY()
Returns the current raw (unprocessed) Y-axis acceleration rate (in G).
Definition: AHRS.cpp:1360
float GetQuaternionY()
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:893
AHRS::BoardYawAxis GetBoardYawAxis()
Returns information regarding which sensor board axis (X,Y or Z) and direction (up/down) is currently...
Definition: AHRS.cpp:1457
double GetAngle() const override
Returns the total accumulated yaw angle (Z Axis, in degrees) reported by the sensor.
Definition: AHRS.cpp:1242
void ZeroYaw()
Sets the user-specified yaw offset to the current yaw value reported by the sensor.
Definition: AHRS.cpp:566
double GetAngleAdjustment()
Returns the currently configured adjustment angle.
Definition: AHRS.cpp:1285
float GetWorldLinearAccelX()
Returns the current linear acceleration in the X-axis (in G).
Definition: AHRS.cpp:685
bool IsBoardlevelYawResetEnabled()
Returns true if Board-level yaw resets are enabled.
Definition: AHRS.cpp:1054
float GetWorldLinearAccelY()
Returns the current linear acceleration in the Y-axis (in G).
Definition: AHRS.cpp:701
bool RegisterCallback(ITimestampedDataSubscriber *callback, void *callback_context)
Registers a callback interface.
Definition: AHRS.cpp:1537
float GetVelocityX()
Returns the velocity (in meters/sec) of the X axis [Experimental].
Definition: AHRS.cpp:949
int16_t GetGyroFullScaleRangeDPS()
Returns the sensor full scale range (in degrees per second) of the X, Y and X-axis gyroscopes.
Definition: AHRS.cpp:1064
float GetQuaternionZ()
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:911
float GetCompassHeading()
Returns the current tilt-compensated compass heading value (in degrees, from 0 to 360) reported by th...
Definition: AHRS.cpp:547
float GetAltitude()
Returns the current altitude, based upon calibrated readings from a barometric pressure sensor,...
Definition: AHRS.cpp:776
std::string GetFirmwareVersion()
Returns the version number of the firmware currently executing on the sensor.
Definition: AHRS.cpp:1493
Structure describing the current board orientation w/respect to the IMU axes of measurement.
Definition: AHRS.h:51
bool up
true if axis is pointing up (with respect to gravity); false if pointing down.
Definition: AHRS.h:55
BoardAxis board_axis
Identifies one of the board axes.
Definition: AHRS.h:53