/// <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); } } }