navX MXP C++ Class Library for RoboRIO
navX MXP Robotics Navigation Sensor
AHRS.h
1 /*
2  * AHRS.h
3  *
4  * Created on: Jul 30, 2015
5  * Author: Scott
6  */
7 
8 #ifndef SRC_AHRS_H_
9 #define SRC_AHRS_H_
10 
11 #include "WPILib.h"
12 #include "ITimestampedDataSubscriber.h"
13 #include <thread>
14 
15 class IIOProvider;
16 class ContinuousAngleTracker;
17 class InertialDataIntegrator;
18 class OffsetTracker;
19 class AHRSInternal;
20 
21 class AHRS : public SensorBase,
22  public LiveWindowSendable,
23  public PIDSource {
24 public:
25 
26  enum BoardAxis {
27  kBoardAxisX = 0,
28  kBoardAxisY = 1,
29  kBoardAxisZ = 2,
30  };
31 
32  struct BoardYawAxis
33  {
34  /* Identifies one of the board axes */
35  BoardAxis board_axis;
36  /* true if axis is pointing up (with respect to gravity); false if pointing down. */
37  bool up;
38  };
39 
49  };
50 
51 private:
52  friend class AHRSInternal;
53  AHRSInternal * ahrs_internal;
54 
55  volatile float yaw;
56  volatile float pitch;
57  volatile float roll;
58  volatile float compass_heading;
59  volatile float world_linear_accel_x;
60  volatile float world_linear_accel_y;
61  volatile float world_linear_accel_z;
62  volatile float mpu_temp_c;
63  volatile float fused_heading;
64  volatile float altitude;
65  volatile float baro_pressure;
66  volatile bool is_moving;
67  volatile bool is_rotating;
68  volatile float baro_sensor_temp_c;
69  volatile bool altitude_valid;
70  volatile bool is_magnetometer_calibrated;
71  volatile bool magnetic_disturbance;
72  volatile float quaternionW;
73  volatile float quaternionX;
74  volatile float quaternionY;
75  volatile float quaternionZ;
76 
77  /* Integrated Data */
78  float velocity[3];
79  float displacement[3];
80 
81 
82  /* Raw Data */
83  volatile int16_t raw_gyro_x;
84  volatile int16_t raw_gyro_y;
85  volatile int16_t raw_gyro_z;
86  volatile int16_t raw_accel_x;
87  volatile int16_t raw_accel_y;
88  volatile int16_t raw_accel_z;
89  volatile int16_t cal_mag_x;
90  volatile int16_t cal_mag_y;
91  volatile int16_t cal_mag_z;
92 
93  /* Configuration/Status */
94  volatile uint8_t update_rate_hz;
95  volatile int16_t accel_fsr_g;
96  volatile int16_t gyro_fsr_dps;
97  volatile int16_t capability_flags;
98  volatile uint8_t op_status;
99  volatile int16_t sensor_status;
100  volatile uint8_t cal_status;
101  volatile uint8_t selftest_status;
102 
103  /* Board ID */
104  volatile uint8_t board_type;
105  volatile uint8_t hw_rev;
106  volatile uint8_t fw_ver_major;
107  volatile uint8_t fw_ver_minor;
108 
109  long last_sensor_timestamp;
110  double last_update_time;
111 
112  std::shared_ptr<ITable> table;
113 
114  InertialDataIntegrator *integrator;
115  ContinuousAngleTracker *yaw_angle_tracker;
116  OffsetTracker * yaw_offset_tracker;
117  IIOProvider * io;
118 
119  std::thread * task;
120 
121 #define MAX_NUM_CALLBACKS 3
122  ITimestampedDataSubscriber *callbacks[MAX_NUM_CALLBACKS];
123  void *callback_contexts[MAX_NUM_CALLBACKS];
124 
125 public:
126  AHRS(SPI::Port spi_port_id);
127  AHRS(I2C::Port i2c_port_id);
128  AHRS(SerialPort::Port serial_port_id);
129 
130  AHRS(SPI::Port spi_port_id, uint8_t update_rate_hz);
131  AHRS(SPI::Port spi_port_id, uint32_t spi_bitrate, uint8_t update_rate_hz);
132 
133  AHRS(I2C::Port i2c_port_id, uint8_t update_rate_hz);
134 
135  AHRS(SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
136 
137  float GetPitch();
138  float GetRoll();
139  float GetYaw();
140  float GetCompassHeading();
141  void ZeroYaw();
142  bool IsCalibrating();
143  bool IsConnected();
144  double GetByteCount();
145  double GetUpdateCount();
146  long GetLastSensorTimestamp();
147  float GetWorldLinearAccelX();
148  float GetWorldLinearAccelY();
149  float GetWorldLinearAccelZ();
150  bool IsMoving();
151  bool IsRotating();
152  float GetBarometricPressure();
153  float GetAltitude();
154  bool IsAltitudeValid();
155  float GetFusedHeading();
156  bool IsMagneticDisturbance();
158  float GetQuaternionW();
159  float GetQuaternionX();
160  float GetQuaternionY();
161  float GetQuaternionZ();
162  void ResetDisplacement();
163  void UpdateDisplacement( float accel_x_g, float accel_y_g,
164  int update_rate_hz, bool is_moving );
165  float GetVelocityX();
166  float GetVelocityY();
167  float GetVelocityZ();
168  float GetDisplacementX();
169  float GetDisplacementY();
170  float GetDisplacementZ();
171  double GetAngle();
172  double GetRate();
173  void Reset();
174  float GetRawGyroX();
175  float GetRawGyroY();
176  float GetRawGyroZ();
177  float GetRawAccelX();
178  float GetRawAccelY();
179  float GetRawAccelZ();
180  float GetRawMagX();
181  float GetRawMagY();
182  float GetRawMagZ();
183  float GetPressure();
184  float GetTempC();
186  std::string GetFirmwareVersion();
187 
188  bool RegisterCallback( ITimestampedDataSubscriber *callback, void *callback_context);
189  bool DeregisterCallback( ITimestampedDataSubscriber *callback );
190 
191  int GetActualUpdateRate();
193 
194 private:
195  void SPIInit( SPI::Port spi_port_id, uint32_t bitrate, uint8_t update_rate_hz );
196  void I2CInit( I2C::Port i2c_port_id, uint8_t update_rate_hz );
197  void SerialInit(SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
198  void commonInit( uint8_t update_rate_hz );
199  static int ThreadFunc(IIOProvider *io_provider);
200 
201  /* LiveWindowSendable implementation */
202  void InitTable(std::shared_ptr<ITable> subtable);
203  std::shared_ptr<ITable> GetTable() const;
204  std::string GetSmartDashboardType() const;
205  void UpdateTable();
206  void StartLiveWindowMode();
207  void StopLiveWindowMode();
208 
209  /* PIDSource implementation */
210  double PIDGet();
211 
212  uint8_t GetActualUpdateRateInternal(uint8_t update_rate);
213 };
214 
215 #endif /* SRC_AHRS_H_ */
float GetRoll()
Returns the current roll value (in degrees, from -180 to 180) reported by the sensor.
Definition: AHRS.cpp:406
float GetWorldLinearAccelZ()
Returns the current linear acceleration in the Z-axis (in G).
Definition: AHRS.cpp:576
long GetLastSensorTimestamp()
Returns the sensor timestamp corresponding to the last sample retrieved from the sensor.
Definition: AHRS.cpp:529
bool DeregisterCallback(ITimestampedDataSubscriber *callback)
Deregisters a previously registered callback interface.
Definition: AHRS.cpp:1301
std::string GetFirmwareVersion()
Returns the version number of the firmware currently executing on the sensor.
Definition: AHRS.cpp:1214
(default): 6 and 9-axis processed data
Definition: AHRS.h:44
AHRS(SPI::Port spi_port_id)
Constructs the AHRS class using SPI communication and the default update rate.
Definition: AHRS.cpp:360
bool IsCalibrating()
Returns true if the sensor is currently performing automatic gyro/accelerometer calibration.
Definition: AHRS.cpp:478
float GetAltitude()
Returns the current altitude, based upon calibrated readings from a barometric pressure sensor...
Definition: AHRS.cpp:635
double GetRate()
Return the rate of rotation of the yaw (Z-axis) gyro, in degrees per second.
Definition: AHRS.cpp:1006
SerialDataType
Definition: AHRS.h:40
float GetTempC()
Returns the current temperature (in degrees centigrade) reported by the sensor&#39;s gyro/accelerometer c...
Definition: AHRS.cpp:1161
void ResetDisplacement()
Zeros the displacement integration variables.
Definition: AHRS.cpp:778
float GetVelocityZ()
Returns the velocity (in meters/sec) of the Z axis [Experimental].
Definition: AHRS.cpp:832
int GetActualUpdateRate()
Returns the navX-Model device&#39;s currently configured update rate.
Definition: AHRS.cpp:1330
bool IsMoving()
Indicates if the sensor is currently detecting motion, based upon the X and Y-axis world linear accel...
Definition: AHRS.cpp:589
unprocessed data from each individual sensor
Definition: AHRS.h:48
float GetFusedHeading()
Returns the "fused" (9-axis) heading.
Definition: AHRS.cpp:670
Definition: AHRS.h:32
float GetVelocityX()
Returns the velocity (in meters/sec) of the X axis [Experimental].
Definition: AHRS.cpp:808
void ZeroYaw()
Sets the user-specified yaw offset to the current yaw value reported by the sensor.
Definition: AHRS.cpp:454
float GetQuaternionY()
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:752
float GetDisplacementY()
Returns the displacement (in meters) of the Y axis since resetDisplacement() was last invoked [Experi...
Definition: AHRS.cpp:860
bool RegisterCallback(ITimestampedDataSubscriber *callback, void *callback_context)
Registers a callback interface.
Definition: AHRS.cpp:1280
float GetQuaternionW()
Returns the imaginary portion (W) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:719
bool IsMagneticDisturbance()
Indicates whether the current magnetic field strength diverges from the calibrated value for the eart...
Definition: AHRS.cpp:684
Definition: AHRS.h:21
float GetRawMagX()
Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla).
Definition: AHRS.cpp:1109
float GetRawAccelZ()
Returns the current raw (unprocessed) Z-axis acceleration rate (in G).
Definition: AHRS.cpp:1094
float GetRawMagY()
Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla).
Definition: AHRS.cpp:1122
void Reset()
Reset the Yaw gyro.
Definition: AHRS.cpp:1017
float GetQuaternionZ()
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:770
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:795
float GetBarometricPressure()
Returns the current barometric pressure, based upon calibrated readings from the onboard pressure sen...
Definition: AHRS.cpp:618
bool IsAltitudeValid()
Indicates whether the current altitude (and barometric pressure) data is valid.
Definition: AHRS.cpp:650
bool IsConnected()
Indicates whether the sensor is currently connected to the host computer.
Definition: AHRS.cpp:492
float GetVelocityY()
Returns the velocity (in meters/sec) of the Y axis [Experimental].
Definition: AHRS.cpp:820
AHRS::BoardYawAxis GetBoardYawAxis()
Returns information regarding which sensor board axis (X,Y or Z) and direction (up/down) is currently...
Definition: AHRS.cpp:1178
float GetWorldLinearAccelY()
Returns the current linear acceleration in the Y-axis (in G).
Definition: AHRS.cpp:560
float GetCompassHeading()
Returns the current tilt-compensated compass heading value (in degrees, from 0 to 360) reported by th...
Definition: AHRS.cpp:442
float GetRawGyroY()
Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec). ...
Definition: AHRS.cpp:1043
float GetRawAccelX()
Returns the current raw (unprocessed) X-axis acceleration rate (in G).
Definition: AHRS.cpp:1068
float GetRawMagZ()
Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla).
Definition: AHRS.cpp:1135
float GetRawGyroX()
Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec). ...
Definition: AHRS.cpp:1031
float GetDisplacementX()
Returns the displacement (in meters) of the X axis since resetDisplacement() was last invoked [Experi...
Definition: AHRS.cpp:846
bool IsMagnetometerCalibrated()
Indicates whether the magnetometer has been calibrated.
Definition: AHRS.cpp:700
float GetRawAccelY()
Returns the current raw (unprocessed) Y-axis acceleration rate (in G).
Definition: AHRS.cpp:1081
bool IsRotating()
Indicates if the sensor is currently detecting yaw rotation, based upon whether the change in yaw ove...
Definition: AHRS.cpp:605
float GetYaw()
Returns the current yaw value (in degrees, from -180 to 180) reported by the sensor.
Definition: AHRS.cpp:420
float GetWorldLinearAccelX()
Returns the current linear acceleration in the X-axis (in G).
Definition: AHRS.cpp:544
double GetUpdateCount()
Returns the count of valid updates which have been received from the sensor.
Definition: AHRS.cpp:516
float GetPitch()
Returns the current pitch value (in degrees, from -180 to 180) reported by the sensor.
Definition: AHRS.cpp:396
double GetAngle()
Returns the total accumulated yaw angle (Z Axis, in degrees) reported by the sensor.
Definition: AHRS.cpp:994
float GetQuaternionX()
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sen...
Definition: AHRS.cpp:734
float GetDisplacementZ()
Returns the displacement (in meters) of the Z axis since resetDisplacement() was last invoked [Experi...
Definition: AHRS.cpp:874
float GetRawGyroZ()
Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec). ...
Definition: AHRS.cpp:1055
double GetByteCount()
Returns the count in bytes of data received from the sensor.
Definition: AHRS.cpp:506
int GetRequestedUpdateRate()
Returns the currently requested update rate.
Definition: AHRS.cpp:1356
float GetPressure()
Returns the current barometric pressure (in millibar) [navX Aero only].
Definition: AHRS.cpp:1146