public winchPosition getWinchPosition(PlaneInfoResponse _planeInfoResponse, double _stringLength)
{
winchPosition _winchPosition = new winchPosition();
_winchPosition.location = FindPointAtDistanceFrom(new GeoLocation(_planeInfoResponse.Latitude, _planeInfoResponse.Longitude), _planeInfoResponse.PlaneHeading, _stringLength / 1000);
_winchPosition.alt = _planeInfoResponse.Altitude + 2;
return(_winchPosition);
}
public void captureTrackPoint(PlaneInfoResponse _planeInfoResponse, PlaneAvionicsResponse _planeAvionicsResponse, double absoluteTime, double baseInterval = 0.5)
{
double trackCaptureInterval = Math.Abs(_planeInfoResponse.AirspeedIndicated) > baseInterval?
Math.Max(baseInterval, Math.Abs(_planeInfoResponse.AirspeedIndicated) / 30 - 1) :
baseInterval * 5;
if (absoluteTime - lastTrackCapture >= trackCaptureInterval)
{
lastTrackCapture = absoluteTime;
trackRecording.Add(new TrackPoint(new GeoLocation(_planeInfoResponse.Latitude, _planeInfoResponse.Longitude), _planeInfoResponse.Altitude, (int)_planeInfoResponse.AltitudeAboveGround,
_planeInfoResponse.VelocityBodyZ, _planeInfoResponse.AirspeedIndicated, normalizeAngle(_planeInfoResponse.PlaneHeading), normalizeAngle(_planeInfoResponse.PlanePitch), normalizeAngle(_planeInfoResponse.PlaneBank),
packLights(_planeAvionicsResponse), packAvionics(_planeAvionicsResponse), DateTime.UtcNow, recordingCounter));
Console.WriteLine("Track capture: " + recordingCounter);
}
}
public winchDirection getForceDirection(winchPosition _winchPosition, PlaneInfoResponse _planeInfoResponse)
{
winchDirection _winchDirection = new winchDirection();
double globalX = (_winchPosition.location.Longitude - _planeInfoResponse.Longitude) * Math.Cos(_winchPosition.location.Latitude) * 6378137;
double globalY = _winchPosition.alt - _planeInfoResponse.Altitude;
double globalZ = (_winchPosition.location.Latitude - _planeInfoResponse.Latitude) * 180 / Math.PI * 111694;
Vector3 globalToWinch = new Vector3((float)globalX, (float)globalY, (float)globalZ);
Vector3 globalToWinchNorm = globalToWinch;
globalToWinchNorm.Normalize();
_winchDirection.climbAngle = Math.Abs(Math.Asin(globalToWinchNorm.Y));
Matrix3x3 attitude = Matrix3x3.CreateFromYawPitchRoll((float)_planeInfoResponse.PlaneHeading, (float)_planeInfoResponse.PlanePitch, (float)_planeInfoResponse.PlaneBank);
_winchDirection.localForceDirection = Matrix3x3.Multiply(attitude.Inverse(), globalToWinchNorm);
_winchDirection.localForceDirection.Normalize();
_winchDirection.heading = Math.Atan2(_winchDirection.localForceDirection.X, _winchDirection.localForceDirection.Z);
_winchDirection.pitch = Math.Asin(_winchDirection.localForceDirection.Y);
_winchDirection.distance = (double)(globalToWinch.Norm);
globalToWinch.Y = 0;
_winchDirection.groundDistance = (double)(globalToWinch.Norm);
if (_winchDirection.heading > Math.PI)
{
_winchDirection.heading -= 2 * Math.PI;
}
if (_winchDirection.heading < -Math.PI)
{
_winchDirection.heading += 2 * Math.PI;
}
if (_winchDirection.pitch > Math.PI)
{
_winchDirection.pitch -= 2 * Math.PI;
}
if (_winchDirection.pitch < -Math.PI)
{
_winchDirection.pitch += 2 * Math.PI;
}
return(_winchDirection);
}
public KeyValuePair <double, string> buildTrackFile(string appName, string nickName, PlaneInfoResponse _planeInfoResponse, PlaneAvionicsResponse _planeAvionicsResponse, MathClass _mathClass, string filename, bool timeAligned = false)
{
string str = "<?xml version=\"1.0\" encoding=\"UTF-8\"?><gpx creator=\"" + appName + "\" version=\"1.0\"><trk><name>" + _planeAvionicsResponse.Title + " - " + nickName + "</name><desc></desc><trkseg>";
TrackPoint prev = new TrackPoint(new GeoLocation(0, 0), 0, 0, 0, 0, 0, 0, 0, 0, 0, DateTime.Now, 0);
double distance = 0;
foreach (TrackPoint trackPoint in trackRecording)
{
if (prev.Elevation != 0)
{
double flatDistance = _mathClass.findDistanceBetweenPoints(prev.Location.Latitude, prev.Location.Longitude, trackPoint.Location.Latitude, trackPoint.Location.Longitude);
distance += flatDistance / Math.Cos((prev.Elevation - trackPoint.Elevation) / flatDistance);
}
DateTime timestamp = !timeAligned ? trackPoint.Time : (new DateTime(2000, 1, 1)).AddSeconds(trackPoint.Timer);
str += Environment.NewLine + "<trkpt lon=\"" + (trackPoint.Location.Longitude * 180 / Math.PI).ToString(CultureInfo.InvariantCulture) + "\" lat=\"" + (trackPoint.Location.Latitude * 180 / Math.PI).ToString(CultureInfo.InvariantCulture) + "\">" +
Environment.NewLine + " <ele>" + trackPoint.Elevation.ToString(CultureInfo.InvariantCulture) + "</ele>" +
Environment.NewLine + " <agl>" + trackPoint.AltitudeAboveGround.ToString() + "</agl>" +
Environment.NewLine + " <velocity>" + trackPoint.Velocity.ToString(CultureInfo.InvariantCulture) + "</velocity>" +
Environment.NewLine + " <airspeed>" + trackPoint.Airspeed.ToString(CultureInfo.InvariantCulture) + "</airspeed>" +
Environment.NewLine + " <heading>" + trackPoint.Heading + "</heading>" +
Environment.NewLine + " <pitch>" + trackPoint.Pitch + "</pitch>" +
Environment.NewLine + " <roll>" + trackPoint.Roll + "</roll>" +
Environment.NewLine + " <lights>" + trackPoint.Lights.ToString() + "</lights>" +
Environment.NewLine + " <avionics>" + trackPoint.Avionics.ToString() + "</avionics>" +
Environment.NewLine + " <time>" + timestamp.ToString("O") + "Z" + "</time>" +
Environment.NewLine + "</trkpt>";
prev = trackPoint;
}
str += "</trkseg></trk></gpx>";
if (trackRecording.Count <= 1)
{
//addLogMessage("Track data damaged!", 2);
}
return(new KeyValuePair <double, string>(distance, str));
}
