AP_RangeFilnder: add clear buffer at INIT state and change send_buffer to member variable to reduce CRC computation for sending request

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

@@ -32,6 +32,18 @@ AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder &_ranger, uint8_t
     uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
     if (uart != nullptr) {
         uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
+
+        // Modbus send request buffer
+        // read input register (function code 0x04)
+        send_request_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
+        send_request_buffer[1] = LEDDARONE_MODOBUS_FUNCTION_CODE;
+        send_request_buffer[2] = 0;
+        send_request_buffer[3] = LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS;   // 20: Address of first register to read
+        send_request_buffer[4] = 0;
+        send_request_buffer[5] = LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER;        // 10: The number of consecutive registers to read
+
+        // CRC16
+        CRC16(send_request_buffer, 6, false);
     }
 }
 
@@ -50,6 +62,8 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
 {
     uint8_t number_detections;
     LeddarOne_Status leddarone_status;
+    uint8_t index = 0;
+    uint32_t nbytes;
 
     if (uart == nullptr) {
         return false;
@@ -57,6 +71,15 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
 
     switch (modbus_status) {
     case LEDDARONE_MODBUS_STATE_INIT:
+        // clear read buffer
+        nbytes = uart->available();
+        while (nbytes-- > 0) {
+            uart->read();
+            if (++index > LEDDARONE_SERIAL_PORT_MAX) {
+                // LEDDARONE_STATE_ERR_SERIAL_PORT
+                return false;
+            }
+        }
         // clear buffer and buffer_len
         memset(read_buffer, 0, sizeof(read_buffer));
         read_len = 0;
@@ -67,13 +90,11 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
 
     case LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST:
         // send a request message for Modbus function 4
-        if (send_request() != LEDDARONE_STATE_OK) {
+        send_request();
-            // TODO: handle LEDDARONE_ERR_SERIAL_PORT
-            break;
-        }
         modbus_status = LEDDARONE_MODBUS_STATE_SENT_REQUEST;
         last_sending_request_ms = AP_HAL::millis();
-        break;
+
+        // FALL THROUGH
 
     case LEDDARONE_MODBUS_STATE_SENT_REQUEST:
         if (uart->available()) {
@@ -159,28 +180,10 @@ bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCh
 /*
    send a request message to execute ModBus function 0x04
  */
-LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
+void AP_RangeFinder_LeddarOne::send_request(void)
 {
-    uint8_t send_buffer[8] = {0};
+    // write send request buffer that is initialized in constructor
-
+    uart->write(send_request_buffer, 8);
-    // Modbus read input register (function code 0x04)
-    send_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
-    send_buffer[1] = LEDDARONE_MODOBUS_FUNCTION_CODE;
-    send_buffer[2] = 0;
-    send_buffer[3] = LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS;   // 20: Address of first register to read
-    send_buffer[4] = 0;
-    send_buffer[5] = LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER;        // 10: The number of consecutive registers to read
-
-    // CRC16
-    CRC16(send_buffer, 6, false);
-
-    // write buffer data with CRC16 bits
-    for (uint8_t index=0; index<8; index++) {
-        uart->write(send_buffer[index]);
-    }
-    uart->flush();
-
-    return LEDDARONE_STATE_OK;
 }
 
  /*

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h

@@ -59,7 +59,7 @@ private:
     bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck);
 
     // send a request message to execute ModBus function
-    LeddarOne_Status send_request(void);
+    void send_request(void);
 
     // parse a response message from ModBus
     LeddarOne_Status parse_response(uint8_t &number_detections);
@@ -74,4 +74,6 @@ private:
     LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT;
     uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
     uint32_t read_len;
+
+    uint8_t send_request_buffer[8] = {0};
 };