コード例 #1
0
ファイル: MSP.cs プロジェクト: mobilewares/UwpDrone
        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();
        }
コード例 #2
0
ファイル: MSP.cs プロジェクト: mobilewares/UwpDrone
        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;
            }
        }
コード例 #3
0
ファイル: MSP.cs プロジェクト: mobilewares/UwpDrone
        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;
                    }
                }
            });
        }