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