VMX-pi C++ HAL Library for Raspberry Pi
VMX-pi Robotics Controller & Vision/Motion Processor
|
The AHRS class provides accesst to the VMX-pi IMU and Attitude/Heading Reference System functionality. More...
#include <AHRS.h>
Classes | |
struct | BoardYawAxis |
Describes the currently-configured Omnimount IMU configuration. More... | |
Public Types | |
enum | BoardAxis { kBoardAxisX = 0, kBoardAxisY = 1, kBoardAxisZ = 2 } |
enum | SerialDataType { kProcessedData = 0, kRawData = 1 } |
Public Member Functions | |
AHRS (SPIClient &client, PIGPIOClient &pigpio, uint8_t update_rate_hz) | |
float | GetPitch () |
Returns the current pitch value (in degrees, from -180 to 180) reported by the sensor. More... | |
float | GetRoll () |
Returns the current roll value (in degrees, from -180 to 180) reported by the sensor. More... | |
float | GetYaw () |
Returns the current yaw value (in degrees, from -180 to 180) reported by the sensor. More... | |
float | GetCompassHeading () |
Returns the current tilt-compensated compass heading value (in degrees, from 0 to 360) reported by the sensor. More... | |
void | ZeroYaw () |
Sets the user-specified yaw offset to the current yaw value reported by the sensor. More... | |
bool | IsCalibrating () |
Returns true if the sensor is currently performing automatic gyro/accelerometer calibration. More... | |
bool | IsConnected () |
Indicates whether the sensor is currently connected to the host computer. More... | |
double | GetByteCount () |
Returns the count in bytes of data received from the sensor. More... | |
double | GetUpdateCount () |
Returns the count of valid updates which have been received from the sensor. More... | |
long | GetLastSensorTimestamp () |
Returns the sensor timestamp corresponding to the last sample retrieved from the sensor. More... | |
float | GetWorldLinearAccelX () |
Returns the current linear acceleration in the X-axis (in G). More... | |
float | GetWorldLinearAccelY () |
Returns the current linear acceleration in the Y-axis (in G). More... | |
float | GetWorldLinearAccelZ () |
Returns the current linear acceleration in the Z-axis (in G). More... | |
bool | IsMoving () |
Indicates if the sensor is currently detecting motion, based upon the X and Y-axis world linear acceleration values. More... | |
bool | IsRotating () |
Indicates if the sensor is currently detecting yaw rotation, based upon whether the change in yaw over the last second exceeds the "Rotation Threshold.". More... | |
float | GetBarometricPressure () |
Returns the current barometric pressure, based upon calibrated readings from the onboard pressure sensor. More... | |
float | GetAltitude () |
Returns the current altitude, based upon calibrated readings from a barometric pressure sensor, and the currently-configured sea-level barometric pressure [navX Aero only]. More... | |
bool | IsAltitudeValid () |
Indicates whether the current altitude (and barometric pressure) data is valid. More... | |
float | GetFusedHeading () |
Returns the "fused" (9-axis) heading. More... | |
bool | IsMagneticDisturbance () |
Indicates whether the current magnetic field strength diverges from the calibrated value for the earth's magnetic field by more than the currently- configured Magnetic Disturbance Ratio. More... | |
bool | IsMagnetometerCalibrated () |
Indicates whether the magnetometer has been calibrated. More... | |
float | GetQuaternionW () |
Returns the imaginary portion (W) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed". More... | |
float | GetQuaternionX () |
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed". More... | |
float | GetQuaternionY () |
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed". More... | |
float | GetQuaternionZ () |
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed". More... | |
void | ResetDisplacement () |
Zeros the displacement integration variables. More... | |
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. More... | |
float | GetVelocityX () |
Returns the velocity (in meters/sec) of the X axis [Experimental]. More... | |
float | GetVelocityY () |
Returns the velocity (in meters/sec) of the Y axis [Experimental]. More... | |
float | GetVelocityZ () |
Returns the velocity (in meters/sec) of the Z axis [Experimental]. More... | |
float | GetDisplacementX () |
Returns the displacement (in meters) of the X axis since resetDisplacement() was last invoked [Experimental]. More... | |
float | GetDisplacementY () |
Returns the displacement (in meters) of the Y axis since resetDisplacement() was last invoked [Experimental]. More... | |
float | GetDisplacementZ () |
Returns the displacement (in meters) of the Z axis since resetDisplacement() was last invoked [Experimental]. More... | |
double | GetAngle () |
Returns the total accumulated yaw angle (Z Axis, in degrees) reported by the sensor. More... | |
double | GetRate () |
Return the rate of rotation of the yaw (Z-axis) gyro, in degrees per second. More... | |
void | Reset () |
Reset the Yaw gyro. More... | |
float | GetRawGyroX () |
Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec). More... | |
float | GetRawGyroY () |
Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec). More... | |
float | GetRawGyroZ () |
Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec). More... | |
float | GetRawAccelX () |
Returns the current raw (unprocessed) X-axis acceleration rate (in G). More... | |
float | GetRawAccelY () |
Returns the current raw (unprocessed) Y-axis acceleration rate (in G). More... | |
float | GetRawAccelZ () |
Returns the current raw (unprocessed) Z-axis acceleration rate (in G). More... | |
float | GetRawMagX () |
Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla). More... | |
float | GetRawMagY () |
Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla). More... | |
float | GetRawMagZ () |
Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla). More... | |
float | GetPressure () |
Returns the current barometric pressure (in millibar) [navX Aero only]. More... | |
float | GetTempC () |
Returns the current temperature (in degrees centigrade) reported by the sensor's gyro/accelerometer circuit. More... | |
AHRS::BoardYawAxis | GetBoardYawAxis () |
Returns information regarding which sensor board axis (X,Y or Z) and direction (up/down) is currently configured to report Yaw (Z) angle values. More... | |
std::string | GetFirmwareVersion () |
Returns the version number of the firmware currently executing on the sensor. More... | |
bool | RegisterCallback (ITimestampedDataSubscriber *callback, void *callback_context) |
Registers a callback interface. More... | |
bool | DeregisterCallback (ITimestampedDataSubscriber *callback) |
Deregisters a previously registered callback interface. More... | |
int | GetActualUpdateRate () |
Returns the navX-Model device's currently configured update rate. More... | |
int | GetRequestedUpdateRate () |
Returns the currently requested update rate. More... | |
void | Stop () |
The AHRS class provides accesst to the VMX-pi IMU and Attitude/Heading Reference System functionality.
enum AHRS::SerialDataType |
bool AHRS::DeregisterCallback | ( | ITimestampedDataSubscriber * | callback | ) |
Deregisters a previously registered callback interface.
Be sure to deregister any callback which have been previously registered, to ensure that the object implementing the callback interface does not continue to be accessed when no longer necessary.
int AHRS::GetActualUpdateRate | ( | ) |
Returns the navX-Model device's currently configured update rate.
Note that the update rate that can actually be realized is a value evenly divisible by the navX-Model device's internal motion processor sample clock (200Hz). Therefore, the rate that is returned may be lower than the requested sample rate.
The actual sample rate is rounded down to the nearest integer that is divisible by the number of Digital Motion Processor clock ticks. For instance, a request for 58 Hertz will result in an actual rate of 66Hz (200 / (200 / 58), using integer math.
float AHRS::GetAltitude | ( | ) |
Returns the current altitude, based upon calibrated readings from a barometric pressure sensor, and the currently-configured sea-level barometric pressure [navX Aero only].
This value is in units of meters.
NOTE: This value is only valid sensors including a pressure sensor. To determine whether this value is valid, see isAltitudeValid().
double AHRS::GetAngle | ( | ) |
Returns the total accumulated yaw angle (Z Axis, in degrees) reported by the sensor.
NOTE: The angle is continuous, meaning it's range is beyond 360 degrees. This ensures that algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
Note that the returned yaw value will be offset by a user-specified offset value; this user-specified offset value is set by invoking the zeroYaw() method.
float AHRS::GetBarometricPressure | ( | ) |
Returns the current barometric pressure, based upon calibrated readings from the onboard pressure sensor.
This value is in units of millibar.
NOTE: This value is only valid for a navX Aero. To determine whether this value is valid, see isAltitudeValid().
AHRS::BoardYawAxis AHRS::GetBoardYawAxis | ( | ) |
Returns information regarding which sensor board axis (X,Y or Z) and direction (up/down) is currently configured to report Yaw (Z) angle values.
NOTE: If the board firmware supports Omnimount, the board yaw axis/direction are configurable.
For more information on Omnimount, please see:
http://navx-mxp.kauailabs.com/navx-mxp/installation/omnimount/
double AHRS::GetByteCount | ( | ) |
Returns the count in bytes of data received from the sensor.
This could can be useful for diagnosing connectivity issues.
If the byte count is increasing, but the update count (see getUpdateCount()) is not, this indicates a software misconfiguration.
float AHRS::GetCompassHeading | ( | ) |
Returns the current tilt-compensated compass heading value (in degrees, from 0 to 360) reported by the sensor.
Note that this value is sensed by a magnetometer, which can be affected by nearby magnetic fields (e.g., the magnetic fields generated by nearby motors).
Before using this value, ensure that (a) the magnetometer has been calibrated and (b) that a magnetic disturbance is not taking place at the instant when the compass heading was generated.
float AHRS::GetDisplacementX | ( | ) |
Returns the displacement (in meters) of the X axis since resetDisplacement() was last invoked [Experimental].
NOTE: This feature is experimental. Displacement measures rely on double-integration of acceleration values from MEMS accelerometers which yield "noisy" values. The resulting displacement are not known to be very accurate, and the amount of error increases quickly as time progresses.
float AHRS::GetDisplacementY | ( | ) |
Returns the displacement (in meters) of the Y axis since resetDisplacement() was last invoked [Experimental].
NOTE: This feature is experimental. Displacement measures rely on double-integration of acceleration values from MEMS accelerometers which yield "noisy" values. The resulting displacement are not known to be very accurate, and the amount of error increases quickly as time progresses.
float AHRS::GetDisplacementZ | ( | ) |
Returns the displacement (in meters) of the Z axis since resetDisplacement() was last invoked [Experimental].
NOTE: This feature is experimental. Displacement measures rely on double-integration of acceleration values from MEMS accelerometers which yield "noisy" values. The resulting displacement are not known to be very accurate, and the amount of error increases quickly as time progresses.
std::string AHRS::GetFirmwareVersion | ( | ) |
Returns the version number of the firmware currently executing on the sensor.
To update the firmware to the latest version, please see:
http://navx-mxp.kauailabs.com/navx-mxp/support/updating-firmware/
float AHRS::GetFusedHeading | ( | ) |
Returns the "fused" (9-axis) heading.
The 9-axis heading is the fusion of the yaw angle, the tilt-corrected compass heading, and magnetic disturbance detection. Note that the magnetometer calibration procedure is required in order to achieve valid 9-axis headings.
The 9-axis Heading represents the sensor's best estimate of current heading, based upon the last known valid Compass Angle, and updated by the change in the Yaw Angle since the last known valid Compass Angle. The last known valid Compass Angle is updated whenever a Calibrated Compass Angle is read and the sensor has recently rotated less than the Compass Noise Bandwidth (~2 degrees).
long AHRS::GetLastSensorTimestamp | ( | ) |
Returns the sensor timestamp corresponding to the last sample retrieved from the sensor.
Note that this sensor timestamp is only provided when the Register-based IO methods (SPI, I2C) are used; sensor timestamps are not provided when Serial-based IO methods (TTL UART, USB) are used.
float AHRS::GetPitch | ( | ) |
Returns the current pitch value (in degrees, from -180 to 180) reported by the sensor.
Pitch is a measure of rotation around the X Axis.
float AHRS::GetPressure | ( | ) |
Returns the current barometric pressure (in millibar) [navX Aero only].
This value is valid only if a barometric pressure sensor is onboard.
float AHRS::GetQuaternionW | ( | ) |
Returns the imaginary portion (W) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed".
Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 to 2. This total range (4) can be associated with a unit circle, since each circle is comprised of 4 PI Radians.
For more information on Quaternions and their use, please see this definition.
float AHRS::GetQuaternionX | ( | ) |
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed".
Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 to 2. This total range (4) can be associated with a unit circle, since each circle is comprised of 4 PI Radians.
For more information on Quaternions and their use, please see this description.
float AHRS::GetQuaternionY | ( | ) |
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed".
Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 to 2. This total range (4) can be associated with a unit circle, since each circle is comprised of 4 PI Radians.
For more information on Quaternions and their use, please see:
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
float AHRS::GetQuaternionZ | ( | ) |
Returns the real portion (X axis) of the Orientation Quaternion which fully describes the current sensor orientation with respect to the reference angle defined as the angle at which the yaw was last "zeroed".
Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 to 2. This total range (4) can be associated with a unit circle, since each circle is comprised of 4 PI Radians.
For more information on Quaternions and their use, please see:
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
double AHRS::GetRate | ( | ) |
Return the rate of rotation of the yaw (Z-axis) gyro, in degrees per second.
The rate is based on the most recent reading of the yaw gyro angle.
float AHRS::GetRawAccelX | ( | ) |
Returns the current raw (unprocessed) X-axis acceleration rate (in G).
NOTE: this value is unprocessed, and should only be accessed by advanced users. This raw value has not had acceleration due to gravity removed from it, and has not been rotated to the world reference frame. Gravity-corrected, world reference frame-corrected X axis acceleration data is accessible via the GetWorldLinearAccelX() method.
float AHRS::GetRawAccelY | ( | ) |
Returns the current raw (unprocessed) Y-axis acceleration rate (in G).
NOTE: this value is unprocessed, and should only be accessed by advanced users. This raw value has not had acceleration due to gravity removed from it, and has not been rotated to the world reference frame. Gravity-corrected, world reference frame-corrected Y axis acceleration data is accessible via the GetWorldLinearAccelY() method.
float AHRS::GetRawAccelZ | ( | ) |
Returns the current raw (unprocessed) Z-axis acceleration rate (in G).
NOTE: this value is unprocessed, and should only be accessed by advanced users. This raw value has not had acceleration due to gravity removed from it, and has not been rotated to the world reference frame. Gravity-corrected, world reference frame-corrected Z axis acceleration data is accessible via the GetWorldLinearAccelZ() method.
float AHRS::GetRawGyroX | ( | ) |
Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec).
NOTE: this value is un-processed, and should only be accessed by advanced users. Typically, rotation about the X Axis is referred to as "Pitch". Calibrated and Integrated Pitch data is accessible via the GetPitch() method.
float AHRS::GetRawGyroY | ( | ) |
Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec).
NOTE: this value is un-processed, and should only be accessed by advanced users. Typically, rotation about the T Axis is referred to as "Roll". Calibrated and Integrated Pitch data is accessible via the GetRoll() method.
float AHRS::GetRawGyroZ | ( | ) |
Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec).
NOTE: this value is un-processed, and should only be accessed by advanced users. Typically, rotation about the T Axis is referred to as "Yaw". Calibrated and Integrated Pitch data is accessible via the GetYaw() method.
float AHRS::GetRawMagX | ( | ) |
Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla).
NOTE: this value is unprocessed, and should only be accessed by advanced users. This raw value has not been tilt-corrected, and has not been combined with the other magnetometer axis data to yield a compass heading. Tilt-corrected compass heading data is accessible via the GetCompassHeading() method.
float AHRS::GetRawMagY | ( | ) |
Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla).
NOTE: this value is unprocessed, and should only be accessed by advanced users. This raw value has not been tilt-corrected, and has not been combined with the other magnetometer axis data to yield a compass heading. Tilt-corrected compass heading data is accessible via the GetCompassHeading() method.
float AHRS::GetRawMagZ | ( | ) |
Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla).
NOTE: this value is unprocessed, and should only be accessed by advanced users. This raw value has not been tilt-corrected, and has not been combined with the other magnetometer axis data to yield a compass heading. Tilt-corrected compass heading data is accessible via the GetCompassHeading() method.
int AHRS::GetRequestedUpdateRate | ( | ) |
Returns the currently requested update rate.
rate. Note that not every update rate can actually be realized, since the actual update rate must be a value evenly divisible by the navX-Model device's internal motion processor sample clock (200Hz).
To determine the actual update rate, use the getActualUpdateRate() method.
float AHRS::GetRoll | ( | ) |
Returns the current roll value (in degrees, from -180 to 180) reported by the sensor.
Roll is a measure of rotation around the X Axis.
float AHRS::GetTempC | ( | ) |
Returns the current temperature (in degrees centigrade) reported by the sensor's gyro/accelerometer circuit.
This value may be useful in order to perform advanced temperature- correction of raw gyroscope and accelerometer values.
double AHRS::GetUpdateCount | ( | ) |
Returns the count of valid updates which have been received from the sensor.
This count should increase at the same rate indicated by the configured update rate.
float AHRS::GetVelocityX | ( | ) |
Returns the velocity (in meters/sec) of the X axis [Experimental].
NOTE: This feature is experimental. Velocity measures rely on integration of acceleration values from MEMS accelerometers which yield "noisy" values. The resulting velocities are not known to be very accurate.
float AHRS::GetVelocityY | ( | ) |
Returns the velocity (in meters/sec) of the Y axis [Experimental].
NOTE: This feature is experimental. Velocity measures rely on integration of acceleration values from MEMS accelerometers which yield "noisy" values. The resulting velocities are not known to be very accurate.
float AHRS::GetVelocityZ | ( | ) |
Returns the velocity (in meters/sec) of the Z axis [Experimental].
NOTE: This feature is experimental. Velocity measures rely on integration of acceleration values from MEMS accelerometers which yield "noisy" values. The resulting velocities are not known to be very accurate.
float AHRS::GetWorldLinearAccelX | ( | ) |
Returns the current linear acceleration in the X-axis (in G).
World linear acceleration refers to raw acceleration data, which has had the gravity component removed, and which has been rotated to the same reference frame as the current yaw value. The resulting value represents the current acceleration in the x-axis of the body (e.g., the robot) on which the sensor is mounted.
float AHRS::GetWorldLinearAccelY | ( | ) |
Returns the current linear acceleration in the Y-axis (in G).
World linear acceleration refers to raw acceleration data, which has had the gravity component removed, and which has been rotated to the same reference frame as the current yaw value. The resulting value represents the current acceleration in the Y-axis of the body (e.g., the robot) on which the sensor is mounted.
float AHRS::GetWorldLinearAccelZ | ( | ) |
Returns the current linear acceleration in the Z-axis (in G).
World linear acceleration refers to raw acceleration data, which has had the gravity component removed, and which has been rotated to the same reference frame as the current yaw value. The resulting value represents the current acceleration in the Z-axis of the body (e.g., the robot) on which the sensor is mounted.
float AHRS::GetYaw | ( | ) |
Returns the current yaw value (in degrees, from -180 to 180) reported by the sensor.
Yaw is a measure of rotation around the Z Axis (which is perpendicular to the earth).
Note that the returned yaw value will be offset by a user-specified offset value; this user-specified offset value is set by invoking the zeroYaw() method.
bool AHRS::IsAltitudeValid | ( | ) |
Indicates whether the current altitude (and barometric pressure) data is valid.
This value will only be true for a sensor with an onboard pressure sensor installed.
If this value is false for a board with an installed pressure sensor, this indicates a malfunction of the onboard pressure sensor.
bool AHRS::IsCalibrating | ( | ) |
Returns true if the sensor is currently performing automatic gyro/accelerometer calibration.
Automatic calibration occurs when the sensor is initially powered on, during which time the sensor should be held still, with the Z-axis pointing up (perpendicular to the earth).
NOTE: During this automatic calibration, the yaw, pitch and roll values returned may not be accurate.
Once calibration is complete, the sensor will automatically remove an internal yaw offset value from all reported values.
bool AHRS::IsConnected | ( | ) |
Indicates whether the sensor is currently connected to the host computer.
A connection is considered established whenever communication with the sensor has occurred recently.
bool AHRS::IsMagneticDisturbance | ( | ) |
Indicates whether the current magnetic field strength diverges from the calibrated value for the earth's magnetic field by more than the currently- configured Magnetic Disturbance Ratio.
This function will always return false if the sensor's magnetometer has not yet been calibrated; see isMagnetometerCalibrated().
bool AHRS::IsMagnetometerCalibrated | ( | ) |
Indicates whether the magnetometer has been calibrated.
Magnetometer Calibration must be performed by the user.
Note that if this function does indicate the magnetometer is calibrated, this does not necessarily mean that the calibration quality is sufficient to yield valid compass headings.
bool AHRS::IsMoving | ( | ) |
Indicates if the sensor is currently detecting motion, based upon the X and Y-axis world linear acceleration values.
If the sum of the absolute values of the X and Y axis exceed a "motion threshold", the motion state is indicated.
bool AHRS::IsRotating | ( | ) |
Indicates if the sensor is currently detecting yaw rotation, based upon whether the change in yaw over the last second exceeds the "Rotation Threshold.".
Yaw Rotation can occur either when the sensor is rotating, or when the sensor is not rotating AND the current gyro calibration is insufficiently calibrated to yield the standard yaw drift rate.
bool AHRS::RegisterCallback | ( | ITimestampedDataSubscriber * | callback, |
void * | callback_context | ||
) |
Registers a callback interface.
This interface will be called back when new data is available, based upon a change in the sensor timestamp.
Note that this callback will occur within the context of the device IO thread, which is not the same thread context the caller typically executes in.
void AHRS::Reset | ( | ) |
Reset the Yaw gyro.
Resets the Gyro Z (Yaw) axis to a heading of zero. This can be used if there is significant drift in the gyro and it needs to be recalibrated after it has been running.
void AHRS::ResetDisplacement | ( | ) |
Zeros the displacement integration variables.
Invoke this at the moment when integration begins.
void AHRS::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.
This function transforms acceleration in G to meters/sec^2, then converts this value to Velocity in meters/sec (based upon velocity in the previous sample). Finally, this value is converted to displacement in meters, and integrated.
void AHRS::ZeroYaw | ( | ) |
Sets the user-specified yaw offset to the current yaw value reported by the sensor.
This user-specified yaw offset is automatically subtracted from subsequent yaw values reported by the getYaw() method.