/// <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 - ProcAccel
/// </summary>
/// <param name="notification">ProcAccel notification</param>
private void ChrProcAccelHandler(chrum6orientationsensor.ProcAccelNotification notification)
{
Tracer.Trace(string.Format("the UM6 Sensor reported ProcAccel: {0} {1} {2} {3}", notification.Body.LastUpdate, notification.Body.xAccel, notification.Body.yAccel, notification.Body.zAccel));
}
/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - ProcMag
/// </summary>
/// <param name="notification">ProcMag notification</param>
private void ChrProcMagHandler(chrum6orientationsensor.ProcMagNotification notification)
{
Tracer.Trace(string.Format("the UM6 Sensor reported ProcMag: {0} {1} {2} {3}", notification.Body.LastUpdate, notification.Body.x, notification.Body.y, notification.Body.z));
}
/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - Euler
/// </summary>
/// <param name="notification">Euler notification</param>
private void ChrEulerHandler(chrum6orientationsensor.EulerNotification notification)
{
Tracer.Trace(string.Format("the UM6 Sensor reported Euler: {0} PHI={1} THETA={2} PSI={3}", notification.Body.LastUpdate, notification.Body.phi, notification.Body.theta, notification.Body.psi));
try
{
// mag heading is reported as a ahort within +-16200 range
double magHeading = Direction.to180(notification.Body.psi * CHR_EILER_YAW_FACTOR); // convert to degrees and ensure that it is within +- 180 degrees.
DirectionData newDir = new DirectionData() { TimeStamp = DateTime.Now.Ticks, heading = magHeading };
DirectionData curDir = MostRecentDirection;
if (curDir == null || Math.Abs(newDir.heading - curDir.heading) > 0.9d) // only react on significant changes in direction
{
MostRecentDirection = newDir; //.Clone()
if (_mapperVicinity.robotDirection.bearing.HasValue)
{
_currentGoalBearing = (double)_mapperVicinity.robotDirection.bearing;
}
if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished)
{
_mapperVicinity.turnState.directionCurrent = new Direction() { heading = newDir.heading, TimeStamp = newDir.TimeStamp };
}
// update mapper with Direction data:
_mapperVicinity.robotDirection = new Direction() { TimeStamp = newDir.TimeStamp, heading = newDir.heading, bearing = _currentGoalBearing };
// update mapper with Odometry data:
//updateMapperWithOdometryData();
// update GUI (compass control):
setGuiCurrentDirection(newDir);
//if (!_doUnitTest)
//{
// Decide(SensorEventSource.Compass, null);
//}
}
}
catch (Exception exc)
{
Tracer.Trace("ChrEulerHandler() - " + exc);
}
}
