internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 4)
{
throw new InvalidDataException();
}
_configSetter(config, reader.ReadUInt32());
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 2)
{
throw new InvalidDataException();
}
config.Frequency = reader.ReadUInt16();
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 6)
{
throw new InvalidDataException();
}
config.Sensor.DeviceId = reader.ReadUInt32();
config.Sensor.DeviceType = reader.ReadByte();
config.Sensor.BodyLocation = reader.ReadByte();
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 6)
{
throw new InvalidDataException();
}
config.Sensor.Version = reader.ReadUInt16();
config.Sensor.MajorVersion = reader.ReadUInt16();
config.Sensor.MinorVersion = reader.ReadUInt16();
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 3)
{
throw new InvalidDataException();
}
config.Button.Id = reader.ReadByte();
config.Button.PayloadLength = reader.ReadUInt16();
config.Button.Active = true;
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 5)
{
throw new InvalidDataException();
}
config.Barometer.Active = true;
config.Barometer.Id = reader.ReadByte();
config.Barometer.SamplingFrequency = reader.ReadUInt16();
config.Barometer.PayloadLength = reader.ReadUInt16();
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 5)
{
throw new InvalidDataException();
}
config.Radio.Id = reader.ReadByte();
config.Radio.PayloadLength = reader.ReadUInt16();
config.Radio.Mode = reader.ReadByte();
config.Radio.Channel = reader.ReadByte();
config.Radio.Active = true;
}
public GaitupConfig GetConfig()
{
var config = new GaitupConfig();
while (true)
{
var frame = GetConfigFrame(config);
if (frame == null)
{
break;
}
}
return(config);
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 7)
{
throw new InvalidDataException();
}
var second = reader.ReadByte();
var minute = reader.ReadByte();
var hour = reader.ReadByte();
var day = reader.ReadByte();
var month = reader.ReadByte();
var year = reader.ReadUInt16();
_configSetter(config, new DateTime(year, month, day, hour, minute, second));
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 30)
{
throw new InvalidDataException();
}
config.Gyro.Id = reader.ReadByte();
config.Gyro.SamplingFrequency = reader.ReadUInt16();
var scale = reader.ReadByte();
switch (scale)
{
case 0:
config.Gyro.Scale = 245;
break;
case 1:
config.Gyro.Scale = 500;
break;
case 2:
config.Gyro.Scale = 1000;
break;
case 3:
config.Gyro.Scale = 2000;
break;
default:
config.Gyro.Scale = 1000;
Debug.WriteLine($"Unknown gyro scale id {scale}, setting to {config.Gyro.Scale}");
break;
}
config.Gyro.OffsetX = reader.ReadInt32() / 10000.0;
config.Gyro.OffsetY = reader.ReadInt32() / 10000.0;
config.Gyro.OffsetZ = reader.ReadInt32() / 10000.0;
config.Gyro.GainX = reader.ReadInt32() / 10000.0;
config.Gyro.GainY = reader.ReadInt32() / 10000.0;
config.Gyro.GainZ = reader.ReadInt32() / 10000.0;
config.Gyro.PayloadLength = reader.ReadUInt16();
config.Gyro.Active = true;
}
public void ParseFile(GaitupConfig config = null)
{
if (config == null)
{
config = GetConfig();
}
config.Name = _name;
config.FileName = _fileName;
var fileLength = _stream.Length;
// Ensure we are at the start of the first sector
_reader.BaseStream.Seek(SectorLength, SeekOrigin.Begin);
_data = new GaitupData(config);
var lastPrint = -1;
try
{
while (true)
{
if (_stream.Length == _stream.Position)
{
// End of file
break;
}
ParseSector(config);
if (!PrintProgress)
{
continue;
}
var print = (int)(_stream.Position * 100 / fileLength);
if (print == lastPrint)
{
continue;
}
Debug.WriteLine($"Handling: {print}%");
lastPrint = print;
}
} catch (EndOfStreamException) { }
}
public static GaitupConfigFrame NextFrame(GaitupConfig config, BinaryReader reader)
{
if (!SyncFrame(reader))
{
return(null);
}
var classNum = reader.ReadByte();
var idNum = reader.ReadByte();
var size = reader.ReadByte();
GaitupConfigFrame frame = SelectFrame(classNum, idNum);
frame.Class = classNum;
frame.Id = idNum;
frame.Size = size;
frame.ParseFrame(config, reader);
frame.Valid = reader.ReadByte() == ChecksumByte1 && reader.ReadByte() == ChecksumByte2;
return(frame);
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
if (Size != 21)
{
throw new InvalidDataException();
}
config.Ble.Id = reader.ReadByte();
config.Ble.PayloadLength = reader.ReadUInt16();
config.Ble.SyncFirstPacket[0] = reader.ReadByte();
config.Ble.SyncFirstPacket[1] = reader.ReadByte();
config.Ble.SyncFirstPacket[2] = reader.ReadByte();
config.Ble.SyncFirstPacket[3] = reader.ReadByte();
config.Ble.SyncFirstPacket[4] = reader.ReadByte();
config.Ble.TimestampFirstPacket = reader.ReadUInt32();
config.Ble.SyncLastPacket[0] = reader.ReadByte();
config.Ble.SyncLastPacket[1] = reader.ReadByte();
config.Ble.SyncLastPacket[2] = reader.ReadByte();
config.Ble.SyncLastPacket[3] = reader.ReadByte();
config.Ble.SyncLastPacket[4] = reader.ReadByte();
config.Ble.TimestampLastPacket = reader.ReadUInt32();
config.Ble.Active = true;
}
internal override void ParseFrame(GaitupConfig config, BinaryReader reader)
{
RawData = reader.ReadBytes(Size);
}
private void ParseSector(GaitupConfig config)
{
if (_stream.Read(_buffer, 0, SectorLength) == 0)
{
throw new EndOfStreamException();
}
var sectorId = BigEndianBinaryReader.GetUInt32(_buffer, 0);
var sectorTimestamp = BigEndianBinaryReader.GetUInt32(_buffer, 4);
for (var i = 8; i < SectorLength; i += DataPayload)
{
var sensorId = _buffer[i];
var subTimestamp = _buffer[i + 1];
long timestamp = sectorTimestamp + subTimestamp;
if (config.Accelerometer.Id == sensorId)
{
var x = (BigEndianBinaryReader.GetInt16(_buffer, i + 2) * config.Accelerometer.Scale / 32768.0 - config.Accelerometer.OffsetX) / config.Accelerometer.GainX;
var y = (BigEndianBinaryReader.GetInt16(_buffer, i + 4) * config.Accelerometer.Scale / 32768.0 - config.Accelerometer.OffsetY) / config.Accelerometer.GainY;
var z = (BigEndianBinaryReader.GetInt16(_buffer, i + 6) * config.Accelerometer.Scale / 32768.0 - config.Accelerometer.OffsetZ) / config.Accelerometer.GainZ;
_data.AddAccel((timestamp, x, y, z));
}
else if (config.Gyro.Id == sensorId)
{
var x = (BigEndianBinaryReader.GetInt16(_buffer, i + 2) * config.Gyro.Scale / 32768.0 - config.Gyro.OffsetX) / config.Gyro.GainX;
var y = (BigEndianBinaryReader.GetInt16(_buffer, i + 4) * config.Gyro.Scale / 32768.0 - config.Gyro.OffsetY) / config.Gyro.GainY;
var z = (BigEndianBinaryReader.GetInt16(_buffer, i + 6) * config.Gyro.Scale / 32768.0 - config.Gyro.OffsetZ) / config.Gyro.GainZ;
_data.AddGyro((timestamp, x, y, z));
}
else if (config.Barometer.Id == sensorId)
{
var tmp = new byte[4];
for (var j = 0; j < 3; j++)
{
tmp[j] = _buffer[i + 2 + j];
}
var pressure = BitConverter.ToInt32(tmp, 0) / 4096.0;
var temperature = BitConverter.ToInt16(_buffer, i + 5) / 100.0;
_data.AddBaro((timestamp, pressure, temperature));
}
else if (config.Button.Id == sensorId)
{
var press = _buffer[i + 2];
if (press != 1)
{
Debug.WriteLine($"None-one button-press: {press}");
}
_data.AddButton(timestamp);
}
else if (config.Radio.Id == sensorId)
{
var timestamp2 = BigEndianBinaryReader.GetInt32(_buffer, i + 2);
var val = (double)timestamp2 / config.Frequency;
_data.AddRadio((timestamp, timestamp2, val));
}
else if (config.Ble.Id == sensorId)
{
throw new NotImplementedException("BLE sync not implemented yet");
// This looks really weird, as i+7 could be outside the buffer.
//var val = BitConverter.ToDouble(_buffer, i + 7);
//_data.AddBle((timestamp, val));
}
else
{
}
}
}
internal abstract void ParseFrame(GaitupConfig config, BinaryReader reader);
internal GaitupConfigFrame GetConfigFrame(GaitupConfig config)
{
return(GaitupConfigFrame.NextFrame(config, _reader));
}