Can't read packets from Cygbot LIDAR properly - arduino

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().