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 -float last_angle; -double last_rate; -int zero_crossing_count; ContinuousAngleTracker::ContinuousAngleTracker() { - last_angle = 0.0f; - zero_crossing_count = 0; - last_rate = 0; + this->last_angle = 0.0f; + this->zero_crossing_count = 0; + this->last_rate = 0; + this->first_sample = false; } void ContinuousAngleTracker::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 ( this->first_sample ) { + this->first_sample = false; + if ( newAngle < 0.0f ) { + this->zero_crossing_count--; + } + } + + /* Calculate delta angle, adjusting appropriately + * if the current sample crossed the -180/180 + * point. + */ + + bool bottom_crossing = false; + float delta_angle = newAngle - this->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) && (this->last_angle >= 0.0f) ) { + this->zero_crossing_count--; + } + } else if ( delta_angle >= 0.0f ) { + if ( (newAngle >= 0.0f) && (last_angle < 0.0f) ) { + this->zero_crossing_count++; + } } } - last_angle = newAngle; - + this->last_angle = newAngle; } double ContinuousAngleTracker::GetAngle() { - double accumulated_angle = (double)zero_crossing_count * 360.0f; - double curr_angle = (double)last_angle; + double accumulated_angle = (double)this->zero_crossing_count * 360.0f; + double curr_angle = (double)this->last_angle; if ( curr_angle < 0.0f ) { curr_angle += 360.0f; } @@ -63,5 +74,5 @@ double ContinuousAngleTracker::GetAngle() { } double ContinuousAngleTracker::GetRate() { - return last_rate; + return this->last_rate; } diff --git a/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.h b/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.h index 830f5c08..70935f07 100644 --- a/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.h +++ b/roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.h @@ -12,6 +12,7 @@ class ContinuousAngleTracker { float last_angle; double last_rate; int zero_crossing_count; + bool first_sample; public: ContinuousAngleTracker(); void NextAngle( float newAngle ); diff --git a/roborio/c++/navx_frc_cpp/src/IIOCompleteNotification.h b/roborio/c++/navx_frc_cpp/src/IIOCompleteNotification.h index 8c7611d4..0c63db90 100644 --- a/roborio/c++/navx_frc_cpp/src/IIOCompleteNotification.h +++ b/roborio/c++/navx_frc_cpp/src/IIOCompleteNotification.h @@ -26,10 +26,10 @@ class IIOCompleteNotification { int16_t accel_fsr_g; int16_t gyro_fsr_dps; }; - virtual void SetYawPitchRoll(IMUProtocol::YPRUpdate& ypr_update) = 0; - virtual void SetAHRSData(AHRSProtocol::AHRSUpdate& ahrs_update) = 0; - virtual void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update) = 0; - virtual void SetRawData(IMUProtocol::GyroUpdate& raw_data_update) = 0; + virtual void SetYawPitchRoll(IMUProtocol::YPRUpdate& ypr_update, long sensor_timestamp) = 0; + virtual void SetAHRSData(AHRSProtocol::AHRSUpdate& ahrs_update, long sensor_timestamp) = 0; + virtual void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update, long sensor_timestamp) = 0; + virtual void SetRawData(IMUProtocol::GyroUpdate& raw_data_update, long sensor_timestamp) = 0; virtual void SetBoardID(AHRSProtocol::BoardID& board_id) = 0; virtual void SetBoardState( BoardState& board_state) = 0; }; diff --git a/roborio/c++/navx_frc_cpp/src/RegisterIO.cpp b/roborio/c++/navx_frc_cpp/src/RegisterIO.cpp index 8a2be253..ad6936b2 100644 --- a/roborio/c++/navx_frc_cpp/src/RegisterIO.cpp +++ b/roborio/c++/navx_frc_cpp/src/RegisterIO.cpp @@ -154,7 +154,7 @@ void RegisterIO::GetCurrentData() { ahrspos_update.disp_x = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_DISP_X_I_L-first_address); ahrspos_update.disp_y = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_DISP_Y_I_L-first_address); ahrspos_update.disp_z = IMURegisters::decodeProtocol1616Float(curr_data + 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; @@ -171,7 +171,7 @@ void RegisterIO::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[NAVX_REG_CAL_STATUS-first_address]; @@ -192,7 +192,7 @@ void RegisterIO::GetCurrentData() { raw_data_update.accel_z = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_ACC_Z_L-first_address); raw_data_update.mag_x = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_MAG_X_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 += buffer_len; diff --git a/roborio/c++/navx_frc_cpp/src/SerialIO.cpp b/roborio/c++/navx_frc_cpp/src/SerialIO.cpp index 1fcbb209..8c7a10d0 100644 --- a/roborio/c++/navx_frc_cpp/src/SerialIO.cpp +++ b/roborio/c++/navx_frc_cpp/src/SerialIO.cpp @@ -92,15 +92,16 @@ void SerialIO::DispatchStreamResponse(IMUProtocol::StreamResponse& response) { int SerialIO::DecodePacketHandler(char * received_data, int bytes_remaining) { int packet_length; + long sensor_timestamp = 0; /* Serial protocols do not provide sensor timestamps. */ if ( (packet_length = IMUProtocol::decodeYPRUpdate(received_data, 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, 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, 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, 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::decodeBoardIdentityResponse(received_data, bytes_remaining, board_id)) > 0) { notify_sink->SetBoardID(board_id); } else { diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java index ba62e4f6..ae458c9d 100644 --- a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java +++ b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java @@ -450,6 +450,19 @@ public double 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. + */ + public long getLastSensorTimestamp() { + return this.last_sensor_timestamp; + } + /** * Returns the current linear acceleration in the X-axis (in G). *

@@ -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 {