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 "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 "ITimestampedDataSubscriber.h"
19 #include "networktables/NetworkTableEntry.h"
20 #include <thread>
21 
22 class IIOProvider;
23 class ContinuousAngleTracker;
24 class InertialDataIntegrator;
25 class OffsetTracker;
26 class AHRSInternal;
27 
28 using namespace frc;
29 
30 class AHRS : public SendableBase,
31  public ErrorBase,
32  public PIDSource {
33 public:
34 
35  enum BoardAxis {
36  kBoardAxisX = 0,
37  kBoardAxisY = 1,
38  kBoardAxisZ = 2,
39  };
40 
41  struct BoardYawAxis
42  {
43  /* Identifies one of the board axes */
44  BoardAxis board_axis;
45  /* true if axis is pointing up (with respect to gravity); false if pointing down. */
46  bool up;
47  };
48 
53  kProcessedData = 0,
57  kRawData = 1
58  };
59 
60 private:
61  friend class AHRSInternal;
62  AHRSInternal * ahrs_internal;
63 
64  volatile float yaw;
65  volatile float pitch;
66  volatile float roll;
67  volatile float compass_heading;
68  volatile float world_linear_accel_x;
69  volatile float world_linear_accel_y;
70  volatile float world_linear_accel_z;
71  volatile float mpu_temp_c;
72  volatile float fused_heading;
73  volatile float altitude;
74  volatile float baro_pressure;
75  volatile bool is_moving;
76  volatile bool is_rotating;
77  volatile float baro_sensor_temp_c;
78  volatile bool altitude_valid;
79  volatile bool is_magnetometer_calibrated;
80  volatile bool magnetic_disturbance;
81  volatile float quaternionW;
82  volatile float quaternionX;
83  volatile float quaternionY;
84  volatile float quaternionZ;
85 
86  /* Integrated Data */
87  float velocity[3];
88  float displacement[3];
89 
90 
91  /* Raw Data */
92  volatile int16_t raw_gyro_x;
93  volatile int16_t raw_gyro_y;
94  volatile int16_t raw_gyro_z;
95  volatile int16_t raw_accel_x;
96  volatile int16_t raw_accel_y;
97  volatile int16_t raw_accel_z;
98  volatile int16_t cal_mag_x;
99  volatile int16_t cal_mag_y;
100  volatile int16_t cal_mag_z;
101 
102  /* Configuration/Status */
103  volatile uint8_t update_rate_hz;
104  volatile int16_t accel_fsr_g;
105  volatile int16_t gyro_fsr_dps;
106  volatile int16_t capability_flags;
107  volatile uint8_t op_status;
108  volatile int16_t sensor_status;
109  volatile uint8_t cal_status;
110  volatile uint8_t selftest_status;
111 
112  /* Board ID */
113  volatile uint8_t board_type;
114  volatile uint8_t hw_rev;
115  volatile uint8_t fw_ver_major;
116  volatile uint8_t fw_ver_minor;
117 
118  long last_sensor_timestamp;
119  double last_update_time;
120 
121  InertialDataIntegrator *integrator;
122  ContinuousAngleTracker *yaw_angle_tracker;
123  OffsetTracker * yaw_offset_tracker;
124  IIOProvider * io;
125 
126  std::thread * task;
127 
128 #define MAX_NUM_CALLBACKS 3
129  ITimestampedDataSubscriber *callbacks[MAX_NUM_CALLBACKS];
130  void *callback_contexts[MAX_NUM_CALLBACKS];
131 
132  bool enable_boardlevel_yawreset;
133  double last_yawreset_request_timestamp;
134  double last_yawreset_while_calibrating_request_timestamp;
135  uint32_t successive_suppressed_yawreset_request_count;
136  bool disconnect_startupcalibration_recovery_pending;
137  bool logging_enabled;
138 
139 public:
140  AHRS(SPI::Port spi_port_id);
141  AHRS(I2C::Port i2c_port_id);
142  AHRS(SerialPort::Port serial_port_id);
143 
144  AHRS(SPI::Port spi_port_id, uint8_t update_rate_hz);
145  AHRS(SPI::Port spi_port_id, uint32_t spi_bitrate, uint8_t update_rate_hz);
146 
147  AHRS(I2C::Port i2c_port_id, uint8_t update_rate_hz);
148 
149  AHRS(SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
150 
151  float GetPitch();
152  float GetRoll();
153  float GetYaw();
154  float GetCompassHeading();
155  void ZeroYaw();
156  bool IsCalibrating();
157  bool IsConnected();
158  double GetByteCount();
159  double GetUpdateCount();
160  long GetLastSensorTimestamp();
161  float GetWorldLinearAccelX();
162  float GetWorldLinearAccelY();
163  float GetWorldLinearAccelZ();
164  bool IsMoving();
165  bool IsRotating();
166  float GetBarometricPressure();
167  float GetAltitude();
168  bool IsAltitudeValid();
169  float GetFusedHeading();
170  bool IsMagneticDisturbance();
171  bool IsMagnetometerCalibrated();
172  float GetQuaternionW();
173  float GetQuaternionX();
174  float GetQuaternionY();
175  float GetQuaternionZ();
176  void ResetDisplacement();
177  void UpdateDisplacement( float accel_x_g, float accel_y_g,
178  int update_rate_hz, bool is_moving );
179  float GetVelocityX();
180  float GetVelocityY();
181  float GetVelocityZ();
182  float GetDisplacementX();
183  float GetDisplacementY();
184  float GetDisplacementZ();
185  double GetAngle();
186  double GetRate();
187  void SetAngleAdjustment(double angle);
188  double GetAngleAdjustment();
189  void Reset();
190  float GetRawGyroX();
191  float GetRawGyroY();
192  float GetRawGyroZ();
193  float GetRawAccelX();
194  float GetRawAccelY();
195  float GetRawAccelZ();
196  float GetRawMagX();
197  float GetRawMagY();
198  float GetRawMagZ();
199  float GetPressure();
200  float GetTempC();
201  AHRS::BoardYawAxis GetBoardYawAxis();
202  std::string GetFirmwareVersion();
203 
204  bool RegisterCallback( ITimestampedDataSubscriber *callback, void *callback_context);
205  bool DeregisterCallback( ITimestampedDataSubscriber *callback );
206 
207  int GetActualUpdateRate();
208  int GetRequestedUpdateRate();
209 
210  void EnableLogging(bool enable);
211  void EnableBoardlevelYawReset(bool enable);
212  bool IsBoardlevelYawResetEnabled();
213 
214  int16_t GetGyroFullScaleRangeDPS();
215  int16_t GetAccelFullScaleRangeG();
216 
217 private:
218  void SPIInit( SPI::Port spi_port_id, uint32_t bitrate, uint8_t update_rate_hz );
219  void I2CInit( I2C::Port i2c_port_id, uint8_t update_rate_hz );
220  void SerialInit(SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
221  void commonInit( uint8_t update_rate_hz );
222  static int ThreadFunc(IIOProvider *io_provider);
223 
224  /* SendableBase implementation */
225  void InitSendable(SendableBuilder& builder) override;
226 
227  /* PIDSource implementation */
228  double PIDGet();
229 
230  uint8_t GetActualUpdateRateInternal(uint8_t update_rate);
231 
232  nt::NetworkTableEntry m_valueEntry;
233 };
234 
235 #endif /* SRC_AHRS_H_ */
SerialDataType
Definition: AHRS.h:49
Definition: AHRS.h:41
Definition: AHRS.h:30