private void setYaw(IIMUEvent info)
{
if (info.Yaw >= 0)
{
if (info.Yaw > 180)
{
yaw.setYaw(info.Yaw - 360);
}
else
{
yaw.setYaw(info.Yaw);
}
}
}
private void ImuEventCallback(IIMUEvent eventResponse)
{
// Angles come between 0 and 360.
// Store angles between -180 and 180
// Also, we sometimes see large negative spikes of noise. Ignore these. I believe that this is a robot specific problem.
if (LastImuMessageReceived == DateTime.MinValue)
{
LastImuMessageReceived = DateTime.Now;
LogMessage("First IMU event message received.");
}
LastImuMessageReceived = DateTime.Now;
if (eventResponse.Yaw >= 0)
{
if (eventResponse.Yaw > 180)
{
ImuYaw = eventResponse.Yaw - 360;
}
else
{
ImuYaw = eventResponse.Yaw;
}
}
if (eventResponse.Pitch >= 0)
{
if (eventResponse.Pitch > 180)
{
ImuPitch = eventResponse.Pitch - 360;
}
else
{
ImuPitch = eventResponse.Pitch;
}
}
if (eventResponse.Roll >= 0)
{
if (eventResponse.Roll > 180)
{
ImuRoll = eventResponse.Roll - 360;
}
else
{
ImuRoll = eventResponse.Roll;
}
}
}
private void ImuEventCallback(IIMUEvent eventResponse)
{
// Angles come between 0 and 360.
// Store angles between -180 and 180
// Also, we sometimes see large negative spikes of noise. Ignore these. I believe that this is a robot specific problem.
if (eventResponse.Yaw >= 0)
{
if (eventResponse.Yaw > 180)
{
ImuYaw = eventResponse.Yaw - 360;
}
else
{
ImuYaw = eventResponse.Yaw;
}
}
if (eventResponse.Pitch >= 0)
{
if (eventResponse.Pitch > 180)
{
ImuPitch = eventResponse.Pitch - 360;
}
else
{
ImuPitch = eventResponse.Pitch;
}
}
if (eventResponse.Roll >= 0)
{
if (eventResponse.Roll > 180)
{
ImuRoll = eventResponse.Roll - 360;
}
else
{
ImuRoll = eventResponse.Roll;
}
}
}