Can't read packets from Cygbot LIDAR properly

Question

I'm trying to get an MCU to read data from a LIDAR via UART, but for some reason the checksum isn't correct. I'm using an ESP32-WROOM dev chip (using Arduino) and a Cygbot D1 LIDAR with this datasheet. I am able to read the 2D data packets, but not the 3D data packets, which are much longer.

After receiving a packet, I use a sequence of Serial.read() calls to read in data, byte by byte. The packet header, packet length bytes, and payload header are read correctly. The payload is 14,401 bytes long.

When I finish reading the data, the checksum doesn't match, and there are ~300 bytes left in the Serial buffer. After a lot of debugging I'm stuck and would appreciate any pointers or ideas for how to get un-stuck.

The relevant portions of my code:

bool readPacket() {
  /*
   * Call when we're expecting a packet. The top of the buffer
   * must be the correct packet header. 
   */
  if (Serial2.available()) {
    //Check the header, return false if it's wrong
    if (not checkHeader()) {
      return false;
    }
    
    //Next 2 bytes are the packet length
    byte b0 = Serial2.read();
    byte b1 = Serial2.read();
    unsigned int payloadLen = combineBytesIntoInt(b0, b1);

    //Read in the data based on the payload header. 
    byte payloadHeader = Serial2.read();
    
    byte sum = b0 ^ b1 ^ payloadHeader; //Start computing checksum
    if (payloadHeader == 0x08) {
      sum ^= parse3D();
    }

    byte packetChecksum = Serial2.read();
    
    if (packetChecksum != sum) {
      Serial.println("Checksum error");
      return false;
    }
    return true;
  } else {
    return false;
  }
}

byte parse3D() {
  /*
   * Parse a 3D dataset, reading directly from the serial. 
   * Reading in batches of 3 due to the way data is formatted in the packet.
   * Returns checksum. 
   */
  byte checksum = 0;
  for (int i=0; i<N_MEASURES_3D; i+=2) {
    int msb0 = -1; 
    //If data hasn't been sent, Serial2.read will return -1. Wait until we receive valid data before proceeding.
    while (msb0 < 0) {
      msb0 = Serial2.read();
    }
    int split = -1;
    while (split < 0) {
      split = Serial2.read();
    }
    int lsb1 = -1;
    while (lsb1 < 0) {
      lsb1 = Serial2.read();
    }
    
    checksum ^= byte(msb0) ^ byte(split) ^ byte(lsb1);
  }
  return checksum
}

void loop() {
  //When the button is clicked, send a packet. 
  if (buttonState == 1) {    
    ct += 1;
    if (ct % 2 == 0) {
      Serial.println("Write packet: start 3d");
      lidarStart3DMode();
    }
  }
  
  bool readSuccess = readPacket();
  if (readSuccess) {
    lidarStop();
  }

  //This code just updates the button  
  if ((digitalRead(BUTTON_PIN) == 1) and (millis() - lastTime > BUTTON_DEBOUNCE_TIME)) {
      lastTime = millis();
      buttonState = 1;
  } else {
    buttonState = 0;
  }
}

Answer 1

All the reads in your code here:

byte b0 = Serial2.read();
byte b1 = Serial2.read();
unsigned int payloadLen = combineBytesIntoInt(b0, b1);

//Read in the data based on the payload header. 
byte payloadHeader = Serial2.read();

and here

byte packetChecksum = Serial2.read();

and possibly in checkHeader() (which you didn't provide), should be safe-guarded against buffer under-run, ie., reading bytes when none are available.

You could add

while (!Serial.available());

before each read().