navX MXP C++ Class Library for RoboRIO
navX MXP Robotics Navigation Sensor
1 /*
2  * AHRS.h
3  *
4  * Created on: Jul 30, 2015
5  * Author: Scott
6  */
8 #ifndef SRC_AHRS_H_
9 #define SRC_AHRS_H_
11 #include "frc/smartdashboard/SendableBase.h"
12 #include "frc/smartdashboard/SendableBuilder.h"
13 #include "frc/I2C.h"
14 #include "frc/SPI.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"
21 #include <thread>
23 #include <hal/SimDevice.h>
25 class IIOProvider;
26 class ContinuousAngleTracker;
27 class InertialDataIntegrator;
28 class OffsetTracker;
29 class AHRSInternal;
33 class AHRS : public frc::SendableBase,
34  public frc::ErrorBase,
35  public frc::PIDSource,
36  public frc::Gyro {
37 public:
40  enum BoardAxis {
47  };
50  struct BoardYawAxis
51  {
55  bool up;
56  };
67  kRawData = 1
68  };
70 private:
71  friend class AHRSInternal;
72  AHRSInternal * ahrs_internal;
74  volatile float yaw;
75  volatile float pitch;
76  volatile float roll;
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;
96  /* Integrated Data */
97  float velocity[3];
98  float displacement[3];
101  /* Raw Data */
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;
112  /* Configuration/Status */
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;
122  /* Board ID */
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;
134  IIOProvider * io;
136  std::thread * task;
138  // Simulation
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;
152 public:
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);
162  AHRS(frc::SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
164  float GetPitch();
165  float GetRoll();
166  float GetYaw();
167  float GetCompassHeading();
168  void ZeroYaw();
169  bool IsCalibrating();
170  bool IsConnected();
171  double GetByteCount();
172  double GetUpdateCount();
173  long GetLastSensorTimestamp();
174  float GetWorldLinearAccelX();
175  float GetWorldLinearAccelY();
176  float GetWorldLinearAccelZ();
177  bool IsMoving();
178  bool IsRotating();
179  float GetBarometricPressure();
180  float GetAltitude();
181  bool IsAltitudeValid();
182  float GetFusedHeading();
183  bool IsMagneticDisturbance();
185  float GetQuaternionW();
186  float GetQuaternionX();
187  float GetQuaternionY();
188  float GetQuaternionZ();
189  void ResetDisplacement();
190  void UpdateDisplacement( float accel_x_g, float accel_y_g,
191  int update_rate_hz, bool is_moving );
192  float GetVelocityX();
193  float GetVelocityY();
194  float GetVelocityZ();
195  float GetDisplacementX();
196  float GetDisplacementY();
197  float GetDisplacementZ();
198  double GetAngle() const override;
199  double GetRate() const override;
200  void SetAngleAdjustment(double angle);
201  double GetAngleAdjustment();
202  void Reset() override;
203  float GetRawGyroX();
204  float GetRawGyroY();
205  float GetRawGyroZ();
206  float GetRawAccelX();
207  float GetRawAccelY();
208  float GetRawAccelZ();
209  float GetRawMagX();
210  float GetRawMagY();
211  float GetRawMagZ();
212  float GetPressure();
213  float GetTempC();
215  std::string GetFirmwareVersion();
217  bool RegisterCallback( ITimestampedDataSubscriber *callback, void *callback_context);
218  bool DeregisterCallback( ITimestampedDataSubscriber *callback );
220  int GetActualUpdateRate();
223  void EnableLogging(bool enable);
224  void EnableBoardlevelYawReset(bool enable);
227  int16_t GetGyroFullScaleRangeDPS();
228  int16_t GetAccelFullScaleRangeG();
231  void Calibrate() override;
233 private:
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);
240  /* SendableBase implementation */
241  void InitSendable(frc::SendableBuilder& builder) override;
243  /* PIDSource implementation */
244  double PIDGet();
246  uint8_t GetActualUpdateRateInternal(uint8_t update_rate);
248  nt::NetworkTableEntry m_valueEntry;
249 };
251 #endif /* SRC_AHRS_H_ */
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
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
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