public void Close()
{
if (onClose != null)
{
try
{
onClose.Invoke(this);
}
catch (SystemException)
{
}
}
socket.Shutdown(SocketShutdown.Both);
}
void processMessage(MSP_Op code, byte[] bytes, byte length)
{
//Debug.WriteLine("message received: " + code.ToString());
switch (code)
{
case MSP_Op.Status:
{
ushort pidDeltaUs = BitConverter.ToUInt16(bytes, 0);
ushort i2cError = BitConverter.ToUInt16(bytes, 2);
ushort activeSensors = BitConverter.ToUInt16(bytes, 4);
ushort mode = BitConverter.ToUInt16(bytes, 6);
ushort profile = BitConverter.ToUInt16(bytes, 10);
ushort cpuload = BitConverter.ToUInt16(bytes, 11);
ushort gyroDeltaUs = BitConverter.ToUInt16(bytes, 13);
if ((mode & 0x1) == 1)
{
if (arm != kChannelArmValue)
{
Debug.WriteLine("Ack! integrity error");
}
}
}
break;
case MSP_Op.Identify:
{
Debug.WriteLine("Received Identity if you care");
}
break;
case MSP_Op.FlightControllerVariant:
{
StringBuilder builder = new StringBuilder();
ASCIIEncoding ai = new ASCIIEncoding();
for (int i = 0; i < 4; i++)
{
builder.Append(ai.GetString(bytes, i, 1));
}
Debug.WriteLine($"Flight Controller ID :{builder.ToString()}");
}
break;
case MSP_Op.RawIMU:
{
imu.accelerometer.X = BitConverter.ToInt16(bytes, 0) / 512.0f;
imu.accelerometer.Y = BitConverter.ToInt16(bytes, 2) / 512.0f;
imu.accelerometer.Z = BitConverter.ToInt16(bytes, 4) / 512.0f;
imu.gyroscope.X = BitConverter.ToInt16(bytes, 6) * 4.0f / 16.4f;
imu.gyroscope.Y = BitConverter.ToInt16(bytes, 8) * 4.0f / 16.4f;
imu.gyroscope.Z = BitConverter.ToInt16(bytes, 10) * 4.0f / 16.4f;
imu.magnetometer.X = BitConverter.ToInt16(bytes, 12) / 1090;
imu.magnetometer.Y = BitConverter.ToInt16(bytes, 14) / 1090;
imu.magnetometer.Z = BitConverter.ToInt16(bytes, 16) / 1090;
}
break;
case MSP_Op.RC:
{
// Should only happen at startup...
int activeChannels = length / 2;
if (activeChannels > 0)
{
roll = BitConverter.ToUInt16(bytes, 0);
}
if (activeChannels > 1)
{
pitch = BitConverter.ToUInt16(bytes, 2);
}
if (activeChannels > 2)
{
yaw = BitConverter.ToUInt16(bytes, 4);
}
if (activeChannels > 3)
{
throttle = BitConverter.ToUInt16(bytes, 6);
}
if (activeChannels > 4)
{
aux1 = BitConverter.ToUInt16(bytes, 8);
}
if (activeChannels > 5)
{
aux2 = BitConverter.ToUInt16(bytes, 10);
}
if (activeChannels > 6)
{
arm = BitConverter.ToUInt16(bytes, 12);
}
if (activeChannels > 7)
{
aux4 = BitConverter.ToUInt16(bytes, 14);
}
ChannelDelegate?.Invoke();
}
break;
}
}