public SbetRecord GetClosestRecord(DateTime pTime)
{
DateTime time = pTime;
SbetRecord lowKey = null, highKey = null;
foreach (KeyValuePair <DateTime, SbetRecord> key in Records)
{
if (key.Key.Ticks > time.Ticks)
{
highKey = key.Value;
break;
}
lowKey = key.Value;
}
SbetRecord rec;
if (time.Ticks - lowKey.utcTime.Ticks > highKey.utcTime.Ticks - time.Ticks)
{
rec = highKey;
}
else
{
rec = lowKey;
}
return(rec);
}
private void Load()
{
SortedList <DateTime, string> camEvents = new SortedList <DateTime, string>();
if (!string.IsNullOrEmpty(EventsList))
{
//Load the camera events
Parallel.ForEach(File.ReadAllLines(EventsList), e =>
{
List <string> temp = e.Split().ToList();
temp.RemoveAll(str => String.IsNullOrEmpty(str));
lock (camEvents) { camEvents.Add(GetFromGps(GPSWeek, Double.Parse(temp[0])), temp[1]); };
});
}
List <double> vals = new List <double>();
// Open an SBET file for reading...
using (BinaryReader reader = new BinaryReader(File.Open(LedgerPath, FileMode.Open, FileAccess.Read)))
{
int length = (int)reader.BaseStream.Length;
while (reader.BaseStream.Position != length)
{
vals.Add(reader.ReadDouble());
}
}
SortedList <DateTime, int> indexes = new SortedList <DateTime, int>();
for (int i = 0; i < vals.Count; i += 17)
{
indexes.Add(GetFromGps(GPSWeek, vals[i]), i);
}
//if no list of camevents is provided, just add all the records, otherwise only add the closest SBet records.
if (camEvents.Count > 0)
{
//Slow bit
SortedList <DateTime, int> newIndexes = new SortedList <DateTime, int>();
Parallel.ForEach(camEvents, (evnt) =>
{
lock (newIndexes) { newIndexes.Add(GetClosestKey(evnt.Key, indexes).Key, GetClosestKey(evnt.Key, indexes).Value); }
});
indexes = newIndexes;
}
LatLngUTMConverter converter = new LatLngUTMConverter("WGS 84");
Parallel.ForEach(indexes, (i) =>
{
DateTime tempUtcTime = i.Key;
double eastng = 0.0;
double northng = 0.0;
double height = vals[i.Value + 3];
if (CoordSyst == "UTM")
{
LatLngUTMConverter.UTMResult res = converter.ConvertLatLngToUtm((vals[i.Value + 1] * 180.0) / Math.PI, (vals[i.Value + 2] * 180.0) / Math.PI);
eastng = res.Easting;
northng = res.Northing;
}
else
{
Console.WriteLine("No coordinate system defined");
}
SbetRecord rec = new SbetRecord
{
//Not required here //time = vals[i],
utcTime = tempUtcTime,
//Not required here //latRad = vals[i + 1],
//Not required here //lonRad = vals[i + 2],
//Not required here //alt = height,
XYZ = new Point3D(eastng, northng, height),
//Not required here //xVel = vals[i + 4],
//Not required here //yVel = vals[i + 5],
//Not required here //zVel = vals[i + 6],
roll = vals[i.Value + 7],
pitch = vals[i.Value + 8],
yaw = vals[i.Value + 9],
//Not required here //headingDeg = RadianToDegree(vals[i + 9]),
//Not required here //wander = vals[i + 10],
//Not required here //xForce = vals[i + 11],
//Not required here //yForce = vals[i + 12],
//Not required here //zForce = vals[i + 13],
//Not required here //xAngRate = vals[i + 14],
//Not required here //yAngRate = vals[i + 15],
//Not required here //zAngRate = vals[i + 16]
};
lock (Records) { Records.Add(rec.utcTime, rec); }
});
}
