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