public class AHRS
extends SensorBase
Modifier and Type | Class and Description |
---|---|
static class |
AHRS.BoardAxis
Identifies one of the three sensing axes on the navX sensor board.
|
static class |
AHRS.BoardYawAxis
Indicates which sensor board axis is used as the "yaw" (gravity) axis.
|
static class |
AHRS.SerialDataType
For use with serial communications, the SerialDataType specifies the
type of data to be streamed from the sensor.
|
Constructor and Description |
---|
AHRS(I2C.Port i2c_port_id)
Constructs the AHRS class using I2C communication and the default update rate.
|
AHRS(I2C.Port i2c_port_id,
byte update_rate_hz)
Constructs the AHRS class using I2C communication, overriding the
default update rate with a custom rate which may be from 4 to 60,
representing the number of updates per second sent by the sensor.
|
AHRS(SerialPort.Port serial_port_id)
Constructs the AHRS class using serial communication and the default update rate,
and returning processed (rather than raw) data.
|
AHRS(SerialPort.Port serial_port_id,
AHRS.SerialDataType data_type,
byte update_rate_hz)
Constructs the AHRS class using serial communication, overriding the
default update rate with a custom rate which may be from 4 to 60,
representing the number of updates per second sent by the sensor.
|
AHRS(SPI.Port spi_port_id)
Constructs the AHRS class using SPI communication and the default update rate.
|
AHRS(SPI.Port spi_port_id,
byte update_rate_hz)
Constructs the AHRS class using SPI communication, overriding the
default update rate with a custom rate which may be from 4 to 60,
representing the number of updates per second sent by the sensor.
|
AHRS(SPI.Port spi_port_id,
int spi_bitrate,
byte update_rate_hz)
The AHRS class provides an interface to AHRS capabilities
of the KauaiLabs navX Robotics Navigation Sensor via SPI, I2C and
Serial (TTL UART and USB) communications interfaces on the RoboRIO.
|
Modifier and Type | Method and Description |
---|---|
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].
|
double |
getAngle()
Returns the total accumulated yaw angle (Z Axis, in degrees)
reported by the sensor.
|
float |
getBarometricPressure()
Returns the current barometric pressure, based upon calibrated readings
from the onboard pressure sensor.
|
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.
|
double |
getByteCount()
Returns the count in bytes of data received from the
sensor.
|
float |
getCompassHeading()
Returns the current tilt-compensated compass heading
value (in degrees, from 0 to 360) reported by the sensor.
|
float |
getDisplacementX()
Returns the displacement (in meters) of the X axis since resetDisplacement()
was last invoked [Experimental].
|
float |
getDisplacementY()
Returns the displacement (in meters) of the Y axis since resetDisplacement()
was last invoked [Experimental].
|
float |
getDisplacementZ()
Returns the displacement (in meters) of the Z axis since resetDisplacement()
was last invoked [Experimental].
|
java.lang.String |
getFirmwareVersion()
Returns the version number of the firmware currently executing
on the sensor.
|
float |
getFusedHeading()
Returns the "fused" (9-axis) heading.
|
PIDSourceType |
getPIDSourceType() |
float |
getPitch()
Returns the current pitch value (in degrees, from -180 to 180)
reported by the sensor.
|
float |
getPressure()
Returns the current barometric pressure (in millibar) [navX Aero only].
|
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".
|
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".
|
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".
|
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".
|
double |
getRate()
Return the rate of rotation of the yaw (Z-axis) gyro, in degrees per second.
|
float |
getRawAccelX()
Returns the current raw (unprocessed) X-axis acceleration rate (in G).
|
float |
getRawAccelY()
Returns the current raw (unprocessed) Y-axis acceleration rate (in G).
|
float |
getRawAccelZ()
Returns the current raw (unprocessed) Z-axis acceleration rate (in G).
|
float |
getRawGyroX()
Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec).
|
float |
getRawGyroY()
Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec).
|
float |
getRawGyroZ()
Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec).
|
float |
getRawMagX()
Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla).
|
float |
getRawMagY()
Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla).
|
float |
getRawMagZ()
Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla).
|
float |
getRoll()
Returns the current roll value (in degrees, from -180 to 180)
reported by the sensor.
|
java.lang.String |
getSmartDashboardType() |
ITable |
getTable() |
float |
getTempC()
Returns the current temperature (in degrees centigrade) reported by
the sensor's gyro/accelerometer circuit.
|
double |
getUpdateCount()
Returns the count of valid updates which have
been received from the sensor.
|
float |
getVelocityX()
Returns the velocity (in meters/sec) of the X axis [Experimental].
|
float |
getVelocityY()
Returns the velocity (in meters/sec) of the Y axis [Experimental].
|
float |
getVelocityZ()
Returns the velocity (in meters/sec) of the Z axis [Experimental].
|
float |
getWorldLinearAccelX()
Returns the current linear acceleration in the X-axis (in G).
|
float |
getWorldLinearAccelY()
Returns the current linear acceleration in the Y-axis (in G).
|
float |
getWorldLinearAccelZ()
Returns the current linear acceleration in the Z-axis (in G).
|
float |
getYaw()
Returns the current yaw value (in degrees, from -180 to 180)
reported by the sensor.
|
void |
initTable(ITable itable) |
boolean |
isAltitudeValid()
Indicates whether the current altitude (and barometric pressure) data is
valid.
|
boolean |
isCalibrating()
Returns true if the sensor is currently performing automatic
gyro/accelerometer calibration.
|
boolean |
isConnected()
Indicates whether the sensor is currently connected
to the host computer.
|
boolean |
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.
|
boolean |
isMagnetometerCalibrated()
Indicates whether the magnetometer has been calibrated.
|
boolean |
isMoving()
Indicates if the sensor is currently detecting motion,
based upon the X and Y-axis world linear acceleration values.
|
boolean |
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."
|
double |
pidGet()
Returns the current yaw value reported by the sensor.
|
void |
reset()
Reset the Yaw gyro.
|
void |
resetDisplacement()
Zeros the displacement integration variables.
|
void |
setPIDSourceType(PIDSourceType type) |
void |
startLiveWindowMode() |
void |
stopLiveWindowMode() |
void |
updateTable() |
void |
zeroYaw()
Sets the user-specified yaw offset to the current
yaw value reported by the sensor.
|
public AHRS(SPI.Port spi_port_id, byte update_rate_hz)
This constructor should be used if communicating via SPI.
Note that increasing the update rate may increase the CPU utilization.
spi_port_id
- SPI Port to useupdate_rate_hz
- Custom Update Rate (Hz)public AHRS(SPI.Port spi_port_id, int spi_bitrate, byte update_rate_hz)
spi_port_id
- SPI Port to usespi_bitrate
- SPI bitrate (Maximum: 2,000,000)update_rate_hz
- Custom Update Rate (Hz)public AHRS(I2C.Port i2c_port_id, byte update_rate_hz)
This constructor should be used if communicating via I2C.
Note that increasing the update rate may increase the CPU utilization.
i2c_port_id
- I2C Port to useupdate_rate_hz
- Custom Update Rate (Hz)public AHRS(SerialPort.Port serial_port_id, AHRS.SerialDataType data_type, byte update_rate_hz)
This constructor should be used if communicating via either TTL UART or USB Serial interface.
Note that the serial interfaces can communicate either processed data, or raw data, but not both simultaneously. If simultaneous processed and raw data are needed, use one of the register-based interfaces (SPI or I2C).
Note that increasing the update rate may increase the CPU utilization.
serial_port_id
- SerialPort to usedata_type
- either kProcessedData or kRawDataupdate_rate_hz
- Custom Update Rate (Hz)public AHRS(SPI.Port spi_port_id)
This constructor should be used if communicating via SPI.
spi_port_id
- SPI port to use.public AHRS(I2C.Port i2c_port_id)
This constructor should be used if communicating via I2C.
i2c_port_id
- I2C port to usepublic AHRS(SerialPort.Port serial_port_id)
This constructor should be used if communicating via either TTL UART or USB Serial interface.
serial_port_id
- SerialPort to usepublic float getPitch()
public float getRoll()
public float getYaw()
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.
public float getCompassHeading()
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.
public void zeroYaw()
This user-specified yaw offset is automatically subtracted from subsequent yaw values reported by the getYaw() method.
public boolean isCalibrating()
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.
public boolean isConnected()
public double getByteCount()
If the byte count is increasing, but the update count (see getUpdateCount()) is not, this indicates a software misconfiguration.
public double getUpdateCount()
public float getWorldLinearAccelX()
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.
public float getWorldLinearAccelY()
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.
public float getWorldLinearAccelZ()
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.
public boolean isMoving()
public boolean isRotating()
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.
public float getBarometricPressure()
NOTE: This value is only valid for a navX Aero. To determine whether this value is valid, see isAltitudeValid().
public float getAltitude()
NOTE: This value is only valid sensors including a pressure sensor. To determine whether this value is valid, see isAltitudeValid().
public boolean isAltitudeValid()
If this value is false for a board with an installed pressure sensor, this indicates a malfunction of the onboard pressure sensor.
public float getFusedHeading()
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).
public boolean isMagneticDisturbance()
This function will always return false if the sensor's magnetometer has not yet been calibrated; see isMagnetometerCalibrated().
public boolean isMagnetometerCalibrated()
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.
public float getQuaternionW()
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.
public float getQuaternionX()
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.
public float getQuaternionY()
public float getQuaternionZ()
public void resetDisplacement()
public float getVelocityX()
public float getVelocityY()
public float getVelocityZ()
public float getDisplacementX()
public float getDisplacementY()
public float getDisplacementZ()
public PIDSourceType getPIDSourceType()
public void setPIDSourceType(PIDSourceType type)
public double pidGet()
public double getAngle()
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.
public double getRate()
The rate is based on the most recent reading of the yaw gyro angle.
public void reset()
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.
public float getRawGyroX()
getPitch()
method.
public float getRawGyroY()
getRoll()
method.
public float getRawGyroZ()
getYaw()
method.
public float getRawAccelX()
getWorldLinearAccelX()
method.
public float getRawAccelY()
getWorldLinearAccelY()
method.
public float getRawAccelZ()
getWorldLinearAccelZ()
method.
public float getRawMagX()
getCompassHeading()
method.
public float getRawMagY()
getCompassHeading()
method.
public float getRawMagZ()
getCompassHeading()
method.
public float getPressure()
This value is valid only if a barometric pressure sensor is onboard.
public float getTempC()
This value may be useful in order to perform advanced temperature- correction of raw gyroscope and accelerometer values.
public AHRS.BoardYawAxis getBoardYawAxis()
For more information on Omnimount, please see:
http://navx-mxp.kauailabs.com/navx-mxp/installation/omnimount/
public java.lang.String getFirmwareVersion()
To update the firmware to the latest version, please see:
http://navx-mxp.kauailabs.com/navx-mxp/support/updating-firmware/
public void updateTable()
public void startLiveWindowMode()
public void stopLiveWindowMode()
public void initTable(ITable itable)
public ITable getTable()
public java.lang.String getSmartDashboardType()