public static bool TryParse(ref NavigationPacket packet, out NavigationData navigationData)
{
navigationData = new NavigationData();
NavdataBag navdataBag;
if (NavdataBagParser.TryParse(ref packet, out navdataBag))
{
navigationData = NavdataConverter.ToNavigationData(navdataBag);
return true;
}
return false;
}
public static bool TryParse(ref NavigationPacket packet, out NavigationData navigationData)
{
navigationData = new NavigationData();
NavdataBag navdataBag;
if (NavdataBagParser.TryParse(ref packet, out navdataBag))
{
navigationData = NavdataConverter.ToNavigationData(navdataBag);
return(true);
}
return(false);
}
public static NavigationData ToNavigationData(NavdataBag navdataBag)
{
var navigationData = new NavigationData();
var ardroneState = (def_ardrone_state_mask_t)navdataBag.ardrone_state;
UpdateStateUsing(ardroneState, ref navigationData.State);
var ctrlState = (CTRL_STATES)(navdataBag.demo.ctrl_state >> 0x10);
UpdateStateUsing(ctrlState, ref navigationData.State);
navigationData.Yaw = DegreeToRadian * (navdataBag.demo.psi / 1000.0f);
navigationData.Pitch = DegreeToRadian * (navdataBag.demo.theta / 1000.0f);
navigationData.Roll = DegreeToRadian * (navdataBag.demo.phi / 1000.0f);
navigationData.Altitude = navdataBag.demo.altitude / 1000.0f;
navigationData.Time = navdataBag.time.time;
navigationData.Velocity.X = navdataBag.demo.vx / 1000.0f;
navigationData.Velocity.Y = navdataBag.demo.vy / 1000.0f;
navigationData.Velocity.Z = navdataBag.demo.vz / 1000.0f;
navigationData.Battery.Low = ardroneState.HasFlag(def_ardrone_state_mask_t.ARDRONE_VBAT_LOW);
navigationData.Battery.Percentage = navdataBag.demo.vbat_flying_percentage;
navigationData.Battery.Voltage = navdataBag.raw_measures.vbat_raw / 1000.0f;
navigationData.Magneto.Rectified.X = navdataBag.magneto.magneto_rectified.x;
navigationData.Magneto.Rectified.Y = navdataBag.magneto.magneto_rectified.y;
navigationData.Magneto.Rectified.Z = navdataBag.magneto.magneto_rectified.z;
navigationData.Magneto.Offset.X = navdataBag.magneto.magneto_offset.x;
navigationData.Magneto.Offset.Y = navdataBag.magneto.magneto_offset.y;
navigationData.Magneto.Offset.Z = navdataBag.magneto.magneto_offset.z;
navigationData.Video.FrameNumber = navdataBag.video_stream.frame_number;
navigationData.Wifi.LinkQuality = 1.0f - ConversionHelper.ToSingle(navdataBag.wifi.link_quality);
return(navigationData);
}
public static NavigationData ToNavigationData(NavdataBag navdataBag)
{
var navigationData = new NavigationData();
var ardroneState = (def_ardrone_state_mask_t) navdataBag.ardrone_state;
UpdateStateUsing(ardroneState, ref navigationData.State);
var ctrlState = (CTRL_STATES) (navdataBag.demo.ctrl_state >> 0x10);
UpdateStateUsing(ctrlState, ref navigationData.State);
navigationData.Yaw = DegreeToRadian*(navdataBag.demo.psi/1000.0f);
navigationData.Pitch = DegreeToRadian*(navdataBag.demo.theta/1000.0f);
navigationData.Roll = DegreeToRadian*(navdataBag.demo.phi/1000.0f);
navigationData.Altitude = navdataBag.demo.altitude/1000.0f;
navigationData.Time = navdataBag.time.time;
navigationData.Velocity.X = navdataBag.demo.vx/1000.0f;
navigationData.Velocity.Y = navdataBag.demo.vy/1000.0f;
navigationData.Velocity.Z = navdataBag.demo.vz/1000.0f;
navigationData.Battery.Low = ardroneState.HasFlag(def_ardrone_state_mask_t.ARDRONE_VBAT_LOW);
navigationData.Battery.Percentage = navdataBag.demo.vbat_flying_percentage;
navigationData.Battery.Voltage = navdataBag.raw_measures.vbat_raw/1000.0f;
navigationData.Magneto.Rectified.X = navdataBag.magneto.magneto_rectified.x;
navigationData.Magneto.Rectified.Y = navdataBag.magneto.magneto_rectified.y;
navigationData.Magneto.Rectified.Z = navdataBag.magneto.magneto_rectified.z;
navigationData.Magneto.Offset.X = navdataBag.magneto.magneto_offset.x;
navigationData.Magneto.Offset.Y = navdataBag.magneto.magneto_offset.y;
navigationData.Magneto.Offset.Z = navdataBag.magneto.magneto_offset.z;
navigationData.Video.FrameNumber = navdataBag.video_stream.frame_number;
navigationData.Wifi.LinkQuality = 1.0f - ConversionHelper.ToSingle(navdataBag.wifi.link_quality);
return navigationData;
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
_navigationData = navigationData;
ProcessTransition();
if (NavigationDataUpdated != null)
NavigationDataUpdated(_navigationData);
}
}
