protected int DecodePacketHandler(byte[] received_data, int offset, int bytes_remaining)
{
int packet_length;
if ((packet_length = IMUProtocol.decodeYPRUpdate(received_data, offset, bytes_remaining, ypr_update_data)) > 0)
{
notify_sink.SetYawPitchRoll(ypr_update_data);
}
else if ((packet_length = AHRSProtocol.decodeAHRSPosUpdate(received_data, offset, bytes_remaining, ahrspos_update_data)) > 0)
{
notify_sink.SetAHRSPosData(ahrspos_update_data);
}
else if ((packet_length = AHRSProtocol.decodeAHRSUpdate(received_data, offset, bytes_remaining, ahrs_update_data)) > 0)
{
notify_sink.SetAHRSData(ahrs_update_data);
}
else if ((packet_length = IMUProtocol.decodeGyroUpdate(received_data, offset, bytes_remaining, gyro_update_data)) > 0)
{
notify_sink.SetRawData(gyro_update_data);
}
else if ((packet_length = AHRSProtocol.decodeBoardIDGetResponse(received_data, offset, bytes_remaining, board_id)) > 0)
{
notify_sink.SetBoardID(board_id);
}
else
{
packet_length = 0;
}
return(packet_length);
}
private void GetCurrentData()
{
byte first_address = IMURegisters.NAVX_REG_UPDATE_RATE_HZ;
bool displacement_registers = board_capabilities.IsDisplacementSupported();
byte[] curr_data;
/* If firmware supports displacement data, acquire it - otherwise implement */
/* similar (but potentially less accurate) calculations on this processor. */
if (displacement_registers)
{
curr_data = new byte[IMURegisters.NAVX_REG_LAST + 1 - first_address];
}
else
{
curr_data = new byte[IMURegisters.NAVX_REG_QUAT_OFFSET_Z_H + 1 - first_address];
}
if (io_provider.Read(first_address, curr_data))
{
long sensor_timestamp = AHRSProtocol.decodeBinaryUint32(curr_data,
IMURegisters.NAVX_REG_TIMESTAMP_L_L - first_address);
ahrspos_update.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS - first_address];
ahrspos_update.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address];
ahrspos_update.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS];
ahrspos_update.sensor_status = curr_data[IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address];
ahrspos_update.yaw = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_YAW_L - first_address);
ahrspos_update.pitch = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_PITCH_L - first_address);
ahrspos_update.roll = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_ROLL_L - first_address);
ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_HEADING_L - first_address);
ahrspos_update.mpu_temp = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address);
ahrspos_update.linear_accel_x = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L - first_address);
ahrspos_update.linear_accel_y = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L - first_address);
ahrspos_update.linear_accel_z = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L - first_address);
ahrspos_update.altitude = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_ALTITUDE_D_L - first_address);
ahrspos_update.barometric_pressure = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_PRESSURE_DL - first_address);
ahrspos_update.fused_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L - first_address);
ahrspos_update.quat_w = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_W_L - first_address) / 32768.0f;
ahrspos_update.quat_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_X_L - first_address) / 32768.0f;
ahrspos_update.quat_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Y_L - first_address) / 32768.0f;
ahrspos_update.quat_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Z_L - first_address) / 32768.0f;
if (displacement_registers)
{
ahrspos_update.vel_x = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_X_I_L - first_address);
ahrspos_update.vel_y = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Y_I_L - first_address);
ahrspos_update.vel_z = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Z_I_L - first_address);
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);
}
else
{
ahrs_update.op_status = ahrspos_update.op_status;
ahrs_update.selftest_status = ahrspos_update.selftest_status;
ahrs_update.cal_status = ahrspos_update.cal_status;
ahrs_update.sensor_status = ahrspos_update.sensor_status;
ahrs_update.yaw = ahrspos_update.yaw;
ahrs_update.pitch = ahrspos_update.pitch;
ahrs_update.roll = ahrspos_update.roll;
ahrs_update.compass_heading = ahrspos_update.compass_heading;
ahrs_update.mpu_temp = ahrspos_update.mpu_temp;
ahrs_update.linear_accel_x = ahrspos_update.linear_accel_x;
ahrs_update.linear_accel_y = ahrspos_update.linear_accel_y;
ahrs_update.linear_accel_z = ahrspos_update.linear_accel_z;
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);
}
board_state.CalStatus = curr_data[IMURegisters.NAVX_REG_CAL_STATUS - first_address];
board_state.OpStatus = curr_data[IMURegisters.NAVX_REG_OP_STATUS - first_address];
board_state.SelftestStatus = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address];
board_state.SensorStatus = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address);
board_state.UpdateRateHz = curr_data[IMURegisters.NAVX_REG_UPDATE_RATE_HZ - first_address];
board_state.GyroFsrDps = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_GYRO_FSR_DPS_L);
board_state.AccelFsrG = (short)curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G];
board_state.CapabilityFlags = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L - first_address);
notify_sink.SetBoardState(board_state);
raw_data_update.gyro_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_X_L - first_address);
raw_data_update.gyro_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Y_L - first_address);
raw_data_update.gyro_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Z_L - first_address);
raw_data_update.accel_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_X_L - first_address);
raw_data_update.accel_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Y_L - first_address);
raw_data_update.accel_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Z_L - first_address);
raw_data_update.mag_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_X_L - first_address);
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);
this.last_update_time = Timer.GetFPGATimestamp();
byte_count += curr_data.Length;
update_count++;
}
}
