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