C# 串行端口 BNO055 传感器通信

Question

我正在尝试使用 C# 和单声道与树莓派上的 IMU 传感器 (BNO055( 通信。无论我做什么，我都无法成功地读取和写入传感器。有更多经验的人可以让我知道正确的方法是什么吗？谢谢。

初始化：

    private void ConnectToBNO055()
    {
        GPIO.Write(ResetPin, true);
        Task.Delay(650).Wait();
        Port                = new SerialPort();
        Port.PortName       = PortName;
        Port.BaudRate       = 115200;
        Port.Parity         = Parity.None;
        Port.DataBits       = 8;
        Port.StopBits       = StopBits.One;
        Port.Handshake      = Handshake.None;
        Port.ReadTimeout    = 5000;
        Port.WriteTimeout   = 5000;
        try
        {
            Port.Open();
            while (!Port.IsOpen) { Task.Delay(1).Wait(); }
            SetMode(OperationMode.OPERATION_MODE_CONFIG);
            WriteRegister((byte)PageIDRegisterDefinition.BNO055_PAGE_ID_ADDR, 0, false);
            byte chipID = ReadRegister((byte)PageRegisterDefinitionStart.BNO055_CHIP_ID_ADDR);
            if ((byte)I2CAddress.BNO055_ID == chipID)
            {
                Reset();
                WriteRegister((byte)ModeRegisters.BNO055_PWR_MODE_ADDR, (byte)PowerMode.POWER_MODE_NORMAL); 
                WriteRegister((byte)ModeRegisters.BNO055_SYS_TRIGGER_ADDR, 0x0); 
                SetMode(OperationMode.OPERATION_MODE_NDOF);
                Connected = true;
                Console.WriteLine("Connected to IMU.");
                Console.WriteLine("Calibrating IMU. . .");
                BNO055.CalibrationStatus lastCal;
                BNO055.CalibrationStatus cal = GetCalibrationStatus();
                Console.WriteLine("System: " + cal.System.ToString() + ", Magnometer: " + cal.Mag.ToString() + ", Gyroscope: " + cal.Gyro.ToString() + ", Accelerometer: " + cal.Accel.ToString());
                while (cal.System != 3 || cal.Mag != 3 || cal.Gyro != 3 || cal.Accel != 3)
                {
                    Task.Delay(250).Wait();
                    lastCal = cal;
                    cal = GetCalibrationStatus();
                    if (cal.Accel != lastCal.Accel || cal.Gyro != lastCal.Gyro || cal.Mag != lastCal.Mag || cal.System != lastCal.System)
                    {
                        Console.WriteLine("System: " + cal.System.ToString() + ", Magnometer: " + cal.Mag.ToString() + ", Gyroscope: " + cal.Gyro.ToString() + ", Accelerometer: " + cal.Accel.ToString());
                    }
                }
                Calibrated = true;
                Console.WriteLine("IMU Calibrated.");
            }
            else
            {
                Console.WriteLine("Unable to connect to IMU.");
            }
        }
        catch (Exception)
        {
            Console.WriteLine("Unable to connect to IMU.");
        }
    }

IO 方法：

    private void WriteRegister(byte address, byte data, bool ack = true)
    {
        byte[] writeBuffer = new byte[] { 0xAA, 0x00, address, 1, data };
        byte[] response;
        try
        {
            response = WriteData(writeBuffer, ack);
            if (ack)
            {
                if (0xEE01 != (response[0] << 8 | response[1]))
                    throw new Exception(string.Format("WriteRegister returned 0x{0:x2},0x{1:x2}", response[0], response[1]));
            }
        }
        catch (Exception ex)
        {
            Console.WriteLine(string.Format("WriteRegister error: {0}", ex.Message));
        }
    }
    private byte ReadRegister(byte register)
    {
        byte[] commandBuffer = new byte[] { 0xAA, 0x01, register, 1 };
        byte[] readBuffer = new byte[1];
        try
        {
            readBuffer = ReadData(commandBuffer, 1);
        }
        catch (Exception ex)
        {
            Console.WriteLine(string.Format("ReadRegister error: {0}", ex.Message));
        }
        return readBuffer[0];
    }
    private byte[] WriteData(byte[] command, bool ack = true, int retries = 5)
    {
        byte[] ackBuffer = new byte[2];
        bool done = false;
        while ((!done) && (retries-- > 0))
        {
            Port.Write(command, 0, command.Length);
            if (ack)
            {
                while (Port.BytesToRead != 2) { Task.Delay(1).Wait(); }
                Port.Read(ackBuffer, 0, 2);
                done = 0xEE07 != (ackBuffer[0] << 8 | ackBuffer[1]);
            }
            else
                done = true;
        }
        if (!done)
            throw new Exception("WriteData retries exceeded");
        return ackBuffer;
    }
    private byte[] ReadData(byte[] command, int readLength)
    {
        byte[] headerBuffer = new byte[2];
        byte[] returnBuffer = new byte[readLength];
        Port.Write(command, 0, command.Length);
        while (Port.BytesToRead != 2) { Task.Delay(1).Wait(); }
        Port.Read(headerBuffer, 0, 2);
        if (headerBuffer[0] != 0xBB)
            throw new Exception(string.Format("ReadData error 0x{0:x2}{1:x2}", headerBuffer[0], headerBuffer[1]));
        if (headerBuffer[1] != readLength)
            throw new Exception(string.Format("ReadData error: failed to read {0} bytes.  Read {1}", readLength, headerBuffer[1]));
        while (Port.BytesToRead != readLength) { Task.Delay(1).Wait(); }
        Port.Read(returnBuffer, 0, readLength);
        return returnBuffer;
    }

Answer 1

这里的问题是我试图在写入数据后立即读取传感器的响应。若要解决此问题，请添加一个循环，等待 BytesToRead 属性等于所需的响应长度，然后读取。