/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - Quaternion
/// </summary>
/// <param name="notification">Quaternion notification</param>
private void ChrQuaternionHandler(chrum6orientationsensor.QuaternionNotification notification)
{
Tracer.Trace(string.Format("the UM6 Sensor reported Quaternion: {0} {1} {2} {3} {4}", notification.Body.LastUpdate, notification.Body.a, notification.Body.b, notification.Body.c, notification.Body.d));
try
{
// we switch a and b here to match WPF quaternion's orientation. Maybe there is a proper transformation to do it, but this seems to work as well.
Quaternion aq = new Quaternion(notification.Body.b, notification.Body.a, notification.Body.c, notification.Body.d); // X, Y, Z, W components in WPF correspond to a, b, c, d in CH UM6 and Wikipedia
// have to turn it still around the Y axis (facing East):
Vector3D axis = new Vector3D(0, 1, 0);
aq = aq * new Quaternion(axis, 180);
libguiwpf.OrientationData attitudeData = new libguiwpf.OrientationData()
{
timestamp = notification.Body.LastUpdate, attitudeQuaternion = aq
};
setGuiCurrentAttitude(attitudeData);
}
catch (Exception exc)
{
Tracer.Trace("ChrQuaternionHandler() - " + exc);
}
}
/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - Quaternion
/// </summary>
/// <param name="notification">Quaternion notification</param>
private void ChrQuaternionHandler(chrum6orientationsensor.QuaternionNotification notification)
{
//Tracer.Trace(string.Format("the UM6 Sensor reported Quaternion: {0} {1} {2} {3} {4}", notification.Body.LastUpdate, notification.Body.a, notification.Body.b, notification.Body.c, notification.Body.d));
if (!_mapperVicinity.robotState.ignoreAhrs && (USE_ORIENTATION_UM6 || USE_DIRECTION_UM6_QUATERNION))
{
try
{
// we switch a and b here to match WPF quaternion's orientation. Maybe there is a proper transformation to do it, but this seems to work as well.
Quaternion aq = new Quaternion(notification.Body.b, notification.Body.a, notification.Body.c, notification.Body.d); // X, Y, Z, W components in WPF correspond to a, b, c, d in CH UM6 and Wikipedia
// have to turn it still around the Y axis (facing East):
Vector3D axis = new Vector3D(0, 1, 0);
aq = aq * new Quaternion(axis, 180);
libguiwpf.OrientationData attitudeData = new libguiwpf.OrientationData()
{
timestamp = notification.Body.LastUpdate, attitudeQuaternion = aq
};
if (USE_ORIENTATION_UM6)
{
setGuiCurrentAttitude(attitudeData);
if (!_doUnitTest && !USE_DIRECTION_UM6_QUATERNION)
{
// do it now if direction is not taken from UM6 quaternion, otherwise let setCurrentDirection() call the Decide():
Decide(SensorEventSource.Orientation);
}
}
if (USE_DIRECTION_UM6_QUATERNION)
{
// do what compass data handlers do:
proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable()
{
TimeStamp = notification.Body.LastUpdate, heading = toDegrees(attitudeData.heading) + CHR_QUATERNION_YAW_TRUE_NORTH_OFFSET
};
setCurrentDirection(newDir);
}
}
catch (Exception exc)
{
Tracer.Trace("ChrQuaternionHandler() - " + exc);
}
}
}