public DirectionDataDssSerializable(DirectionData directionData)
        {
            TimeStamp = new DateTime(directionData.TimeStamp);

            heading = directionData.heading;
            bearing = directionData.bearing;
        }
        /// <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);
            }
        }
        protected void setGuiCurrentDirection(DirectionData dir)
        {
            if (_mainWindow != null)
            {
                ccrwpf.Invoke invoke = new ccrwpf.Invoke(delegate()
                {
                    _mainWindow.CurrentDirection = new DirectionData() { TimeStamp = dir.TimeStamp, heading = dir.heading, bearing = _currentGoalBearing };
                }
                );

                _wpfServicePort.Post(invoke);

                Arbiter.Activate(TaskQueue,
                    invoke.ResponsePort.Choice(
                        s => { }, // delegate for success
                        ex => { } //Tracer.Error(ex) // delegate for failure
                ));
            }
        }
Exemplo n.º 4
0
        private void pmFrameCompleteHandler_UI(object sender, AsyncInputFrameArgs aira)
        {
            long timestamp = aira.timestamp;

            // have raw data displayed in a text box:

            StringBuilder frameValue = new StringBuilder();

            frameValue.AppendFormat("{0}   POT: {1}\r\n", aira.dPos1Mks, aira.sensorsState.potValue);
            frameValue.AppendFormat("{0}\r\n", aira.dPos2Mks);
            frameValue.AppendFormat("{0}  ", aira.dPing1DistanceM * 1000.0d);   // print distance in milimeters
            frameValue.AppendFormat("{0}\r\n", aira.fromPingScanStop);
            frameValue.AppendFormat("{0}\r\n", aira.dPing2DistanceM * 1000.0d);

            // expect 7 on "not in sight", 3 on "far", 1 on "medium" and 0 on "close":
            frameValue.AppendFormat("{0} ", aira.sensorsState.irbE1);
            frameValue.AppendFormat("{0} ", aira.sensorsState.irbE2);
            frameValue.AppendFormat("{0} ", aira.sensorsState.irbE3);
            frameValue.AppendFormat("{0} ", aira.sensorsState.irbE4);

            frameValue.AppendFormat("{0} ", aira.sensorsState.irbO1);
            frameValue.AppendFormat("{0} ", aira.sensorsState.irbO2);
            frameValue.AppendFormat("{0} ", aira.sensorsState.irbO3);
            frameValue.AppendFormat("{0}\r\n", aira.sensorsState.irbO4);

            frameValue.AppendFormat("{0} : {1:0.0} * {2:0.0} * {3:0.0} * {4:0.0} \r\n", aira.sensorsState.parkingSensorsCount,
                aira.sensorsState.parkingSensorMetersLB, aira.sensorsState.parkingSensorMetersLF, aira.sensorsState.parkingSensorMetersRF, aira.sensorsState.parkingSensorMetersRB);

            /*
            Tracer.Trace(string.Format("=== xpsi: {0} : {1} {2} {3} {4} : {5} {6} {7} {8}", aira.sensorsState.parkingSensorsCount,
                aira.sensorsState.parkingSensors[0], aira.sensorsState.parkingSensors[1], aira.sensorsState.parkingSensors[2], aira.sensorsState.parkingSensors[3],
                aira.sensorsState.parkingSensors[4], aira.sensorsState.parkingSensors[5], aira.sensorsState.parkingSensors[6], aira.sensorsState.parkingSensors[7]));

            Tracer.Trace(string.Format("=== bpsi: {0} : {1} {2} {3} {4} : {5} {6} {7} {8}", aira.sensorsState.parkingSensorsCount,
                Convert.ToString(aira.sensorsState.parkingSensors[0],2), Convert.ToString(aira.sensorsState.parkingSensors[1],2), Convert.ToString(aira.sensorsState.parkingSensors[2],2), Convert.ToString(aira.sensorsState.parkingSensors[3],2),
                Convert.ToString(aira.sensorsState.parkingSensors[4],2), Convert.ToString(aira.sensorsState.parkingSensors[5],2), Convert.ToString(aira.sensorsState.parkingSensors[6],2), Convert.ToString(aira.sensorsState.parkingSensors[7],2)));
            */

            //frameValue.AppendFormat("{0} {1} {2} {3}\r\n", aira.sensorsState.parkingSensors[4], aira.sensorsState.parkingSensors[5], aira.sensorsState.parkingSensors[6], aira.sensorsState.parkingSensors[7]);

            pmValuesLabel.Content = frameValue.ToString();

            // Proximity sensors display their data in dedicated control:

            lastProxData = new ProximityData() { TimeStamp = timestamp };

            lastProxData.setProximityData(aira.sensorsState.irbE1, aira.sensorsState.irbE2, aira.sensorsState.irbE3, aira.sensorsState.irbE4,
                                        aira.sensorsState.irbO1, aira.sensorsState.irbO2, aira.sensorsState.irbO3, aira.sensorsState.irbO4);

            lastProxDataChanged = true;

            // accelerometer has its own control for displaying data:

            lastAccData = new AccelerometerDataWpf() { TimeStamp = timestamp };

            lastAccData.setAccelerometerData(aira.sensorsState.accelX, aira.sensorsState.accelY, aira.sensorsState.accelZ);

            lastAccDataChanged = true;

            // compass has its own control for displaying data:

            if (aira.sensorsState.compassHeading >= 0.0d && aira.sensorsState.compassHeading <= 359.999999d && (lastDirData == null || Math.Abs(lastDirData.heading - aira.sensorsState.compassHeading) > 2.0d))
            {
                lastDirData = new DirectionData() { heading = aira.sensorsState.compassHeading, TimeStamp = timestamp };

                lastDirDataChanged = true;
            }
            else if ((bool)compassCheckBox.IsChecked && (lastCompassSlider != compassSlider.Value || timestamp > lastCompassSliderTimestamp + 20000000L))
            {
                // simulated direction, controlled by a slider:

                lastCompassSliderTimestamp = timestamp;
                lastCompassSlider = compassSlider.Value;

                double hdg = compassSlider.Value;   // -180 ... 180
                if (hdg < 0)
                {
                    hdg += 360.0d;      // 0...360
                }

                lastDirData = new DirectionData() { heading = hdg, TimeStamp = timestamp };

                lastDirDataChanged = true;
            }

            if (aira.sensorsState.parkingSensorsValid)
            {
                lastPsiData = new ParkingSensorData()
                {
                    parkingSensorMetersLB = aira.sensorsState.parkingSensorMetersLB,
                    parkingSensorMetersLF = aira.sensorsState.parkingSensorMetersLF,
                    parkingSensorMetersRB = aira.sensorsState.parkingSensorMetersRB,
                    parkingSensorMetersRF = aira.sensorsState.parkingSensorMetersRF,
                    TimeStamp = timestamp
                };

                lastPsiDataChanged = true;
            }

            // frames that are marked "" feed the sonar sweep view controls. They are updated in real time:

            if (aira.fromPingScanStop)
            {
                int angleRaw1 = (int)aira.dPos1Mks;
                double distM1 = aira.dPing1DistanceM;

                int angleRaw2 = (int)aira.dPos2Mks;
                double distM2 = aira.dPing2DistanceM;

                if (inTestSamples > 0)
                {
                    // prerun - for a while just try figuring out what comes in - sweep angle ranges for both sides

                    inTestSamples--;

                    if (inTestSamples < TEST_SAMPLES_COUNT - 10)        // first few frames are garbled
                    {
                        board.registerAnglesRaw(angleRaw1, angleRaw2);

                        if (inTestSamples == 0)     // last count
                        {
                            board.finishPrerun();

                            Tracer.Trace(board.ToString());
                        }
                    }
                }
                else
                {
                    // real sonar data is coming, and we finished prerun and know how many beams per sweep we are dealing with

                    lastRangeReadingSet = new RangeReadingSet() { left = board.angleRawToSonarAngle(new RangeReading(angleRaw1, distM1, timestamp)), right = board.angleRawToSonarAngle(new RangeReading(angleRaw2, distM2, timestamp)) };

                    lastRangeReadingSetChanged = true;

                    lastRangeReadingSet.combo1 = board.angleRawToSonarAngleCombo(1, angleRaw1, distM1, timestamp);
                    lastRangeReadingSet.combo2 = board.angleRawToSonarAngleCombo(2, angleRaw2, distM2, timestamp);
                    lastRangeReadingSet.hasCombo = true;

                    if (angleRaw1 == board.rawAngle1_Min || angleRaw1 == board.rawAngle1_Max)
                    {
                        lastRangeReadingSet.sweepFrameReady = true;
                    }
                }
            }

            // sanity check:
            if (lastRangeReadingSetChanged && lastRangeReadingSet.sweepFrameReady && lastDirData == null)
            {
                // we probably don't have compass connected - have a fake dir data for now.
                lastDirData = new DirectionData() { heading = 0.0d, TimeStamp = timestamp };
                lastDirDataChanged = true;
            }

            // update all controls with collected data:

            setSonarViewControls(timestamp);

            updateMapperVicinity(timestamp, sweepViewControlCombo.sonarData);

            lastRangeReadingSetChanged = false;

            setViewControls(timestamp);
        }
        void DirectionMeasurementHandler(DirectionData measurement)
        {
            //Tracer.Trace("TrackRoamerBrickProximityBoardService::DirectionMeasurementHandler()");

            try
            {
                _state.LastSampleTimestamp = new DateTime(measurement.TimeStamp);
                _state.MostRecentDirection = new DirectionDataDssSerializable(measurement);
                _state.LinkState = "receiving Direction Data";

                //
                // Inform subscribed services that the state has changed.
                //
                _submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));

                UpdateDirectionData usd = new UpdateDirectionData();
                usd.Body = _state.MostRecentDirection;

                base.SendNotification<UpdateDirectionData>(_submgrPort, usd);
            }
            catch (Exception e)
            {
                _state.LinkState = "Error while receiving Direction Data";
                LogError(e);
            }
        }