Skip to content

Commit

Permalink
Fixed issue w/ContinuousAngleTracker; added LastSensorTimestamp
Browse files Browse the repository at this point in the history
  • Loading branch information
@kauailabs
kauailabs committed Feb 21, 2016
1 parent 90a0613 commit 7a1a601
Show file tree
Hide file tree
Showing 12 changed files with 168 additions and 99 deletions.
1 change: 1 addition & 0 deletions roborio/c++/navx_frc_cpp/include/AHRS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,7 @@ class AHRS : public SensorBase,
bool IsConnected();
double GetByteCount();
double GetUpdateCount();
long GetLastSensorTimestamp();
float GetWorldLinearAccelX();
float GetWorldLinearAccelY();
float GetWorldLinearAccelZ();
Expand Down
34 changes: 26 additions & 8 deletions roborio/c++/navx_frc_cpp/src/AHRS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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).
*<p>
Expand Down
83 changes: 47 additions & 36 deletions roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,54 +7,65 @@

#include <ContinuousAngleTracker.h>

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;
}
Expand All @@ -63,5 +74,5 @@ double ContinuousAngleTracker::GetAngle() {
}

double ContinuousAngleTracker::GetRate() {
return last_rate;
return this->last_rate;
}
1 change: 1 addition & 0 deletions roborio/c++/navx_frc_cpp/src/ContinuousAngleTracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 );
Expand Down
8 changes: 4 additions & 4 deletions roborio/c++/navx_frc_cpp/src/IIOCompleteNotification.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down
6 changes: 3 additions & 3 deletions roborio/c++/navx_frc_cpp/src/RegisterIO.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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];
Expand All @@ -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;
Expand Down
9 changes: 5 additions & 4 deletions roborio/c++/navx_frc_cpp/src/SerialIO.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
28 changes: 24 additions & 4 deletions roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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).
*<p>
Expand Down Expand Up @@ -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 */

Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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 */

Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit 7a1a601

Please sign in to comment.