private async Task sendMessage(MSP_Op op, byte[] bytes = null)
{
byte opCode = (byte)op;
byte dataLength = 0;
if (bytes != null)
{
if (bytes.Length > 255)
{
Debug.WriteLine("Sending a message longer than an MSP message");
return;
}
else
{
dataLength = (byte)bytes.Length;
}
}
writer.WriteByte(36); // $
writer.WriteByte(77); // M
writer.WriteByte(60); // <
writer.WriteByte(dataLength);
writer.WriteByte(opCode);
byte checksum = (byte)(dataLength ^ opCode);
if (dataLength == 0)
{
writer.WriteByte(checksum);
}
else
{
foreach (var b in bytes)
{
writer.WriteByte(b);
checksum ^= b;
}
writer.WriteByte(checksum);
}
await writer.StoreAsync();
}
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.VoltMeter:
{
voltage = bytes[0];
}
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.Altitude:
{
altitude = BitConverter.ToUInt32(bytes, 0);
BitConverter.ToUInt16(bytes, 4); // read dead variability
}
break;
case MSP_Op.RC:
{
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)
{
althold = 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);
}
}
break;
}
}
private void startWatchingResponses()
{
Task t = Task.Run(async() =>
{
byte[] messagehistory = new byte[1024000];
ReadState[] statehistory = new ReadState[1024000];
uint messagehistoryindex = 0;
byte[] payload = new byte[MspPayloadSize];
ReadState readState = ReadState.Idle;
//MessageDirection direction = MessageDirection.Inbound;
byte checksum = 0;
byte specifiedChecksum = 0;
byte messageLengthExpectation = 0;
byte messageIndex = 0;
MSP_Op opcode = MSP_Op.None;
while (true)
{
var result = await reader.LoadAsync(1);
byte readByte = reader.ReadByte();
messagehistory[messagehistoryindex] = readByte;
statehistory[messagehistoryindex] = readState;
messagehistoryindex++;
switch (readState)
{
case ReadState.Idle:
if (readByte == Convert.ToByte('$'))
{
readState = ReadState.Preamble;
}
break;
case ReadState.Preamble:
if (readByte == Convert.ToByte('M'))
{
readState = ReadState.Direction;
}
else
{
readState = ReadState.Idle;
}
break;
case ReadState.Direction:
if (readByte == Convert.ToByte('>'))
{
//direction = MessageDirection.Inbound;
}
else if (readByte == Convert.ToByte('<'))
{
//direction = MessageDirection.Outbound;
}
else if (readByte == Convert.ToByte('!'))
{
Debug.WriteLine("Flight controller reports an unsupported command");
}
else
{
Debug.WriteLine("Unknown token reading Direction - " + readByte.ToString("x"));
}
// Advance anyway;
readState = ReadState.Length;
break;
case ReadState.Length:
messageLengthExpectation = readByte;
checksum = readByte;
readState = ReadState.OpCode;
break;
case ReadState.OpCode:
opcode = (MSP_Op)readByte;
checksum ^= readByte;
if (messageLengthExpectation > 0)
{
readState = ReadState.Payload;
}
else
{
readState = ReadState.ProcessPayload;
}
break;
case ReadState.Payload:
payload[messageIndex++] = readByte;
checksum ^= readByte;
if (messageIndex >= messageLengthExpectation)
{
readState = ReadState.ProcessPayload;
}
break;
case ReadState.ProcessPayload:
specifiedChecksum = readByte;
if (specifiedChecksum == checksum)
{
processMessage(opcode, payload, messageLengthExpectation);
}
else
{
Debug.WriteLine($"Processing Opcode {opcode} Checksum failed: Seen {checksum} but expected {specifiedChecksum}");
}
readState = ReadState.Idle;
opcode = MSP_Op.None;
messageIndex = 0;
break;
}
}
});
}
