From 4b127edd5e065d6a4a929b51902176c631df2d16 Mon Sep 17 00:00:00 2001 From: Shingo Matsuura Date: Fri, 4 Nov 2016 23:15:31 +0900 Subject: [PATCH] AP_RangeFinder: fixed LeddarOne busy wait --- .../AP_RangeFinder_LeddarOne.cpp | 151 +++++++++++------- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 15 +- libraries/AP_RangeFinder/RangeFinder.cpp | 4 - 3 files changed, 105 insertions(+), 65 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index dbcbfcb57a..bbb93eb464 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/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 - if (send_request() != LEDDARONE_OK) { - // TODO: handle LEDDARONE_ERR_SERIAL_PORT - return false; + switch (modbus_status) { + + case LEDDARONE_MODBUS_PRE_SEND_REQUEST: + // 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(); - 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; + return false; } /* @@ -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 i = 0; + uint8_t send_buffer[10] = {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; - data_buffer[1] = 0x04; - data_buffer[2] = 0; - data_buffer[3] = 20; - data_buffer[4] = 0; - data_buffer[5] = 10; + // send_buffer[3] = 20: Address of first register to read + // send_buffer[5] = 10: The number of consecutive registers to read + send_buffer[0] = LEDDARONE_DEFAULT_ADDRESS; + send_buffer[1] = 0x04; + send_buffer[2] = 0; + 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++) { - uart->write(data_buffer[i]); + for (index=0; index<8; index++) { + 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}; - uint32_t start_ms = AP_HAL::millis(); - uint32_t len = 0; - uint8_t i; - uint8_t index_offset = 11; + uint8_t index; + uint8_t index_offset = LEDDARONE_DATA_INDEX_OFFSET; // read serial - while (AP_HAL::millis() - start_ms < 10) { - uint32_t nbytes = uart->available(); - if (len == 25 && nbytes == 0) { - break; - } else { - for (i=len; i= 25) { - return LEDDARONE_ERR_BAD_RESPONSE; - } - data_buffer[i] = uart->read(); + uint32_t nbytes = uart->available(); + + if (nbytes != 0) { + for (index=read_len; index= 25) { + return LEDDARONE_ERR_BAD_RESPONSE; } - start_ms = AP_HAL::millis(); - len += nbytes; + read_buffer[index] = uart->read(); + } + + read_len += nbytes; + + if (read_len < 25) { + return LEDDARONE_READING_BUFFER; } } - if (len != 25) { - return LEDDARONE_ERR_BAD_RESPONSE; + if (read_len != 25 || read_buffer[1] != 0x04) { + 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(data_buffer[index_offset])*256 + data_buffer[index_offset+1]) / 10; - sum_distance += detections[i]; + detections[index] = (static_cast(read_buffer[index_offset])*256 + read_buffer[index_offset+1]) / 10; + sum_distance += detections[index]; index_offset += 4; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 80da913833..8003dbd4e3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -2,16 +2,17 @@ #include "RangeFinder.h" #include "RangeFinder_Backend.h" -#include #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; }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 32881851c9..341aa5f5b2 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/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)