public void QuickGetGeoLocation(double inLon, double inLat, double distance, double angle, double expLon, double expLat)
{
GpsLocation a = new GpsLocation(inLon, inLat, 0);
var b = GpsUtils.QuickGetGeoLocation(a, distance, angle);
Assert.AreEqual(expLat, Math.Round(b.Latitude * 100) / 100);
Assert.AreEqual(expLon, Math.Round(b.Longitude * 100) / 100);
}
public double GetVerticalViewAngle(GpsLocation myLoc)
{
if (VerticalViewAngle == null)
{
this.VerticalViewAngle = GpsUtils.VerticalAngle(this.Altitude - myLoc.Altitude, Distance.Value);
}
return(VerticalViewAngle.Value);
}
protected override void UpdateStatusBar()
{
if (GpsUtils.HasLocation(Context.MyLocation))
{
var gpsLocation = $"GPS:{Context.MyLocation.LocationAsString()} Alt:{Context.MyLocation.Altitude:F0}m";
SetStatusLineText($"{gpsLocation} ({Context.MyLocationPlaceInfo.PlaceName})");
}
else
{
SetStatusLineText(Resources.GetText(Resource.String.Main_WaitingForGps), true);
}
}
public static apollo.drivers.gnss.Imu ConvertFrom(ImuData data)
{
return(new apollo.drivers.gnss.Imu()
{
header = new apollo.common.Header()
{
timestamp_sec = data.Time,
sequence_num = data.Sequence,
},
measurement_time = GpsUtils.UtcSecondsToGpsSeconds(data.Time),
measurement_span = (float)data.MeasurementSpan,
linear_acceleration = ConvertToPoint(new Vector3(data.Acceleration.x, data.Acceleration.y, data.Acceleration.z)),
angular_velocity = ConvertToPoint(new Vector3(data.AngularVelocity.x, data.AngularVelocity.y, data.AngularVelocity.z)),
});
}
void PaintProfile2(PaintEventArgs e)
{
List <GpsLocation> lst;
if (elevationProfileNew == null)
{
return;
}
var pen = new Pen(Brushes.Black, 3);
var data = elevationProfileNew.GetData();
for (ushort i = 0; i < 360; i++)
{
var thisAngle = elevationProfileNew.GetData(i);
var prevAngle = elevationProfileNew.GetData(i - 1);
if (thisAngle != null && prevAngle != null)
{
foreach (var point in thisAngle.GetPoints())
{
foreach (var otherPoint in prevAngle.GetPoints())
{
if (Math.Abs(point.Distance.Value - otherPoint.Distance.Value) < point.Distance / _minDist)
{
var b1 = GpsUtils.Normalize360(point.Bearing.Value - _heading);
var x1 = GpsUtils.Normalize360(b1 + 35) * DG_WIDTH;
var y1 = 250 - point.VerticalViewAngle.Value * 40;
var b2 = GpsUtils.Normalize360(otherPoint.Bearing.Value - _heading);
var x2 = GpsUtils.Normalize360(b2 + 35) * DG_WIDTH;
var y2 = 250 - otherPoint.VerticalViewAngle.Value * 40;
if (Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2)) < 100)
{
e.Graphics.DrawLine(pen, (float)x1, (float)y1, (float)x2, (float)y2);
}
}
}
}
}
}
}
public static Apollo.Gps ApolloConvertFrom(GpsOdometryData data)
{
var angles = data.Orientation.eulerAngles;
float yaw = -angles.y;
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
return(new Apollo.Gps()
{
header = new Apollo.Header()
{
timestamp_sec = GpsUtils.UtcToGpsSeconds(dt),
sequence_num = data.Sequence,
},
localization = new Apollo.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
position = new Apollo.PointENU()
{
x = data.Easting, // East from the origin, in meters.
y = data.Northing, // North from the origin, in meters.
z = data.Altitude // Up from the WGS-84 ellipsoid, in meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the world coordinate (East/North/Up).
orientation = ConvertApolloQuaternion(data.Orientation),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_velocity = new Apollo.Point3D()
{
x = data.Velocity.x,
y = data.Velocity.z,
z = data.Velocity.y,
},
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
}
});
}
public bool IsValid(GpsLocation newLocation, double newMaxDistance)
{
//If me move more than 100m
if (GpsUtils.QuickDistance(newLocation, MyLocation) > 0.1)
{
return(false);
}
if (Math.Abs(newLocation.Altitude - MyLocation.Altitude) > 100)
{
return(false);
}
if (newMaxDistance > MaxDistance)
{
return(false);
}
return(true);
}
public void CheckAndReloadElevationProfile(Context appContext, int maxDistance, IAppContext context)
{
if (context.Settings.ShowElevationProfile)
{
if (GpsUtils.HasAltitude(context.MyLocation))
{
if (_elevationProfileBeingGenerated == false)
{
if (context.ElevationProfileData == null || !context.ElevationProfileData.IsValid(context.MyLocation, context.Settings.MaxDistance))
{
GenerateElevationProfile(appContext, maxDistance, context.MyLocation);
}
else
{
RefreshElevationProfile(context.ElevationProfileData);
}
}
}
}
}
private void GenerateElevationProfile(Context appContext, int maxDistance, GpsLocation myLocation)
{
try
{
if (!GpsUtils.HasAltitude(myLocation))
{
PopupHelper.ErrorDialog(appContext, Resource.String.Main_ErrorUnknownAltitude);
return;
}
_elevationProfileBeingGenerated = true;
var ec = new ElevationCalculation(myLocation, maxDistance);
var size = ec.GetSizeToDownload();
if (size == 0)
{
StartDownloadAndCalculate(appContext, ec);
return;
}
using (var builder = new AlertDialog.Builder(appContext))
{
builder.SetCancelable(false);
builder.SetTitle(appContext.Resources.GetText(Resource.String.Common_Question));
builder.SetMessage(String.Format(appContext.Resources.GetText(Resource.String.Download_Confirmation), size));
builder.SetIcon(Android.Resource.Drawable.IcMenuHelp);
builder.SetPositiveButton(appContext.Resources.GetText(Resource.String.Common_Yes), (senderAlert, args) => { StartDownloadAndCalculateAsync(appContext, ec); });
builder.SetNegativeButton(appContext.Resources.GetText(Resource.String.Common_No), (senderAlert, args) => { _elevationProfileBeingGenerated = false; });
var myCustomDialog = builder.Create();
myCustomDialog.Show();
}
}
catch (Exception ex)
{
PopupHelper.ErrorDialog(appContext, Resource.String.Main_ErrorGeneratingElevationProfile, ex.Message);
}
}
public static Apollo.GnssBestPose ConvertFrom(GpsData data)
{
float Accuracy = 0.01f; // just a number to report
double Height = 0; // sea level to WGS84 ellipsoid
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
var measurement_time = GpsUtils.UtcToGpsSeconds(dt);
return(new Apollo.GnssBestPose()
{
header = new Apollo.Header()
{
timestamp_sec = measurement_time,
sequence_num = data.Sequence++,
},
measurement_time = measurement_time,
sol_status = 0,
sol_type = 50,
latitude = data.Latitude,
longitude = data.Longitude,
height_msl = Height,
undulation = 0,
datum_id = 61, // datum id number
latitude_std_dev = Accuracy, // latitude standard deviation (m)
longitude_std_dev = Accuracy, // longitude standard deviation (m)
height_std_dev = Accuracy, // height standard deviation (m)
base_station_id = "0", // base station id
differential_age = 2.0f, // differential position age (sec)
solution_age = 0.0f, // solution age (sec)
num_sats_tracked = 15, // number of satellites tracked
num_sats_in_solution = 15, // number of satellites used in solution
num_sats_l1 = 15, // number of L1/E1/B1 satellites used in solution
num_sats_multi = 12, // number of multi-frequency satellites used in solution
extended_solution_status = 33, // extended solution status - OEMV and greater only
galileo_beidou_used_mask = 0,
gps_glonass_used_mask = 51
});
}
public static apollo.drivers.gnss.GnssBestPose ConvertFrom(GpsData data)
{
float Accuracy = 0.01f; // just a number to report
double Height = 0; // sea level to WGS84 ellipsoid
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
var measurement_time = GpsUtils.UtcToGpsSeconds(dt);
return(new apollo.drivers.gnss.GnssBestPose()
{
header = new apollo.common.Header()
{
sequence_num = data.Sequence,
frame_id = data.Frame,
timestamp_sec = measurement_time,
},
measurement_time = measurement_time,
sol_status = apollo.drivers.gnss.SolutionStatus.SolComputed,
sol_type = apollo.drivers.gnss.SolutionType.NarrowInt,
latitude = data.Latitude, // in degrees
longitude = data.Longitude, // in degrees
height_msl = Height, // height above mean sea level in meters
undulation = 0, // undulation = height_wgs84 - height_msl
datum_id = apollo.drivers.gnss.DatumId.Wgs84, // datum id number
latitude_std_dev = Accuracy, // latitude standard deviation (m)
longitude_std_dev = Accuracy, // longitude standard deviation (m)
height_std_dev = Accuracy, // height standard deviation (m)
base_station_id = Encoding.UTF8.GetBytes("0"), //CopyFrom((byte)"0"), // base station id
differential_age = 2.0f, // differential position age (sec)
solution_age = 0.0f, // solution age (sec)
num_sats_tracked = 15, // number of satellites tracked
num_sats_in_solution = 15, // number of satellites used in solution
num_sats_l1 = 15, // number of L1/E1/B1 satellites used in solution
num_sats_multi = 12, // number of multi-frequency satellites used in solution
extended_solution_status = 33, // extended solution status - OEMV and greater only
galileo_beidou_used_mask = 0,
gps_glonass_used_mask = 51
});
}
public void GetPointsFromLocationsTest1()
{
var locations = new List <GpsLocation>()
{
new GpsLocation {
Timestamp = 0
},
new GpsLocation {
Timestamp = 1
},
new GpsLocation {
Timestamp = 2
},
new GpsLocation {
Timestamp = 10
},
new GpsLocation {
Timestamp = 11
},
new GpsLocation {
Timestamp = 20
}
};
var paths = (List <PathData>)GpsUtils.GetPathPointsFromLocations(locations, 1);
Assert.AreEqual(3, paths.Count);
Assert.AreEqual(0, paths[0].Id);
Assert.AreEqual(3, paths[0].Points.Count);
Assert.AreEqual(1, paths[1].Id);
Assert.AreEqual(2, paths[1].Points.Count);
Assert.AreEqual(2, paths[2].Id);
Assert.AreEqual(1, paths[2].Points.Count);
}
public void CalculateProfile()
{
GpsUtils.BoundingRect(_myLocation, _visibility * 1000, out var min, out var max);
_data = new List <GpsLocation>();
string inputFileName = @"c:\Temp\ElevationMap\ALPSMLC30_N049E018_DSM.tif";
GeoTiffReaderList.ReadTiff(inputFileName, min, max, _myLocation, 3, _data);
//Calculate old profile
/*ElevationProfile ep = new ElevationProfile();
* ep.GenerateElevationProfile(_myLocation, _visibility, _data, progress => { });
* elevationProfileOld = ep.GetProfile();*/
//Calucate new profile
var etc = new ElevationTileCollection(_myLocation, (int)_visibility);
var d = etc.GetSizeToDownload();
etc.Download(progress => { });
etc.Read(progress => { });
profileGenerator.Generate(_myLocation, 12, etc, progress => { });
/*ProfileGeneratorOld ep2 = new ProfileGeneratorOld();
* //ep2.GenerateElevationProfile3(_myLocation, _visibility, _data, progress => { });
* ep2.GenerateElevationProfile3(_myLocation, _visibility, elevationPainter3.list, progress => { });
* elevationProfileNew = ep2.GetProfile();*/
ElevationProfile ep3 = new ElevationProfile();
ep3.GenerateElevationProfile3(_myLocation, _visibility, profileGenerator.GetProfile(), progress => { });
elevationProfileNew = ep3.GetProfile();
Invalidate();
}
public string LocationAsShortString()
{
return(GpsUtils.LocationAsShortString(Latitude, Longitude));
}
void PaintTerrain(PaintEventArgs e)
{
var pen = new Pen(Brushes.Black, 20);
var _data2 = new List <GpsLocation>();
var sortedData = _data
.Where(i =>
i.Distance > MIN_DISTANCE && i.Distance < _visibility * 1000 &&
GpsUtils.IsAngleBetween(i.Bearing.Value, _heading, 35))
.OrderByDescending(i2 => i2.Distance);
foreach (var point in sortedData)
{
var b = GpsUtils.Normalize360(point.Bearing.Value - _heading);
var c = (point.Distance / (_visibility * 1000)) * 200;
pen.Color = Color.FromArgb(100, (int)c, (int)c, (int)c);
var x = GpsUtils.Normalize360(b + 35) * DG_WIDTH;
var y = 250 - point.VerticalViewAngle * 40;
e.Graphics.DrawLine(pen, (float)x, (float)500, (float)x, (float)y);
}
var z = _data
.Where(i => i.Distance > MIN_DISTANCE && i.Distance < _visibility * 1000)
.GroupBy(i => Math.Floor(i.Bearing.Value));
foreach (var i in z)
{
var bearing = i.Key;
var points = i.OrderBy(i2 => i2.Distance);
List <GpsLocation> displayedPoints = new List <GpsLocation>();
foreach (var point in points)
{
bool display = true;
foreach (var poi in displayedPoints)
{
if (point.VerticalViewAngle < poi.VerticalViewAngle)
{
display = false;
break;
}
}
if (display || displayedPoints.Count == 0)
{
displayedPoints.Add(point);
}
}
displayedPoints.OrderByDescending(j => j.Distance);
foreach (var point in displayedPoints)
{
bool display = true;
foreach (var otherPoint in displayedPoints)
{
if (point.Altitude < otherPoint.Altitude && Math.Abs(point.Distance.Value - otherPoint.Distance.Value) < 500)
{
display = false;
}
}
if (display)
{
_data2.Add(point);
}
}
}
//foreach (var point in sortedData)
//{
// var b = GeoPoint.Normalize360(point.Bearing - _heading);
// var c = (point.Distance / (_visibility * 1000)) * 200;
// pen.Color = Color.FromArgb(100, (int)c, (int)c, (int)c);
// var x = GeoPoint.Normalize360(b + 35) * DG_WIDTH;
// var y = 250 - point.VerticalAngle * 40;
// if (IsMax(point))
// {
// e.Graphics.DrawLine(pen, (float)x, (float)y-10, (float)x, (float)y);
// }
//}
}
public static Apollo.ChassisMsg ConvertFrom(CanBusData data)
{
var eul = data.Orientation.eulerAngles;
float dir;
if (eul.y >= 0)
{
dir = 45 * UnityEngine.Mathf.Round((eul.y % 360) / 45.0f);
}
else
{
dir = 45 * UnityEngine.Mathf.Round((eul.y % 360 + 360) / 45.0f);
}
var measurement_time = GpsUtils.UtcSecondsToGpsSeconds(data.Time);
var gpsTime = DateTimeOffset.FromUnixTimeSeconds((long)measurement_time).DateTime.ToLocalTime();
return(new Apollo.ChassisMsg()
{
header = new Apollo.Header()
{
timestamp_sec = data.Time,
module_name = "chassis",
sequence_num = data.Sequence,
},
engine_started = data.EngineOn,
engine_rpm = data.EngineRPM,
speed_mps = data.Speed,
odometer_m = 0,
fuel_range_m = 0,
throttle_percentage = data.Throttle,
brake_percentage = data.Braking,
steering_percentage = -data.Steering * 100,
parking_brake = data.ParkingBrake,
high_beam_signal = data.HighBeamSignal,
low_beam_signal = data.LowBeamSignal,
left_turn_signal = data.LeftTurnSignal,
right_turn_signal = data.RightTurnSignal,
wiper = data.Wipers,
driving_mode = Apollo.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
gear_location = data.InReverse ? Apollo.Chassis.GearPosition.GEAR_REVERSE : Apollo.Chassis.GearPosition.GEAR_DRIVE,
chassis_gps = new Apollo.Chassis.ChassisGPS()
{
latitude = data.Latitude,
longitude = data.Longitude,
gps_valid = true,
year = gpsTime.Year,
month = gpsTime.Month,
day = gpsTime.Day,
hours = gpsTime.Hour,
minutes = gpsTime.Minute,
seconds = gpsTime.Second,
compass_direction = dir,
pdop = 0.1,
is_gps_fault = false,
is_inferred = false,
altitude = data.Altitude,
heading = eul.y,
hdop = 0.1,
vdop = 0.1,
quality = Apollo.Chassis.GpsQuality.FIX_3D,
num_satellites = 15,
gps_speed = data.Velocity.magnitude,
}
});
}
public ElevationData GetData(int angle)
{
return(elevationData.SingleOrDefault(i => i.Angle == GpsUtils.Normalize360(angle)));
}
