AP_RangeFinder: fixed LeddarOne busy wait

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

@@ -48,36 +48,67 @@ bool AP_RangeFinder_LeddarOne::detect(RangeFinder &_ranger, uint8_t instance, AP
 // read - return last value measured by sensor
 bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
 {
+    uint8_t number_detections;
+    LeddarOne_Status leddarone_status;
+
     if (uart == nullptr) {
         return false;
     }
 
-    // send a request message for Modbus function 4
+    switch (modbus_status) {
-    if (send_request() != LEDDARONE_OK) {
+
-        // TODO: handle LEDDARONE_ERR_SERIAL_PORT
+        case LEDDARONE_MODBUS_PRE_SEND_REQUEST:
-        return false;
+            // clear buffer and buffer_len
+            memset(read_buffer, 0, sizeof(read_buffer));
+            read_len = 0;
+
+            // send a request message for Modbus function 4
+            if (send_request() != LEDDARONE_OK) {
+                // TODO: handle LEDDARONE_ERR_SERIAL_PORT
+                break;
+            }
+            modbus_status = LEDDARONE_MODBUS_SENT_REQUEST;
+            last_sending_request_ms = AP_HAL::millis();
+            break;
+
+        case LEDDARONE_MODBUS_SENT_REQUEST:
+            if (uart->available()) {
+                // change mod_bus status to read available buffer
+                modbus_status = LEDDARONE_MODBUS_AVAILABLE;
+            } else {
+                if (AP_HAL::millis() - last_sending_request_ms > 200) {
+                    // reset mod_bus status to read new buffer
+                    modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
+                }
+            }
+            break;
+
+        case LEDDARONE_MODBUS_AVAILABLE:
+            // parse a response message, set number_detections, detections and sum_distance
+            leddarone_status = parse_response(number_detections);
+
+            if (leddarone_status == LEDDARONE_OK) {
+                reading_cm = sum_distance / number_detections;
+
+                // reset mod_bus status to read new buffer
+                modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
+
+                return true;
+            }
+            // keep reading next buffer
+            else if (leddarone_status == LEDDARONE_READING_BUFFER) {
+                // not change mod_bus status, keep reading
+                break;
+            }
+            // leddarone_status is LEDDARONE_ERR_*
+            else {
+                // reset mod_bus status to read new buffer
+                modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
+            }
+            break;
     }
 
-    uint32_t start_ms = AP_HAL::millis();
+    return false;
-    while (!uart->available()) {
-        // wait up to 200ms
-        if (AP_HAL::millis() - start_ms > 200) {
-            return false;
-        }
-    }
-
-    // parse a response message, set number_detections, detections and sum_distance
-    // must be signed to handle errors
-    uint8_t number_detections;
-    if (parse_response(number_detections) != LEDDARONE_OK) {
-        // TODO: when (not LEDDARONE_OK) handle LEDDARONE_ERR_
-        return false;
-    }
-
-    // calculate average distance
-    reading_cm = sum_distance / number_detections;
-
-    return true;
 }
 
 /*
@@ -130,33 +161,35 @@ bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCh
  */
 LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
 {
-    uint8_t data_buffer[10] = {0};
+    uint8_t send_buffer[10] = {0};
-    uint8_t i = 0;
+    uint8_t index = 0;
 
     uint32_t nbytes = uart->available();
 
     // clear buffer
     while (nbytes-- > 0) {
         uart->read();
-        if (++i > 250) {
+        if (++index > 250) {
             return LEDDARONE_ERR_SERIAL_PORT;
         }
     }
 
     // Modbus read input register (function code 0x04)
-    data_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
+    // send_buffer[3] = 20: Address of first register to read
-    data_buffer[1] = 0x04;
+    // send_buffer[5] = 10: The number of consecutive registers to read
-    data_buffer[2] = 0;
+    send_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
-    data_buffer[3] = 20;
+    send_buffer[1] = 0x04;
-    data_buffer[4] = 0;
+    send_buffer[2] = 0;
-    data_buffer[5] = 10;
+    send_buffer[3] = 20;
+    send_buffer[4] = 0;
+    send_buffer[5] = 10;
 
     // CRC16
-    CRC16(data_buffer, 6, false);
+    CRC16(send_buffer, 6, false);
 
     // write buffer data with CRC16 bits
-    for (i=0; i<8; i++) {
+    for (index=0; index<8; index++) {
-        uart->write(data_buffer[i]);
+        uart->write(send_buffer[index]);
     }
     uart->flush();
 
@@ -168,40 +201,38 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
   */
 LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detections)
 {
-    uint8_t data_buffer[25] = {0};
+    uint8_t index;
-    uint32_t start_ms = AP_HAL::millis();
+    uint8_t index_offset = LEDDARONE_DATA_INDEX_OFFSET;
-    uint32_t len = 0;
-    uint8_t i;
-    uint8_t index_offset = 11;
 
     // read serial
-    while (AP_HAL::millis() - start_ms < 10) {
+    uint32_t nbytes = uart->available();
-        uint32_t nbytes = uart->available();
+
-        if (len == 25 && nbytes == 0) {
+    if (nbytes != 0)  {
-            break;
+        for (index=read_len; index<nbytes+read_len; index++) {
-        } else {
+            if (index >= 25) {
-            for (i=len; i<nbytes+len; i++) {
+                return LEDDARONE_ERR_BAD_RESPONSE;
-                if (i >= 25) {
-                    return LEDDARONE_ERR_BAD_RESPONSE;
-                }
-                data_buffer[i] = uart->read();
             }
-            start_ms = AP_HAL::millis();
+            read_buffer[index] = uart->read();
-            len += nbytes;
+        }
+
+        read_len += nbytes;
+
+        if (read_len < 25) {
+            return LEDDARONE_READING_BUFFER;
         }
     }
 
-    if (len != 25) {
+    if (read_len != 25 || read_buffer[1] != 0x04) {
-    	return LEDDARONE_ERR_BAD_RESPONSE;
+        return LEDDARONE_ERR_BAD_RESPONSE;
     }
 
     // CRC16
-    if (!CRC16(data_buffer, len-2, true)) {
+    if (!CRC16(read_buffer, read_len-2, true)) {
         return LEDDARONE_ERR_BAD_CRC;
     }
 
     // number of detections
-    number_detections = data_buffer[10];
+    number_detections = read_buffer[10];
 
     // if the number of detection is over or zero , it is false
     if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) {
@@ -210,10 +241,10 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
 
     memset(detections, 0, sizeof(detections));
     sum_distance = 0;
-    for (i=0; i<number_detections; i++) {
+    for (index=0; index<number_detections; index++) {
         // construct data word from two bytes and convert mm to cm
-        detections[i] =  (static_cast<uint16_t>(data_buffer[index_offset])*256 + data_buffer[index_offset+1]) / 10;
+        detections[index] =  (static_cast<uint16_t>(read_buffer[index_offset])*256 + read_buffer[index_offset+1]) / 10;
-        sum_distance += detections[i];
+        sum_distance += detections[index];
         index_offset += 4;
     }
 

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h

@@ -2,16 +2,17 @@
 
 #include "RangeFinder.h"
 #include "RangeFinder_Backend.h"
-#include <GCS_MAVLink/GCS.h>
 
 #define LEDDARONE_DETECTIONS_MAX 3
 
 // default slave address
 #define LEDDARONE_DEFAULT_ADDRESS 0x01
+#define LEDDARONE_DATA_INDEX_OFFSET 11
 
 // LeddarOne status
 enum LeddarOne_Status {
     LEDDARONE_OK = 0,
+	LEDDARONE_READING_BUFFER = 1,
     LEDDARONE_ERR_BAD_CRC = -1,
     LEDDARONE_ERR_NO_RESPONSES = -2,
     LEDDARONE_ERR_BAD_RESPONSE = -3,
@@ -20,6 +21,13 @@ enum LeddarOne_Status {
     LEDDARONE_ERR_NUMBER_DETECTIONS = -6
 };
 
+// LeddarOne Modbus status
+enum LeddarOne_ModbusStatus {
+	LEDDARONE_MODBUS_PRE_SEND_REQUEST = 0,
+	LEDDARONE_MODBUS_SENT_REQUEST,
+	LEDDARONE_MODBUS_AVAILABLE
+};
+
 class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
 {
 
@@ -49,7 +57,12 @@ private:
 
     AP_HAL::UARTDriver *uart = nullptr;
     uint32_t last_reading_ms;
+    uint32_t last_sending_request_ms;
 
     uint16_t detections[LEDDARONE_DETECTIONS_MAX];
     uint32_t sum_distance;
+
+    LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
+    uint8_t read_buffer[25];
+    uint32_t read_len;
 };

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -634,15 +634,11 @@ void RangeFinder::detect_instance(uint8_t instance)
         }
     }
     if (type == RangeFinder_TYPE_LEDDARONE) {
-#if 0
         if (AP_RangeFinder_LeddarOne::detect(*this, instance, serial_manager)) {
             state[instance].instance = instance;
             drivers[instance] = new AP_RangeFinder_LeddarOne(*this, instance, state[instance], serial_manager);
             return;
         }
-#else
-        hal.console->printf("LEDDARONE driver disabled\n");
-#endif
     }
 #if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO)