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;
}
}
}
public void SetYawPitchRoll(IMUProtocol.YPRUpdate yprUpdate)
{
m_parent.m_yaw = yprUpdate.yaw;
m_parent.m_pitch = yprUpdate.pitch;
m_parent.m_roll = yprUpdate.roll;
m_parent.m_compassHeading = yprUpdate.compass_heading;
}