public ComboHprReading(double yaw, double pitch, double roll, double compassHeading, ComboHprFlags flags, DateTimeOffset timestamp)
{
this.Yaw = yaw > 360?yaw - 360:yaw;
this.Pitch = pitch > 360 ? pitch - 360 : pitch;
this.Roll = roll > 360 ? roll - 360 : roll;
this.Yaw = Yaw < 0 ? Yaw + 360 : Yaw;
this.Pitch = Pitch < 0 ? Pitch + 360 : Pitch;
this.Roll = Roll < 0 ? Roll + 360 : Roll;
this.Yaw = Yaw < 0 ? Yaw + 360 : Yaw;
this.Pitch = Pitch < 0 ? Pitch + 360 : Pitch;
this.Roll = Roll < 0 ? Roll + 360 : Roll;
if (Yaw < 0 || Pitch < 0 || Roll < 0)
{
throw new ArgumentException();
}
this.CompassHeading = compassHeading;
this.TimeStamp = timestamp;
this.Flags = flags;
}
public GyroChangedEventArgs(double yaw, double pitch, double roll, double compass, ComboHprFlags flags, DateTimeOffset timeOffset)
{
this.Flags = flags;
Pitch = pitch;
Roll = roll;
Yaw = yaw;
Compass = compass;
TimeOffset = timeOffset;
}
/// <summary>
/// ComboHPR value changed event handler.
/// </summary>
/// <param name="sender">The source of the event.</param>
/// <param name="args">A GattValueChangedEventArgs that contains the event data.</param>
private void ComboHprCharacteristicValueChanged(GattCharacteristic sender, GattValueChangedEventArgs args)
{
lock (syncRoot)
{
// The desired buffer length.
const uint desiredBufferLength = 14;
// Make sure we have a characteristic value.
if (args.CharacteristicValue == null || args.CharacteristicValue.Length < desiredBufferLength)
{
return;
}
// Note that we're grabbing more values than we're using right now in the code below because
// the code existed, and we'll use them eventually, so leaving it in place makes the most sense.
byte[] data = args.CharacteristicValue.ToArray();
if (data != null && data.Length >= desiredBufferLength)
{
// Obtain the raw data values.
ushort fusedHeadingRaw = BitConverter.ToUInt16(data, 0);
Int16 pitchRaw = BitConverter.ToInt16(data, 2);
Int16 rollRaw = BitConverter.ToInt16(data, 4);
byte paceRaw = data[6];
byte flagsRaw = data[7];
ComboHprFlags flags = (ComboHprFlags)Enum.Parse(typeof(ComboHprFlags), flagsRaw.ToString());
//Int16 flagsRaw = BitConverter.ToInt16(data, 7);
if ((flags & ComboHprFlags.MageneticDesturbance) != 0)
{
System.Diagnostics.Debug.WriteLine("ComboHPRFlags.MageneticDesturbance");
}
if ((flags & ComboHprFlags.AutoCalibrated) != 0)
{
System.Diagnostics.Debug.WriteLine("ComboHPRFlags.AutoCalibrated");
}
//bool magneticDisturbance = flagsRaw & 0x02;
ushort fieldStrengthRaw = BitConverter.ToUInt16(data, 8);
ushort yawRaw = BitConverter.ToUInt16(data, 10);
ushort cheadingRaw = BitConverter.ToUInt16(data, 12);
// Obtain the finished values.
double fusedHeading = (double)fusedHeadingRaw / 10.0;
double pitch = (double)pitchRaw / 10.0;
double roll = (double)rollRaw / 10.0;
double yaw = (double)yawRaw / 10.0;
double compassHeading = (double)cheadingRaw / 10.0;
// Raise the CompassChanged and GyroChanged event.
if (Attached && Connected)
{
ComboHprReading comboHprReading = new ComboHprReading(fusedHeading - yawOffset, pitch - pitchOffset, roll - rollOffset, compassHeading, flags, args.Timestamp);
currentRawComboHprReading = new ComboHprReading(fusedHeading, pitch, roll, compassHeading, flags, args.Timestamp);
currentReading.CurrentComboHprReading = comboHprReading;
//SensorGeocoordinate gps = new SensorGeocoordinate()
//OnGeocoordinateChanged(new GeocoordinateChangedEventArgs( )
OnCompassChanged(new CompassChangedEventArgs(compassHeading, args.Timestamp));
OnGyroChanged(new GyroChangedEventArgs(comboHprReading.Yaw, comboHprReading.Pitch, comboHprReading.Roll, comboHprReading.CompassHeading, flags, comboHprReading.TimeStamp));
//System.Diagnostics.Debug.WriteLine("Fushed Heading {0}", fusedHeading);
}
}
}
}
