Port série C# BNO055 Communication par capteur

Question

J'essaie d'utiliser C# et mono pour parler à un capteur IMU (BNO055) sur un pi de framboise. Peu importe ce que je fais, je ne peux pas lire et écrire avec succès sur le capteur. Quelqu'un avec plus d'expérience peut-il me dire quelle est la bonne façon de le faire? Merci.

Initialisation:

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 Méthodes:

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

La question ici est que je tentais de lire une réponse du capteur immédiatement après l'écriture des données. Pour résoudre le problème, ajoutez une boucle en attente pour que la propriété BytesToRead soit égale à la longueur de réponse souhaitée, puis lue.