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