protected void DispatchStreamResponse(IMUProtocol.StreamResponse response)
{
board_state.CalStatus = (byte)(response.flags & IMUProtocol.NAV6_FLAG_MASK_CALIBRATION_STATE);
board_state.CapabilityFlags = (short)(response.flags & ~IMUProtocol.NAV6_FLAG_MASK_CALIBRATION_STATE);
board_state.OpStatus = 0x04; /* TODO: Create a symbol for this */
board_state.SelftestStatus = 0x07; /* TODO: Create a symbol for this */
board_state.AccelFsrG = response.accel_fsr_g;
board_state.GyroFsrDps = response.gyro_fsr_dps;
board_state.UpdateRateHz = (byte)response.update_rate_hz;
notify_sink.SetBoardState(board_state);
/* If AHRSPOS is update type is request, but board doesn't support it, */
/* retransmit the stream config, falling back to AHRS Update mode. */
if (this.update_type == AHRSProtocol.MSGID_AHRSPOS_UPDATE)
{
if (!board_capabilities.IsDisplacementSupported())
{
this.update_type = AHRSProtocol.MSGID_AHRS_UPDATE;
signal_retransmit_stream_config = true;
}
}
}
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++;
}
}
