/// <summary>
/// Retreives a high priority message from the current state of the XDK device model.
/// </summary>
/// <param name="xdk">The XDK device model instance.</param>
public static byte[] ToHighPriorityMessage(this Xdk xdk)
{
using (var memoryStream = new MemoryStream())
{
using (var stream = new BinaryWriter(memoryStream))
{
if (xdk.UseBuiltInSensorFusion)
{
stream.Write(xdk.Orientation.W);
stream.Write(xdk.Orientation.X);
stream.Write(xdk.Orientation.Y);
stream.Write(xdk.Orientation.Z);
}
else
{
stream.Write(xdk.AccelerometerSensor.XAxis.Value);
stream.Write(xdk.AccelerometerSensor.YAxis.Value);
stream.Write(xdk.AccelerometerSensor.ZAxis.Value);
stream.Write(xdk.GyroSensor.XAxis.Value);
stream.Write(xdk.GyroSensor.YAxis.Value);
stream.Write(xdk.GyroSensor.ZAxis.Value);
}
memoryStream.Position = 0;
return(memoryStream.ToArray());
}
}
}
/// <summary>
/// Updates the low priority values of the given message in the given instance of the XDK.
/// </summary>
/// <param name="message">The message.</param>
/// <param name="xdk">The XDK.</param>
public static void SetLowPriorityValues(this byte[] message, Xdk xdk)
{
if (message == null || message.Length == 0)
{
return;
}
using (var memoryStream = new MemoryStream(message)
{
Position = 0
})
using (var stream = new BinaryReader(memoryStream))
{
byte messageId = stream.ReadByte();
switch (messageId)
{
case Message1Identifier:
SetLowPriorityMessage1Values(stream, xdk);
break;
case Message2Identifier:
SetLowPriorityMessage2Values(stream, xdk);
break;
}
}
}
/// <summary>
/// Sets the high priority sensor values which are supposed to be updated frequently.
/// </summary>
/// <param name="message">The message payload.</param>
/// <param name="xdk">The XDK device model instance.</param>
public static void SetHighPriorityValues(this byte[] message, Xdk xdk)
{
if (message == null || message.Length < 12)
{
return;
}
using (var memoryStream = new MemoryStream(message)
{
Position = 0
})
using (var stream = new BinaryReader(memoryStream))
{
if (xdk.UseBuiltInSensorFusion)
{
xdk.Orientation.W = stream.ReadSingle();
xdk.Orientation.X = stream.ReadSingle();
xdk.Orientation.Y = stream.ReadSingle();
xdk.Orientation.Z = stream.ReadSingle();
}
else
{
xdk.AccelerometerSensor.XAxis.Value = stream.ReadInt16();
xdk.AccelerometerSensor.YAxis.Value = stream.ReadInt16();
xdk.AccelerometerSensor.ZAxis.Value = stream.ReadInt16();
xdk.GyroSensor.XAxis.Value = stream.ReadInt16();
xdk.GyroSensor.YAxis.Value = stream.ReadInt16();
xdk.GyroSensor.ZAxis.Value = stream.ReadInt16();
xdk.CalculatePositionUsingSensorFusion();
}
}
}
private static void SetLowPriorityMessage1Values(BinaryReader stream, Xdk xdk)
{
xdk.LightSensor.Lux.Value = Convert.ToInt32(stream.ReadUInt32()) / 1000;
xdk.NoiseSensor.DbSpl.Value = stream.ReadByte();
xdk.Environment.Pressure.Value = Convert.ToInt32(stream.ReadUInt32());
xdk.Environment.Temperature.Value = stream.ReadInt32();
xdk.Environment.Humidity.Value = Convert.ToInt32(stream.ReadUInt32());
xdk.IsSdCardInserted.Value = stream.ReadByte();
byte readByte = stream.ReadByte();
var bitMask = (BitMask)readByte;
xdk.IsButton1Pressed = (bitMask & BitMask.Button1) == BitMask.Button1;
xdk.IsButton2Pressed = (bitMask & BitMask.Button2) == BitMask.Button2;
}
/// <summary>
/// Calculates the new orientation of the model based on the xdk sensor values.
/// </summary>
/// <param name="xdk"></param>
/// <returns></returns>
public Quaternion CalculatePosition(Xdk xdk)
{
// integrate gyro and compute new attitude
_gyroAttitude *= GetGyroDeltaRotation(new Vector3(xdk.GyroSensor.YAxis.Value, -1f * xdk.GyroSensor.XAxis.Value, xdk.GyroSensor.ZAxis.Value));
// build acc and mag vectors
var accelerometerVector = new Vector3(xdk.AccelerometerSensor.YAxis.Value, -1f * xdk.AccelerometerSensor.XAxis.Value, xdk.AccelerometerSensor.ZAxis.Value);
// get quaternion attitude from the vector (orinetation from vector)
var accelerometerAttitude = Quaternion.FromToRotation(new Vector3(0, 0, 1), accelerometerVector);
// complementary filter wit 0.98 as filter coefficient
const float filterCoefficient = 0.98f;
var attitude = Quaternion.Euler(ComplimentaryFilter(_gyroAttitude.eulerAngles, accelerometerAttitude.eulerAngles, new Vector3(filterCoefficient, filterCoefficient, filterCoefficient)));
return(attitude);
}
private static void SetLowPriorityMessage2Values(BinaryReader stream, Xdk xdk)
{
xdk.MagnetometerSensor.XAxis.Value = stream.ReadInt16();
xdk.MagnetometerSensor.YAxis.Value = stream.ReadInt16();
xdk.MagnetometerSensor.ZAxis.Value = stream.ReadInt16();
xdk.MagnetometerSensor.Resistance.Value = stream.ReadInt16();
byte readByte = stream.ReadByte();
for (int i = 0; i < Xdk.LedCount; i++)
{
xdk.Leds[i].IsOn = GetBitInByte(readByte, i);
}
if (xdk.UseBuiltInSensorFusion)
{
xdk.AccelerometerSensor.XAxis.Value = stream.ReadInt16();
xdk.AccelerometerSensor.YAxis.Value = stream.ReadInt16();
xdk.AccelerometerSensor.ZAxis.Value = stream.ReadInt16();
}
}
/// <summary>
/// Retreives a low priority message 2 from the current state of the XDK device model.
/// </summary>
/// <param name="xdk">The XDK device model instance.</param>
public static byte[] ToLowPriorityMessage2(this Xdk xdk)
{
using (var memoryStream = new MemoryStream())
{
using (var stream = new BinaryWriter(memoryStream))
{
stream.Write(Message2Identifier);
stream.Write(xdk.MagnetometerSensor.XAxis.Value);
stream.Write(xdk.MagnetometerSensor.YAxis.Value);
stream.Write(xdk.MagnetometerSensor.ZAxis.Value);
stream.Write(xdk.MagnetometerSensor.Resistance.Value);
byte ledByte = 0;
for (int i = 0; i < Xdk.LedCount; i++)
{
SetBitInByte(ref ledByte, i, xdk.Leds[i].IsOn);
}
stream.Write(ledByte);
memoryStream.Position = 0;
return(memoryStream.ToArray());
}
}
}
/// <summary>
/// Retreives a low priority message 1 from the current state of the XDK device model.
/// </summary>
/// <param name="xdk">The XDK device model instance.</param>
public static byte[] ToLowPriorityMessage1(this Xdk xdk)
{
using (var memoryStream = new MemoryStream())
{
using (var stream = new BinaryWriter(memoryStream))
{
stream.Write(Message1Identifier);
stream.Write(Convert.ToUInt32(xdk.LightSensor.Lux.Value * 1000));
stream.Write(xdk.NoiseSensor.DbSpl.Value);
stream.Write(Convert.ToUInt32(xdk.Environment.Pressure.Value));
stream.Write(xdk.Environment.Temperature.Value);
stream.Write(Convert.ToUInt32(xdk.Environment.Humidity.Value));
stream.Write(xdk.IsSdCardInserted.Value);
byte buttonByte = 0;
SetBitInByte(ref buttonByte, 0, xdk.IsButton1Pressed);
SetBitInByte(ref buttonByte, 1, xdk.IsButton2Pressed);
stream.Write(buttonByte);
memoryStream.Position = 0;
return(memoryStream.ToArray());
}
}
}
