コード例 #1
0
        private const double CHR_QUATERNION_YAW_TRUE_NORTH_OFFSET = 14.0;              // manual correction to the Quaternion yaw, to align it with true North reading.

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

            if (USE_DIRECTION_UM6_EULER)
            {
                try
                {
                    // mag heading is reported as a short within +-16200 range
                    double magHeading = Direction.to180(notification.Body.psi * CHR_EULER_YAW_FACTOR + CHR_EULER_YAW_TRUE_NORTHOFFSET);      // convert to degrees and ensure that it is within +- 180 degrees and related to True North.

                    // we still use proxibrick.DirectionDataDssSerializable because it is the direction data in the State:
                    proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable()
                    {
                        TimeStamp = DateTime.Now, heading = magHeading
                    };

                    setCurrentDirection(newDir);
                }
                catch (Exception exc)
                {
                    Tracer.Trace("ChrEulerHandler() - " + exc);
                }
            }
        }
コード例 #2
0
        private const double CHR_EILER_YAW_FACTOR = 180.0d / 16200.0d;      // mag heading is reported as a ahort within +-16200 range. This is to convert it to degrees.

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