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