diff --git a/roborio/c++/navx_frc_cpp/include/AHRS.h b/roborio/c++/navx_frc_cpp/include/AHRS.h index 8cc36940..2a459ceb 100644 --- a/roborio/c++/navx_frc_cpp/include/AHRS.h +++ b/roborio/c++/navx_frc_cpp/include/AHRS.h @@ -137,6 +137,7 @@ class AHRS : public SensorBase, bool IsConnected(); double GetByteCount(); double GetUpdateCount(); + long GetLastSensorTimestamp(); float GetWorldLinearAccelX(); float GetWorldLinearAccelY(); float GetWorldLinearAccelZ(); diff --git a/roborio/c++/navx_frc_cpp/src/AHRS.cpp b/roborio/c++/navx_frc_cpp/src/AHRS.cpp index dda46513..800a9dc2 100644 --- a/roborio/c++/navx_frc_cpp/src/AHRS.cpp +++ b/roborio/c++/navx_frc_cpp/src/AHRS.cpp @@ -39,14 +39,15 @@ class AHRSInternal : public IIOCompleteNotification, public IBoardCapabilities { /* IIOCompleteNotification Interface Implementation */ /***********************************************************/ - void SetYawPitchRoll(IMUProtocol::YPRUpdate& ypr_update) { - ahrs->yaw = ypr_update.yaw; - ahrs->pitch = ypr_update.pitch; - ahrs->roll = ypr_update.roll; - ahrs->compass_heading = ypr_update.compass_heading; + void SetYawPitchRoll(IMUProtocol::YPRUpdate& ypr_update, long sensor_timestamp) { + ahrs->yaw = ypr_update.yaw; + ahrs->pitch = ypr_update.pitch; + ahrs->roll = ypr_update.roll; + ahrs->compass_heading = ypr_update.compass_heading; + ahrs->last_sensor_timestamp = sensor_timestamp; } - void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update) { + void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update, long sensor_timestamp) { /* Update base IMU class variables */ ahrs->yaw = ahrs_update.yaw; @@ -107,9 +108,10 @@ class AHRSInternal : public IIOCompleteNotification, public IBoardCapabilities { ahrs->displacement[2] = ahrs_update.disp_z; ahrs->yaw_angle_tracker->NextAngle(ahrs->GetYaw()); + ahrs->last_sensor_timestamp = sensor_timestamp; } - void SetRawData(AHRSProtocol::GyroUpdate& raw_data_update) { + void SetRawData(AHRSProtocol::GyroUpdate& raw_data_update, long sensor_timestamp) { ahrs->raw_gyro_x = raw_data_update.gyro_x; ahrs->raw_gyro_y = raw_data_update.gyro_y; ahrs->raw_gyro_z = raw_data_update.gyro_z; @@ -120,9 +122,10 @@ class AHRSInternal : public IIOCompleteNotification, public IBoardCapabilities { ahrs->cal_mag_y = raw_data_update.mag_y; ahrs->cal_mag_z = raw_data_update.mag_z; ahrs->mpu_temp_c = raw_data_update.temp_c; + ahrs->last_sensor_timestamp = sensor_timestamp; } - void SetAHRSData(AHRSProtocol::AHRSUpdate& ahrs_update) { + void SetAHRSData(AHRSProtocol::AHRSUpdate& ahrs_update, long sensor_timestamp) { /* Update base IMU class variables */ ahrs->yaw = ahrs_update.yaw; @@ -180,6 +183,8 @@ class AHRSInternal : public IIOCompleteNotification, public IBoardCapabilities { ahrs->quaternionY = ahrs_update.quat_y; ahrs->quaternionZ = ahrs_update.quat_z; + ahrs->last_sensor_timestamp = sensor_timestamp; + ahrs->UpdateDisplacement( ahrs->world_linear_accel_x, ahrs->world_linear_accel_y, ahrs->update_rate_hz, @@ -480,6 +485,19 @@ double AHRS::GetUpdateCount() { return io->GetUpdateCount(); } +/** + * 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. + * @return The sensor timestamp corresponding to the current AHRS sensor data. + */ +long AHRS::GetLastSensorTimestamp() { + return this->last_sensor_timestamp; +} + /** * Returns the current linear acceleration in the X-axis (in G). *
diff --git a/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.cpp b/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.cpp
index 7fdd6c33..3d98b53f 100644
--- a/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.cpp
+++ b/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.cpp
@@ -7,54 +7,65 @@
#include
@@ -1132,15 +1145,16 @@ public boolean isDisplacementSupported()
class IOCompleteNotification implements IIOCompleteNotification {
@Override
- public void setYawPitchRoll(YPRUpdate ypr_update) {
+ public void setYawPitchRoll(YPRUpdate ypr_update, long sensor_timestamp) {
AHRS.this.yaw = ypr_update.yaw;
AHRS.this.pitch = ypr_update.pitch;
AHRS.this.roll = ypr_update.roll;
AHRS.this.compass_heading = ypr_update.compass_heading;
+ AHRS.this.last_sensor_timestamp = sensor_timestamp;
}
@Override
- public void setAHRSPosData(AHRSPosUpdate ahrs_update) {
+ public void setAHRSPosData(AHRSPosUpdate ahrs_update, long sensor_timestamp) {
/* Update base IMU class variables */
@@ -1194,6 +1208,8 @@ public void setAHRSPosData(AHRSPosUpdate ahrs_update) {
AHRS.this.quaternionY = ahrs_update.quat_y;
AHRS.this.quaternionZ = ahrs_update.quat_z;
+ AHRS.this.last_sensor_timestamp = sensor_timestamp;
+
velocity[0] = ahrs_update.vel_x;
velocity[1] = ahrs_update.vel_y;
velocity[2] = ahrs_update.vel_z;
@@ -1205,7 +1221,7 @@ public void setAHRSPosData(AHRSPosUpdate ahrs_update) {
}
@Override
- public void setRawData(AHRSProtocol.GyroUpdate raw_data_update) {
+ public void setRawData(AHRSProtocol.GyroUpdate raw_data_update, long sensor_timestamp) {
AHRS.this.raw_gyro_x = raw_data_update.gyro_x;
AHRS.this.raw_gyro_y = raw_data_update.gyro_y;
AHRS.this.raw_gyro_z = raw_data_update.gyro_z;
@@ -1216,10 +1232,12 @@ public void setRawData(AHRSProtocol.GyroUpdate raw_data_update) {
AHRS.this.cal_mag_y = raw_data_update.mag_y;
AHRS.this.cal_mag_z = raw_data_update.mag_z;
AHRS.this.mpu_temp_c = raw_data_update.temp_c;
+
+ AHRS.this.last_sensor_timestamp = sensor_timestamp;
}
@Override
- public void setAHRSData(AHRSProtocol.AHRSUpdate ahrs_update) {
+ public void setAHRSData(AHRSProtocol.AHRSUpdate ahrs_update, long sensor_timestamp) {
/* Update base IMU class variables */
@@ -1278,6 +1296,8 @@ public void setAHRSData(AHRSProtocol.AHRSUpdate ahrs_update) {
AHRS.this.quaternionY = ahrs_update.quat_y;
AHRS.this.quaternionZ = ahrs_update.quat_z;
+ AHRS.this.last_sensor_timestamp = sensor_timestamp;
+
updateDisplacement( AHRS.this.world_linear_accel_x,
AHRS.this.world_linear_accel_y,
update_rate_hz,
diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/ContinuousAngleTracker.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/ContinuousAngleTracker.java
index c80d3e9b..5f659f40 100644
--- a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/ContinuousAngleTracker.java
+++ b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/ContinuousAngleTracker.java
@@ -9,52 +9,68 @@
/*----------------------------------------------------------------------------*/
package com.kauailabs.navx.frc;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
class ContinuousAngleTracker {
private float last_angle;
private double last_rate;
private int zero_crossing_count;
+ private boolean first_sample;
public ContinuousAngleTracker() {
last_angle = 0.0f;
zero_crossing_count = 0;
last_rate = 0;
+ first_sample = true;
}
public void nextAngle( float newAngle ) {
- int angle_last_direction;
- float adjusted_last_angle = ( last_angle < 0.0f ) ? last_angle + 360.0f : last_angle;
- float adjusted_curr_angle = ( newAngle < 0.0f ) ? newAngle + 360.0f : newAngle;
- float delta_angle = adjusted_curr_angle - adjusted_last_angle;
- this.last_rate = delta_angle;
-
- angle_last_direction = 0;
- if ( adjusted_curr_angle < adjusted_last_angle ) {
- if ( delta_angle < -180.0f ) {
- angle_last_direction = -1;
- } else {
- angle_last_direction = 1;
- }
- } else if ( adjusted_curr_angle > adjusted_last_angle ) {
- if ( delta_angle > 180.0f ) {
- angle_last_direction = -1;
- } else {
- angle_last_direction = 1;
- }
+ /* If the first received sample is negative,
+ * ensure that the zero crossing count is
+ * decremented.
+ */
+
+ if ( first_sample ) {
+ first_sample = false;
+ if ( newAngle < 0.0f ) {
+ zero_crossing_count--;
+ }
+ }
+
+ /* Calculate delta angle, adjusting appropriately
+ * if the current sample crossed the -180/180
+ * point.
+ */
+
+ boolean bottom_crossing = false;
+ float delta_angle = newAngle - last_angle;
+ /* Adjust for wraparound at -180/+180 point */
+ if ( delta_angle >= 180.0f ){
+ delta_angle = 360.0f - delta_angle;
+ bottom_crossing = true;
+ } else if ( delta_angle <= -180.0f ){
+ delta_angle = 360.0f + delta_angle;
+ bottom_crossing = true;
}
+ this.last_rate = delta_angle;
- if ( angle_last_direction < 0 ) {
- if ( ( adjusted_curr_angle < 0.0f ) && ( adjusted_last_angle >= 0.0f ) ) {
- zero_crossing_count--;
- }
- } else if ( angle_last_direction > 0 ) {
- if ( ( adjusted_curr_angle >= 0.0f ) && ( adjusted_last_angle < 0.0f ) ) {
- zero_crossing_count++;
- }
+ /* If a zero crossing occurred, increment/decrement
+ * the zero crossing count appropriately.
+ */
+ if ( !bottom_crossing ) {
+ if ( delta_angle < 0.0f ) {
+ if ( (newAngle < 0.0f) && (last_angle >= 0.0f) ) {
+ zero_crossing_count--;
+ }
+ } else if ( delta_angle >= 0.0f ) {
+ if ( (newAngle >= 0.0f) && (last_angle < 0.0f) ) {
+ zero_crossing_count++;
+ }
+ }
}
- last_angle = newAngle;
-
+ this.last_angle = newAngle;
}
public double getAngle() {
diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/IIOCompleteNotification.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/IIOCompleteNotification.java
index 3bbff44b..55c1cf73 100644
--- a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/IIOCompleteNotification.java
+++ b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/IIOCompleteNotification.java
@@ -23,10 +23,10 @@ class BoardState {
public short accel_fsr_g;
public short gyro_fsr_dps;
}
- void setYawPitchRoll(IMUProtocol.YPRUpdate yprupdate);
- void setAHRSData(AHRSProtocol.AHRSUpdate ahrs_update);
- void setAHRSPosData(AHRSProtocol.AHRSPosUpdate ahrs_update);
- void setRawData(IMUProtocol.GyroUpdate raw_data_update);
+ void setYawPitchRoll(IMUProtocol.YPRUpdate yprupdate, long sensor_timestamp);
+ void setAHRSData(AHRSProtocol.AHRSUpdate ahrs_update, long sensor_timestamp);
+ void setAHRSPosData(AHRSProtocol.AHRSPosUpdate ahrs_update, long sensor_timestamp);
+ void setRawData(IMUProtocol.GyroUpdate raw_data_update, long sensor_timestamp);
void setBoardID(AHRSProtocol.BoardID board_id);
void setBoardState( BoardState board_state);
}
diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO.java
index 385c3be8..6f60ff7b 100644
--- a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO.java
+++ b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO.java
@@ -141,7 +141,7 @@ private void getCurrentData() {
ahrspos_update.disp_x = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_X_I_L-first_address);
ahrspos_update.disp_y = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Y_I_L-first_address);
ahrspos_update.disp_z = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Z_I_L-first_address);
- notify_sink.setAHRSPosData(ahrspos_update);
+ notify_sink.setAHRSPosData(ahrspos_update, sensor_timestamp);
} else {
ahrs_update.op_status = ahrspos_update.op_status;
ahrs_update.selftest_status = ahrspos_update.selftest_status;
@@ -158,7 +158,7 @@ private void getCurrentData() {
ahrs_update.altitude = ahrspos_update.altitude;
ahrs_update.barometric_pressure = ahrspos_update.barometric_pressure;
ahrs_update.fused_heading = ahrspos_update.fused_heading;
- notify_sink.setAHRSData( ahrs_update );
+ notify_sink.setAHRSData( ahrs_update, sensor_timestamp );
}
board_state.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS-first_address];
@@ -181,7 +181,7 @@ private void getCurrentData() {
raw_data_update.mag_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Y_L-first_address);
raw_data_update.mag_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Z_L-first_address);
raw_data_update.temp_c = ahrspos_update.mpu_temp;
- notify_sink.setRawData(raw_data_update);
+ notify_sink.setRawData(raw_data_update, sensor_timestamp);
this.last_update_time = Timer.getFPGATimestamp();
byte_count += curr_data.length;
diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/SerialIO.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/SerialIO.java
index 73d0ce32..8928c9fe 100644
--- a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/SerialIO.java
+++ b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/SerialIO.java
@@ -118,15 +118,16 @@ protected void dispatchStreamResponse(IMUProtocol.StreamResponse response) {
protected int decodePacketHandler(byte[] received_data, int offset, int bytes_remaining) {
int packet_length;
+ int sensor_timestamp = 0; /* Note: Serial Protocols don't provide sensor timestamps */
if ( (packet_length = IMUProtocol.decodeYPRUpdate(received_data, offset, bytes_remaining, ypr_update_data)) > 0) {
- notify_sink.setYawPitchRoll(ypr_update_data);
+ notify_sink.setYawPitchRoll(ypr_update_data, sensor_timestamp);
} else if ( ( packet_length = AHRSProtocol.decodeAHRSPosUpdate(received_data, offset, bytes_remaining, ahrspos_update_data)) > 0) {
- notify_sink.setAHRSPosData(ahrspos_update_data);
+ notify_sink.setAHRSPosData(ahrspos_update_data, sensor_timestamp);
} else if ( ( packet_length = AHRSProtocol.decodeAHRSUpdate(received_data, offset, bytes_remaining, ahrs_update_data)) > 0) {
- notify_sink.setAHRSData(ahrs_update_data);
+ notify_sink.setAHRSData(ahrs_update_data, sensor_timestamp);
} else if ( ( packet_length = IMUProtocol.decodeGyroUpdate(received_data, offset, bytes_remaining, gyro_update_data)) > 0) {
- notify_sink.setRawData(gyro_update_data);
+ notify_sink.setRawData(gyro_update_data, sensor_timestamp);
} else if ( ( packet_length = AHRSProtocol.decodeBoardIDGetResponse(received_data, offset, bytes_remaining, board_id)) > 0) {
notify_sink.setBoardID(board_id);
} else {