public static byte[] WriteReport(UavPositionReport item)
{
using (var stream = new MemoryStream())
{
using (var writer = new BinaryWriter(stream))
{
try
{
var position = item.ToUavPosition();
writer.WriteStruct(position);
writer.Write((byte)item.UavStatus.Count);
foreach (UavStatus status in item.UavStatus)
{
writer.WriteStruct(status.ToAutpStatusMessage());
}
}
catch (Exception e) when(!(e is SerializationException))
{
throw new SerializationException(e.ToString());
}
}
return(stream.ToArray());
}
}
/// <summary>
/// Convert an AUTP <see cref="UavPosition"/> to a <see cref="UavPositionReport"/>
/// </summary>
/// <returns>A UavPositionReport object</returns>
public static UavPositionReport ToPositionReport(this UavPosition position)
{
var report = new UavPositionReport
{
Alt = new Altitude(position.GpsAltitude, AltitudeDatum.Wgs84, position.AltitudeAccuracy),
IsAirborne = Enum.IsDefined(typeof(AirborneStatus), position.IsAirborne)
? (AirborneStatus)position.IsAirborne
: AirborneStatus.Unknown,
Pos = new Position(
(float)position.Latitude / UavPositionReportExtensions.PositionMultiplier,
(float)position.Longitude / UavPositionReportExtensions.PositionMultiplier,
position.PositionAccuracy
),
Velocity = new Velocity(
(float)position.EastVelocity / UavPositionReportExtensions.VelocityMultiplier,
(float)position.NorthVelocity / UavPositionReportExtensions.VelocityMultiplier,
(float)position.UpVelocity / UavPositionReportExtensions.VelocityMultiplier,
(float)position.VelocityAccuracy / UavPositionReportExtensions.VelocityMultiplier
),
Fuel = position.Fuel,
SatellitesVisible = position.SatellitesVisible,
GpsTimestamp = GpsTime.ToDateTime(position.GpsTimestamp).AddMilliseconds(position.GpsTimestampOffset)
};
return(report);
}
/// <summary>
/// Convert a <see cref="UavPositionReport"/> to an Autp <see cref="UavPosition"/>
/// </summary>
/// <remarks>
/// Altitude should be specified in Wgs8, as that's what the UDP paket is expected to provide.
/// If it isn't the altitude will be inaccurate if it is serialized back to canonical again.
/// No conversion is made.
/// </remarks>
/// <returns>A UavPosition object</returns>
public static UavPosition ToUavPosition(this UavPositionReport report)
{
var position = new UavPosition
{
// Position
Latitude = (int)(report.Pos.Lat * UavPositionReportExtensions.PositionMultiplier),
Longitude = (int)(report.Pos.Lon * UavPositionReportExtensions.PositionMultiplier),
PositionAccuracy = report.Pos.Accuracy,
IsAirborne = (byte)report.IsAirborne,
// Altitude
GpsAltitude = report.Alt.Value,
AltitudeAccuracy = report.Alt.Accuracy,
// Speed and heading
EastVelocity = (int)(report.Velocity.X * UavPositionReportExtensions.VelocityMultiplier),
NorthVelocity = (int)(report.Velocity.Y * UavPositionReportExtensions.VelocityMultiplier),
UpVelocity = (int)(report.Velocity.Z * UavPositionReportExtensions.VelocityMultiplier),
VelocityAccuracy = (int)(report.Velocity.Accuracy * UavPositionReportExtensions.VelocityMultiplier),
Fuel = report.Fuel,
SatellitesVisible = report.SatellitesVisible,
GpsTimestamp = GpsTime.ToGpsTimestamp(report.GpsTimestamp),
GpsTimestampOffset = (ushort)report.GpsTimestamp.Millisecond
};
return(position);
}
private async Task SendTelemetry(Models.FlightData flightData)
{
if (_settings.CurrentFlightId == null)
{
await _messagesService.AddMessageAsync(
new Message("Not sending telemetry as no current flight ID is set."));
return;
}
try
{
var lat = (float)flightData.CurrentPosition.Latitude;
var lon = (float)flightData.CurrentPosition.Longitude;
var alt = (ushort)flightData.CurrentPosition.Altitude;
var courseInRadians = flightData.CurrentPosition.Course / 180 * Math.PI;
var xVel = (float)(Math.Sin(courseInRadians) * flightData.CurrentPosition.Speed);
var yVel = (float)(Math.Cos(courseInRadians) * flightData.CurrentPosition.Speed);
var zVel = flightData.CurrentPosition.VerticalSpeed;
// Create dataStructure
var udpMessage = new UavPositionReport
{
GpsTimestamp = DateTime.UtcNow,
Pos = new Position(lat, lon, 0),
Alt = new Altitude(alt, AltitudeDatum.Agl, 1),
Velocity = new Velocity(xVel, yVel, zVel, 1),
IsAirborne = AirborneStatus.Airborne,
SatellitesVisible = 3
};
var telemetryId = Guid.Parse(_settings.CurrentTelemetryId);
var telemetry = new TelemetryEvent <UavPositionReport>(telemetryId, udpMessage)
{
SequenceNumber = _sequenceNumber
};
var message = string.Format($"Sending Telemetry {flightData.CurrentPosition.Latitude}, {flightData.CurrentPosition.Longitude}, {flightData.CurrentPosition.Altitude}");
await _messagesService.AddMessageAsync(new Message(message));
_client.SendTelemetry(telemetry, _settings.TelemetryHostName, _settings.TelemetryPortNumber, _settings.EncryptionKey);
_sequenceNumber += 1;
}
catch (Exception)
{
await _messagesService.AddMessageAsync(new Message("ERROR: Sending telemetry failed."));
}
}
public static bool TryReadReport(byte[] bytes, out UavPositionReport report)
{
report = null;
using (var stream = new MemoryStream(bytes))
{
using (var reader = new BinaryReader(stream))
{
if (!reader.TryReadStruct(out UavPosition position))
{
Debug.WriteLine("Could not read UavPosition", "Serialization Error");
return(false);
}
if (reader.BytesRemaining() < 1)
{
Debug.WriteLine("Missing UavPosition.NumStatuses", "Serialization Error");
return(false);
}
byte numStatuses = reader.ReadByte();
int expectedBytes = numStatuses * Marshal.SizeOf(typeof(AutpStatusMessage));
if (reader.BytesRemaining() < expectedBytes)
{
Debug.WriteLine($"Invalid status message bytes remaining. Expected {expectedBytes} bytes, got {reader.BytesRemaining()}", "Serialization Error");
return(false);
}
var statuses = new List <AutpStatusMessage>();
for (byte i = 0; i < numStatuses; i++)
{
statuses.Add(reader.ReadStruct <AutpStatusMessage>());
}
report = position.ToPositionReport();
report.UavStatus = statuses.Select(s => s.ToUavStatus()).ToList();
return(true);
}
}
}
