/// <summary>
/// This is our basic method for handling new asynchronous data packets
/// received events. When this method is called by VnSensor, the packet has
/// already been verified as valid and determined to be an asynchronous
/// data packet. Howerver, some processing is required on the user side to
/// make sure it is the expected type of asynchronous message so that it
/// can be parsed correctly.
/// </summary>
private static void AsyncPacketReceived(object sender, PacketFoundEventArgs packetFoundEventArgs)
{
var packet = packetFoundEventArgs.FoundPacket;
// Make sure we have an ASCII packet and not a binary packet.
if (packet.Type != PacketType.Ascii)
{
Console.WriteLine("Error - PacketType is not ASCII");
}
// Make sure we have a VNYPR data packet.
if (packet.AsciiAsyncType != AsciiAsync.VNYPR)
{
Console.WriteLine("Error- PacketAscii type is not VNYPR");
}
// We now need to parse out the yaw, pitch, roll data.
vec3f ypr;
packet.ParseVNYPR(out ypr);
// Now print out the yaw, pitch, roll measurements.
Console.WriteLine("ASCII Async YPR: {0}", ypr);
}
private void SensorOnAsyncPacketReceived(object sender, PacketFoundEventArgs packetFoundEventArgs)
{
var p = packetFoundEventArgs.FoundPacket;
var nd = new CompositeData();
lock (_data)
{
CompositeData.ParsePacket(p, new[] { nd, _data });
}
lock (this)
{
_nextData = nd;
}
_nextDataEvent.Set();
}
private static void AsciiOrBinaryAsyncPacketReceived(object sender, PacketFoundEventArgs packetFoundEventArgs)
{
var packet = packetFoundEventArgs.FoundPacket;
if (packet.Type == PacketType.Ascii && packet.AsciiAsyncType == AsciiAsync.VNYPR)
{
vec3f ypr;
packet.ParseVNYPR(out ypr);
Console.WriteLine("ASCII Async YPR: {0}", ypr);
}
else if (packet.Type == PacketType.Binary)
{
// First make sure we have a binary packet type we expect since there
// are many types of binary output types that can be configured.
if (!packet.IsCompatible(
CommonGroup.TimeStartup | CommonGroup.YawPitchRoll,
TimeGroup.None,
ImuGroup.None,
GpsGroup.None,
AttitudeGroup.None,
InsGroup.None))
{
// Not the type of binary packet we are expecting.
return;
}
// Ok, we have our expected binary output packet. Since there are many
// ways to configure the binary data output, the burden is on the user
// to correctly parse the binary packet. However, we can make use of
// the parsing convenience methods provided by the Packet structure.
// When using these convenience methods, you have to extract them in
// the order they are organized in the binary packet per the User Manual.
var timeStartup = packet.ExtractUint64();
var ypr = packet.ExtractVec3f();
Console.WriteLine("Binary Async TimeStartup: {0}", timeStartup);
Console.WriteLine("Binary Async YPR: {0}", ypr);
}
}
private static void ValidPacketFoundHandler(object sender, PacketFoundEventArgs args)
{
// When this method is called, the packet will already have been
// validated so no checksum/CRC check is required.
var packet = args.FoundPacket;
// First see if this is an ASCII or binary packet.
if (packet.Type == PacketType.Ascii)
{
// Now that we know this is an ASCII packet, we can call the
// various ASCII methods to further process this packet.
if (packet.IsAsciiAsync)
{
// We know we have an ASCII asynchronous data packet. Let's see
// if this is a message type we are looking for.
if (packet.AsciiAsyncType == AsciiAsync.VNYMR)
{
// Parse the VNYMR message.
vec3f ypr, mag, accel, angularRate;
packet.ParseVNYMR(out ypr, out mag, out accel, out angularRate);
Console.WriteLine("[Found VNYMR Packet]");
Console.WriteLine(" YawPitchRoll: {0}", ypr);
Console.WriteLine(" Magnetic: {0}", mag);
Console.WriteLine(" Acceleration: {0}", accel);
Console.WriteLine(" Angular Rate: {0}", angularRate);
}
}
else if (packet.IsResponse)
{
if (IsCheckingForModelNumberResponse)
{
var modelNumber = packet.ParseModelNumber();
Console.WriteLine("Model Number: {0}", modelNumber);
}
else if (IsCheckingForAsyncOutputFreqResponse)
{
var asyncFreq = packet.ParseAsyncDataOutputFrequency();
Console.WriteLine("Asynchronous Output Frequency: {0}", asyncFreq);
}
else if (IsCheckingForVpeBasicControlResponse)
{
packet.ParseVpeBasicControl(out enable, out headingMode, out filteringMode, out tuningMode);
Console.WriteLine("[VPE Basic Control]");
Console.WriteLine(" Enable: {0}", enable != 0);
Console.WriteLine(" Heading Mode: {0}", (HeadingMode) headingMode);
Console.WriteLine(" Filtering Mode: {0}", (FilterMode) filteringMode);
Console.WriteLine(" Tuning Mode: {0}", (FilterMode) tuningMode);
}
}
else if (packet.IsError)
{
Console.WriteLine("Sensor Error: {0}", packet.Error);
}
}
else if (packet.Type == PacketType.Binary)
{
// See if this is a binary packet type we are expecting.
if (!packet.IsCompatible(
CommonGroup.TimeStartup | CommonGroup.YawPitchRoll,
TimeGroup.None,
ImuGroup.None,
GpsGroup.None,
AttitudeGroup.None,
InsGroup.None))
{
// Not the type of binary packet we are expecting.
return;
}
// Ok, we have our expected binary output packet. Since there are
// many ways to configure the binary data output, the burden is on
// the user to correctly parse the binary packet. However, we can
// make use of the parsing convenience methods provided by the
// Packet class. When using these convenience methods, you have to
// extract them in the order they are organized in the binary
// packet per the User Manual.
var timeStartup = packet.ExtractUint64();
var ypr = packet.ExtractVec3f();
Console.WriteLine("[Binary Packet Received]");
Console.WriteLine(" TimeStartup: {0}", timeStartup);
Console.WriteLine(" Yaw Pitch Roll: {0}", ypr);
}
}
private void PfOnValidPacketFound(object sender, PacketFoundEventArgs packetFoundEventArgs)
{
WaitForCheckingOnPort.Set();
}
void HandleAsyncPacketReceived(object sender, PacketFoundEventArgs e)
{
var p = e.FoundPacket;
if (!p.IsAsciiAsync)
{
// Might be a binary packet but we are not handling this
// scenario in this example. Please refer to the net/examples/getting_started
// for an example of how to parse binary data.
return;
}
// This is the factory default output data type for VN-100 sensors.
if (p.AsciiAsyncType == AsciiAsync.VNYMR)
{
vec3f ypr, magnetic, acceleration, angularRate;
// Now that we have identified the type of packet we have, we
// can call the corresponding parse method.
p.ParseVNYMR(
out ypr,
out magnetic,
out acceleration,
out angularRate);
// Set the current orientation of the 3D cube. Note we must
// convert the yaw, pitch, roll from the sensor reference
// frame to the reference frame used by Unity.
transform.localEulerAngles = Conv.VnYprInDegsToUnityEulerAngles(ypr);
}
// This is the factory default output data type for VN-200 and
// VN-300 sensors.
else if (p.AsciiAsyncType == AsciiAsync.VNINS)
{
double time;
ushort week, status;
vec3f ypr, nedVel;
vec3d lla;
float attUncertainty, posUncertainty, velUncertainty;
// Now that we have identified the type of packet we have, we
// can call the corresponding parse method.
p.ParseVNINS(
out time,
out week,
out status,
out ypr,
out lla,
out nedVel,
out attUncertainty,
out posUncertainty,
out velUncertainty);
// Set the current orientation of the 3D cube. Note we must
// convert the yaw, pitch, roll from the sensor reference
// frame to the reference frame used by Unity.
transform.localEulerAngles = Conv.VnYprInDegsToUnityEulerAngles(ypr);
}
// If you have configured the sensor to output another message type,
// you can add more parsers here.
}
