Untitled

#include <iostream>
#include <cstdint>
#include <libserial/SerialPort.h>

const std::string uart = "/dev/ttyS0"; 
const int baud_rate = 115200;

// Function to calculate the checksum of the packet
uint8_t calculate_checksum(uint8_t* packet) {
    uint8_t sum = 0;
    for(int i=0; i<8; i++) {
        sum += packet[i];
    }
    return (0xff - sum) & 0xff;
}

// Function to read a packet from the serial port
int read_packet(LibSerial::SerialPort& serial_port, uint16_t* distance) {
    uint8_t packet[9] = {0};
    while(true) {
        serial_port.ReadByte(packet[0]);
        if(packet[0] == 0x59) {
            break;
        }
    }

    for(int i=1; i<9; i++) {
        serial_port.ReadByte(packet[i]);
    }

    if(calculate_checksum(packet) != packet[8]) {
        return -1;
    }

    *distance = static_cast<uint16_t>(packet[1] | (packet[2] << 8));
    return 0;
}

int main() {
    try {
        LibSerial::SerialPort serial_port(uart);
        serial_port.Open();
        serial_port.SetBaudRate(baud_rate);
        serial_port.SetCharacterSize(LibSerial::SerialPort::CHAR_SIZE_8);

        while(true) {
            uint16_t distance = 0;
            if(read_packet(serial_port, &distance) == 0) {
                std::cout << "Distance: " << distance << " cm" << std::endl;
            }
        }
    }
    catch(const LibSerial::OpenFailed& e) {
        std::cerr << "Failed to open serial port" << std::endl;
        return 1;
    }

    return 0;
}