public IMU440(string port, int baudrate, log loger, bool debug = false)
{
lg = loger;
nav = new Serial(port, baudrate);
this.debug = debug;
nav.PackageMode = Serial.PackageModes.UseFunction;
nav.EscapeToken = new byte[0];
nav.FindPackageEnd = (bytes =>
{
int offset = 0;
for (int i = 0; i < bytes.Length - 1; i++)
{
if (bytes[i] == 0x55 && bytes[i + 1] == 0x55)
{
offset = i;
}
}
if (bytes.Length >= 5 && bytes[offset] == 0x55 && bytes[offset + 1] == 0x55)
{
if (bytes.Length > offset + 4 &&
bytes.Length >= (offset + 7 + bytes[offset + 4]))
{
return(offset + 7 + bytes[offset + 4]);
}
}
return(-1);
});
nav.PackageReceived = navCallback;
nav.Start();
}