protected override void GenerateMessage()
        {
            DeviceHelper.SetCurrentTime(gps.Time);

            // Get postion in Cartesian gazebo frame
            var worldPosition = gpsLink.position;

            // Apply position noise before converting to global frame
            // TODO: Applying noise

            // Convert to global frames
            spherical = sphericalCoordinates.SphericalFromLocal(worldPosition);

            gps.LatitudeDeg  = spherical.x;
            gps.LongitudeDeg = spherical.y;
            gps.Altitude     = spherical.z;

            // Convert to global frame
            gpsVelocity = sphericalCoordinates.GlobalFromLocal(sensorVelocity);

            // Apply noise after converting to global frame
            // TODO: Applying noise

            gps.VelocityEast  = gpsVelocity.x;
            gps.VelocityNorth = gpsVelocity.y;
            gps.VelocityUp    = gpsVelocity.z;

            PushData <messages.Gps>(gps);
        }
Exemple #2
0
        protected override void GenerateMessage()
        {
            DeviceHelper.SetCurrentTime(gps.Time);

            // Apply position noise before converting to global frame
            // TODO: Applying noise

            // Convert to global frames
            var convertedPosition = DeviceHelper.Convert.Position(worldPosition);

            gpsCoordinates = sphericalCoordinates.SphericalFromLocal(convertedPosition);

            gps.LatitudeDeg  = gpsCoordinates.x;
            gps.LongitudeDeg = gpsCoordinates.y;
            gps.Altitude     = gpsCoordinates.z;

            // Convert to global frame
            var convertedVelocity = DeviceHelper.Convert.Position(sensorVelocity);

            gpsVelocity = sphericalCoordinates.GlobalFromLocal(convertedVelocity);

            // Apply noise after converting to global frame
            // TODO: Applying noise

            gps.VelocityEast  = gpsVelocity.x;
            gps.VelocityNorth = gpsVelocity.y;
            gps.VelocityUp    = gpsVelocity.z;

            PushData <messages.Gps>(gps);
        }
Exemple #3
0
        protected override void GenerateMessage()
        {
            DeviceHelper.SetCurrentTime(gps.Time);

            // Convert to global frames
            var convertedPosition = DeviceHelper.Convert.Position(worldPosition);

            gpsCoordinates = sphericalCoordinates.SphericalFromLocal(convertedPosition);

            // Apply noise after converting to global frame
            if (position_sensing_noises["horizontal"] != null)
            {
                position_sensing_noises["horizontal"].Apply <double>(ref gpsCoordinates.x);
            }

            if (position_sensing_noises["vertical"] != null)
            {
                position_sensing_noises["vertical"].Apply <double>(ref gpsCoordinates.y);
            }

            gps.LatitudeDeg  = gpsCoordinates.x;
            gps.LongitudeDeg = gpsCoordinates.y;
            gps.Altitude     = gpsCoordinates.z;

            // Convert to global frame
            var convertedVelocity = DeviceHelper.Convert.Position(sensorVelocity);

            gpsVelocity = sphericalCoordinates.GlobalFromLocal(convertedVelocity);

            // Apply noise after converting to global frame
            if (velocity_sensing_noises["horizontal"] != null)
            {
                velocity_sensing_noises["horizontal"].Apply <double>(ref gpsVelocity.x);
            }

            if (velocity_sensing_noises["vertical"] != null)
            {
                velocity_sensing_noises["vertical"].Apply <double>(ref gpsVelocity.y);
            }

            gps.VelocityNorth = gpsVelocity.x;
            gps.VelocityEast  = gpsVelocity.y;
            gps.VelocityUp    = gpsVelocity.z;

            PushDeviceMessage <messages.Gps>(gps);
        }