static (View view, PoseDesc pose) GnssToViewPose(GPSPosition origin, GPSPosition gnssPos, uint TimeStamp)
{
var frameNum = FrameChunks.FrameCloseTo(TimeStamp + 500, SourceVideo.TIME_BASE);
var id = ViewIDs.Get(frameNum);
var pos = origin.GetVector(gnssPos);
var pose = new PoseDesc
{
poseId = $"{id}",
pose = new Pose
{
transform = new Transform
{
center = new float[] { pos.x, -pos.y, pos.z },
rotation = new float[]
{
1, 0, 0,
0, 1, 0,
0, 0, 1,
},
}
}
};
var view = new View
{
viewId = pose.poseId,
poseId = pose.poseId,
path = frameNum.ToString("D4") + ".jpg",
metadata = new Metadata {
},
};
return(view, pose);
}
private void AddPosition()
{
GPSPosition gPSPosition = new GPSPosition(Name.text, float.Parse(Latitude.text), float.Parse(Longitude.text));
SocketLinkerObj.clientSender.SendPacket(new CarGPSSpotStatusChangeReqPacket(GPSSpotManagerChangeType.AddSpot, gPSPosition));
AddPosPanel.SetActive(false);
}
private void GPSPositionChangedEventHandler(object sender, EventArgs e)
{
try
{
NMEAGPSClient.NMEAGPSClient.GPSPositionEvent gpsPosition = (NMEAGPSClient.NMEAGPSClient.GPSPositionEvent)e;
GPSPosition position = gpsPosition.Position;
NMEAGPSClient.NMEAGPSClient nmeaClient = (NMEAGPSClient.NMEAGPSClient)sender;
// make sure we have valid data
byte gpsState = (byte)position.NMEAMode;
if (position.HorizontalErrorEstimate > TrackingService.DefaultAcceptableError)
{
gpsState &= 0xfe;
}
// publish the new position to CAN clients.
CANFrame frame = CNXCANMsgHelper.PackGPSFrame(gpsState, position.CurrentPosition.Latitude, position.CurrentPosition.Longitude, position.SpeedOverGround);
int count = mCANClient.Send(frame);
if ((gpsState == (byte)CANGPSState.GoodFix) || (gpsState == (byte)CANGPSState.PoorFix))
{
// publish the new time.
frame = CNXCANMsgHelper.PackDateTime(position.TimeStamp);
count = mCANClient.Send(frame);
}
CNXLog.Debug("GPSPositionChangedEventHandler");
}
catch (Exception ex)
{
CNXLog.Error("CAN GPSPositionChangedEventHandler", ex);
}
}
public void UpdatePosition(string Projection,
double North, double East, double Altitude)
{
Position = new GPSPosition(Projection, North, East, Altitude);
PositionUpdatedEvent?.Invoke(Position);
UpdatedEvent?.Invoke();
}
public CarGPSSpotStatusChangeReqPacket(GPSSpotManagerChangeType gPSSpotManagerChangeType, object value) : this()
{
GPSSpotManagerChangeType = gPSSpotManagerChangeType;
switch (gPSSpotManagerChangeType)
{
case GPSSpotManagerChangeType.AddSpot:
GPSPosition = value as GPSPosition;
break;
case GPSSpotManagerChangeType.RemoveSpot:
Index = (int)value;
break;
case GPSSpotManagerChangeType.SetCurrentPos:
Index = (int)value;
break;
case GPSSpotManagerChangeType.OverWrite:
GPSMover = value as GPSSpotManager;
break;
default:
break;
}
}
private void UpdatePosition()
{
if (mNMEAClient != null)
{
try
{
if ((mNMEAClient.Status & NMEAGPSClient.NMEAGPSClient.GPS_STATUS.Fix) == NMEAGPSClient.NMEAGPSClient.GPS_STATUS.Fix)
{
GPSPosition position = mNMEAClient.PositionInformation;
mLocationMessage.SetLocation((uint)position.HorizontalErrorEstimate, position.CurrentPosition.Latitude, position.CurrentPosition.Longitude, position.SpeedOverGround);
TrackingEvent tEv = (mNMEAClient.TravellingState == NMEAGPSClient.NMEAGPSClient.MovingState.Moving) ? TrackingEvent.POSITION_MOVING : TrackingEvent.POSITION_STATIONARY;
StateMachine(tEv);
}
else
{
mLocationMessage.MessageId = (mNMEAClient.Status == NMEAGPSClient.NMEAGPSClient.GPS_STATUS.OFFLINE) ? RTTMesg.RTTOutMsgIds.GPSNone : RTTMesg.RTTOutMsgIds.GPSNoFix;
StateMachine(TrackingEvent.LOST_GPS);
//CNXLog.WarnFormat("Setting state for No Fix {0}", mMobileMessage);(mNMEAClient.Status == NMEAGPSClient.NMEAGPSClient.GPS_STATUS.OFFLINE)
}
}
catch (Exception ex)
{
CNXLog.Error("TrackerState GPS position error {0}", ex);
}
}
}
public SceneModel(string ModelID, string Name, GameObject Prefab,
GPSPosition Position, bool Hidden = false)
{
this.CloudID = ModelID;
this.Name = Name;
this.Prefab = Prefab;
this.Position = Position;
this.Hidden = Hidden;
}
public void SetDisplayGPSLocation(GPSPosition gPSPosition)
{
if (gPSPosition == null)
{
TextLocation.text = "N/A";
GPSLocation.text = "0, 0";
return;
}
TextLocation.text = gPSPosition.LocationName;
GPSLocation.text = gPSPosition.latitude + ", " + gPSPosition.longitude;
}
public static string Format(GPSPosition pos, FixQuality Quality)
{
return(string.Format(
"{0}\n" +
"X {1:0.00}\n" +
"Y {2:0.00}\n" +
"Z {3:0.00}\n" +
"Q {4}",
pos.Projection.Replace("_", " "),
pos.North, pos.East, pos.Altitude,
Quality.ToString()));
}
//float[,] CreateHeightMap(List<GPSPosition> gpsdata)
//{
// var accuracy = 5000;
// //find size of array
// IEnumerable<GPSPosition> orderedlat = gpsdata.OrderByDescending(x => x.Latitude);
// double highestlat = orderedlat.First().Latitude;
// double lowestlat = orderedlat.Last().Latitude;
// IEnumerable<GPSPosition> orderedlong = gpsdata.OrderByDescending(x => x.Longitude);
// double leftmostlong = orderedlong.Last().Longitude;
// double rightmostlong = orderedlong.First().Longitude;
// double xdisplace = leftmostlong;
// double ydisplace = lowestlat;
// int xheight = Convert.ToInt32((rightmostlong - leftmostlong) * accuracy);
// int yheight = Convert.ToInt32((highestlat - lowestlat) * accuracy);
// //get max and min altitudes
// IEnumerable<GPSPosition> orderedalt = gpsdata.OrderByDescending(x => x.Altitude);
// double high = orderedalt.First().Altitude;
// double low = orderedalt.Last().Altitude;
// Texture2D texture = new Texture2D(xheight + 1, yheight + 1);
// var asdf = texture.GetPixel(1, 1);
// Color32[] colors = CreateBaseColours(xheight, yheight);
// float[,] rasterarray = new float[yheight + 1, xheight + 1]; //create array to that size
// GPSPosition previousPos = null;
// //insert gps data into appropriate index
// foreach (var pos in gpsdata)
// { //add estimates between points: store previous value, compare current and previous, calculate intermediate points, update them with averaged altitude
// int relativelatitude = Convert.ToInt32((pos.Latitude - ydisplace) * accuracy);
// int relativelongitude = Convert.ToInt32((pos.Longitude - xdisplace) * accuracy);
// rasterarray[relativelatitude, relativelongitude] = GetRelativeAltitude(high, low, pos); //array in (y, x) format
// var colorindex = (relativelatitude * (xheight + 1)) + relativelongitude; //find the flattened index of a 2d array
// float altitudefloat = GetRelativeAltitude(high, low, pos);
// Debug.Log(altitudefloat);
// colors[colorindex] = new Color(altitudefloat, altitudefloat, altitudefloat);
// if (previousPos != null)
// {
// //calculate rasterarray indexes between previousPos and Pos
// //set each value to the estimated height
// var y2 = relativelatitude;
// var x2 = relativelongitude;
// var x1 = Convert.ToInt32((previousPos.Longitude - xdisplace) * accuracy);
// var y1 = Convert.ToInt32((previousPos.Latitude - ydisplace) * accuracy);
// //formula y = ax + b
// // for calculating the coordinates between the two latest points
// //if(x1 != x2)
// //{
// // var a = (y2 - y1) / (x2 - x1); //if x changes but y stays the same, then this breaks. What to do to fix?
// // var b = (y1 - (a * x1));
// // for (int x = x1; x <= x2; x++)
// // {
// // int y = Convert.ToInt32((a * x) + b);
// // try
// // {
// // rasterarray[y, x] = GetRelativeAltitude(high, low, pos);
// // }
// // catch (Exception e)
// // {
// // Debug.Log(y + " " + x);
// // }
// // }
// //}
// }
// if (pos.Altitude != 0)
// {
// previousPos = pos;
// }
// }
// texture.SetPixels32(colors); //set 1d array of colors to 2d texture pixels
// texture.Apply();
// File.WriteAllBytes(Application.persistentDataPath + "/gpsraster.png", texture.EncodeToPNG());
// //create terrain
// TerrainData terraindata = new TerrainData {
// size = new Vector3(xheight + 1, 500, yheight + 1), //change value to increase scale in the upwards direction, change to be based on x and y values?
// };
// terraindata.heightmapResolution = xheight > yheight ? xheight * 2 + 1 : yheight * 2 + 1; //*2 for now just to stop error. todo: fix bug that makes resolution less than biggest height for some reason
// terraindata.SetHeights(0, 0, rasterarray);
// //change texture
// var textureInSplats = new SplatPrototype[1];
// textureInSplats[0] = new SplatPrototype();
// textureInSplats[0].texture = texture;
// textureInSplats[0].tileSize = new Vector2((xheight + 1) * terraindata.heightmapScale.x, (yheight + 1) * terraindata.heightmapScale.z);
// terraindata.splatPrototypes = textureInSplats;
// var terrain = Terrain.CreateTerrainGameObject(terraindata);
// return rasterarray;
//}
private static float GetRelativeAltitude(double high, double low, GPSPosition pos) //converts the altitude into a scale between 0 and 1 based on max and min altitudes
{
if (pos.Altitude != 0)
{
var altitudedifference = high - low;
var b = pos.Altitude - low;
float altitudefloat = (float)(b / altitudedifference);
return(altitudefloat);
}
else
{
return(0.5f);
}
}
public void toWGS84(GPSPosition pos, out double longitude, out double latitude, out double elevation)
{
var wgsPos = Proj4.proj_trans(Proj,
Proj4.PJ_DIRECTION.PJ_INV,
Proj4.proj_coord(pos.East, pos.North, pos.Altitude));
if (double.IsInfinity(wgsPos.north) || double.IsInfinity(wgsPos.east) || double.IsInfinity(wgsPos.up))
{
/* proj4 returns infinity value if it can't transform position */
throw new CoordinateTransformationException("WGS84", pos);
}
longitude = Utils.toDegrees(wgsPos.east);
latitude = Utils.toDegrees(wgsPos.north);
elevation = wgsPos.up;
}
public SceneModel GetModel(string ModelID, string Name,
GameObject Prefab, GPSPosition Position)
{
var modInst = Find(ModelID);
var sceneMod = modInst == null ?
/* instance for this video not found */
new SceneModel(ModelID, Name, Prefab, Position) :
/* instance found, override position */
new SceneModel(ModelID, Name, Prefab, modInst.position.GpsPos, modInst.hidden);
/* listen for this model's changes, so we can write 'em down */
sceneMod.UpdatedEvent += () => ModelUpdated(sceneMod);
return(sceneMod);
}
public static List <GPSPosition> ReadLog()
{
List <GPSPosition> gpsList = new List <GPSPosition>();
if (File.Exists(path))
{
var logFile = File.ReadAllLines(path);
var logList = new List <string>(logFile);
foreach (var item in logList)
{
var split = item.Split(',');
GPSPosition x = new GPSPosition(Convert.ToDouble(split[0]), Convert.ToDouble(split[1]), Convert.ToDouble(split[2]));
gpsList.Add(x);
}
}
return(gpsList);
}
void setTrackerHomeFromModule(object sender, EventArgs e)
{
GPSDevice gpsModule = getDevice();
if (gpsModule != null)
{
_Available = true;
GPSPosition pos = gpsModule.GetCoordinates();
double alt = getGEAlt(pos.Lat, pos.Lng);
Host.comPort.MAV.cs.TrackerLocation = new PointLatLngAlt(pos.Lat, pos.Lng, alt, "Tracker Home");
}
else
{
_Available = false;
CustomMessageBox.Show("No GPS Device connected. Please verify it is connected and try again.");
}
}
public void SetStatus(Cardevice cardevice, GPSPosition _gPSPosition, float _rotation)
{
f_sonardist = cardevice.F_sonardist;
rh_sonardist = cardevice.RH_sonardist;
lh_sonardist = cardevice.LH_sonardist;
rs_sonardist = cardevice.RS_sonardist;
ls_sonardist = cardevice.LS_sonardist;
R_motorDIR = cardevice.R_motorDIR;
L_motorDIR = cardevice.L_motorDIR;
r_motorpower = cardevice.R_motorpower;
l_motorpower = cardevice.L_motorpower;
rf_LED = cardevice.RF_LED;
lf_LED = cardevice.LF_LED;
rb_LED = cardevice.RB_LED;
lb_LED = cardevice.LB_LED;
gPSPosition = _gPSPosition;
rotation = _rotation;
}
private static void ProcessPosition(GPSPosition position)
{
//last_point = new GPSPoint(position.Time.ToUniversalTime(), (float)position.dblLatitude, (float)position.dblLongitude, Convert.ToInt32(position.flSpeed))
last_point = new GPSPoint(position.Time, (float)position.dblLatitude, (float)position.dblLongitude, Convert.ToInt32(position.flSpeed))
{
Course = position.flHeading
};
T.INFO(last_point.ToString());
double delta_distance = -1;
mobile.Fix(last_point);
if (destination_point != null)
{
delta_distance = Distances.Rhumb(last_point, destination_point);
T.TRACE(String.Format("TRACKER: delta_distance={0}", delta_distance));
//Locked = delta_distance <= destination_area_radio;
/*if (delta_distance <= destination_radio)
* {
* T.INFO("TRACKER: Llegamos al destino. Informando.");
* Beep("gps_in_destination");
* posicion.Clear();
* destination_point = null;
* var dummy = new byte[2];
* dummy[0] = 0;
* posicion.Push(last_point.AsMessage((int)delta_distance), dummy);
* mobile.ActiveOT(last_point, active_ot);
* }*/
}
if (last_sent_point == null || (last_point.Date - last_sent_point.Date).TotalSeconds >= fix_interval)
{
T.INFO("TRACKER: fix seleccionado para enviar.");
last_sent_point = last_point;
mobile.Fix(last_sent_point);
posicion.Clear();
var dummy = new byte[2];
dummy[0] = 0;
posicion.Push(last_sent_point.AsMessage((int)delta_distance), dummy);
}
}
List <GPSPosition> ConvertToMeters(List <GPSPosition> gpsdata)
{
IEnumerable <GPSPosition> orderedlat = gpsdata.OrderByDescending(x => x.Latitude);
double highestlat = orderedlat.First().Latitude;
double lowestlat = orderedlat.Last().Latitude;
IEnumerable <GPSPosition> orderedlong = gpsdata.OrderByDescending(x => x.Longitude);
double leftmostlong = orderedlong.Last().Longitude;
double rightmostlong = orderedlong.First().Longitude;
GPSPosition metersbasis = new GPSPosition(lowestlat, leftmostlong, 0.0);
List <GPSPosition> gpsinmeters = new List <GPSPosition>();
foreach (var pos in gpsdata)
{
//convert to meters, from metersbasis
GPSPosition converted = ToMeters(pos, metersbasis);
gpsinmeters.Add(converted);
}
return(gpsinmeters);
}
private GPSPosition GetCurrentLocation()
{
//IGPSPositionProvider gpsPositionProvider = new GPSPositionProvider(new GPSPositionProviderConfiguration(), new GeopositionToGPSPositionConverter());
GPSPositionProvider gpsPositionProvider = GPSPositionProvider.CurrentInstance;
GPSPosition position = null;
try
{
//Task<GPSPosition> location = null;
GPSPosition location = null;
//Task.Run(() => location = gpsPositionProvider.GetCurrentPositionAsync()).Wait();
location = gpsPositionProvider.GetCurrentPositionAsync();
position = location;
}
catch (COMException comException)
{
// The GPS location was not resolved.
}
return(position);
}
void deviceChanged(MainV2.WM_DEVICECHANGE_enum cause)
{
GPSDevice gpsModule = getDevice();
if ((gpsModule != null))
{
if (_Available == false)
{
_Available = true;
if (CustomMessageBox.Show("A GPS module was detected on your system. Would you like to use it to set your tracker home location?", "Tracker Home", MessageBoxButtons.YesNo) == DialogResult.Yes)
{
GPSPosition pos = gpsModule.GetCoordinates();
double alt = getGEAlt(pos.Lat, pos.Lng);
Host.comPort.MAV.cs.TrackerLocation = new PointLatLngAlt(pos.Lat, pos.Lng, alt, "Tracker Home");
}
}
}
else if (_Available == true)
{
_Available = false;
}
}
public static void toWGS84(GPSPosition pos, out double longitude, out double latitude, out double elevation)
{
var converter = GeodesyProjections.GetConverter(pos.Projection);
converter.toWGS84(pos, out longitude, out latitude, out elevation);
}
private void gpsTrimble_NewPosition(GPSPosition position)
{
pos = position;
statusTrimble();
}
GPSPosition ToMeters(GPSPosition pos, GPSPosition metersbasis)
{
//convert gpsdata to meters from metersbasis
//this may have to change from 'meters' to 'decimal degrees'
return(null);
}
public CarStatusPacket(Cardevice _cardevice, GPSPosition _position, float _rotation) : this()
{
cardevice = _cardevice;
position = _position;
rotation = _rotation;
}
public void SetDisplayPosition(int index, GPSPosition gPSPosition)
{
Index = index;
Position.text = "Lat : " + gPSPosition.latitude + ", Long : " + gPSPosition.longitude;
}
private void ticking()
{
try
{
if (!glbGPS.IsOpen())
{
ST("No esta abierto el GPS!!!");
}
GPSPosition Position = glbGPS.GetPosition();
if ((Position != null))
{
//HDOPLabel.Text = Position.flHorizontalDilutionOfPrecision.ToString("0.00");
//ErrorLabel.Text = (Position.flHorizontalDilutionOfPrecision * 6) + " m";
//SatelitesLabel.Text = Position.dwSatelliteCount.ToString();
//if (Position.isValidField(GPSPosition.GPS_VALID_FIELDS.GPS_VALID_LONGITUDE))
//{
// LongLabel.Text = Position.dblLongitude.ToString("0.0000000");
//}
//else
//{
// LongLabel.Text = "";
//}
//if (Position.isValidField(GPSPosition.GPS_VALID_FIELDS.GPS_VALID_LATITUDE))
//{
// LatLabel.Text = Position.dblLatitude.ToString("0.0000000");
//}
//else
//{
// LatLabel.Text = "";
//}
//try
//{
// if (Position.isValidField(GPSPosition.GPS_VALID_FIELDS.GPS_VALID_UTC_TIME))
// {
// StampLabel.Text = Position.Time.ToString("yyyy/MM/dd HH:mm");
// }
// else
// {
// StampLabel.Text = "";
// }
//}
//catch (Exception ex)
//{
// StampLabel.Text = ex.ToString(); //Position.Time.ToString();
//}
//switch (Position.fixType)
//{
// case GPSFixType.Unknown:
// FixTypeLabel.Text = "";
// break;
// case GPSFixType.XyD:
// FixTypeLabel.Text = "2D";
// break;
// case GPSFixType.XyzD:
// FixTypeLabel.Text = "3D";
// break;
//}
//switch (Position.fixQuality)
//{
// case GPSFixQuality.DGps:
// FixTypeLabel.Text += " DGPS";
// break;
// case GPSFixQuality.Gps:
// FixTypeLabel.Text += " GPS";
// break;
//}
lastPosition = Position;
if (glbGPS.IsPositionValid(Position))
{
pnlFixed.BackColor = Color.LimeGreen;
StatusLabel.BackColor = Color.LimeGreen;
count++;
ST(String.Concat("Posicion obtenida ", count.ToString()));
if (count == 3)
{
gotFix = true;
btAceptar.Enabled = true;
Time = DateTime.Now;
//cierro porque ya tengo fix
timer1.Enabled = false;
this.Close();
}
}
else
{
count = 0;
pnlFixed.BackColor = Color.OrangeRed;
StatusLabel.BackColor = Color.OrangeRed;
if (!odd)
{
odd = true;
ST("Esperando posición");
}
else
{
odd = false;
ST("Buscando satélites");
}
gotFix = false;
btAceptar.Enabled = false;
}
}
else
{
//GrabarButton.Enabled = False
pnlFixed.BackColor = Color.Red;
}
}
catch (Exception ex)
{
ST(ex.Message.ToString());
}
}
public static GPSsPosition GetGPSS(this GPSPosition gPSPosition)
{
return(new GPSsPosition(gPSPosition.latitude, gPSPosition.longitude));
}
} /* read only property */
public CoordinateTransformationException(string Projection, GPSPosition Position)
{
_Projection = Projection;
_Position = Position;
}
public Airport(string airportName, GPSPosition gPSPosition)
{
this.AirportName = airportName;
this._GPSPosition = gPSPosition;
}
private void gpsTrimble_NewPosition(GPSPosition position)
{
pos = position;
statusTrimble();
}
