From e9feb7bdda1f7c4226d26ec87ad420c4a5f5e362 Mon Sep 17 00:00:00 2001 From: ShingoMatsuura Date: Wed, 23 Nov 2016 06:40:37 +0900 Subject: [PATCH] AP_RangeFilnder: moved the const buffer definition to the header and changed from '8' to 'sizeof(send_request_buffer)' --- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp | 16 ++-------------- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 13 ++++++++++++- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 82af5fba39..b770955ba5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -27,19 +27,7 @@ extern const AP_HAL::HAL& hal; AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_SerialManager &serial_manager) : - AP_RangeFinder_Backend(_ranger, instance, _state), - // Modbus send request buffer - // read input register (function code 0x04) - send_request_buffer { - LEDDARONE_DEFAULT_ADDRESS, - LEDDARONE_MODOBUS_FUNCTION_CODE, - 0, - LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS, // 20: Address of first register to read - 0, - LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER, // 10: The number of consecutive registers to read - 0x30, // CRC Lo - 0x09 // CRC Hi - } + AP_RangeFinder_Backend(_ranger, instance, _state) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0); if (uart != nullptr) { @@ -90,7 +78,7 @@ 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 - uart->write(send_request_buffer, 8); + uart->write(send_request_buffer, sizeof(send_request_buffer)); modbus_status = LEDDARONE_MODBUS_STATE_SENT_REQUEST; last_sending_request_ms = AP_HAL::millis(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index c385bcd89b..0a3a048b4a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -73,5 +73,16 @@ private: uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE]; uint32_t read_len; - const uint8_t send_request_buffer[8]; + // Modbus send request buffer + // read input register (function code 0x04) + const uint8_t send_request_buffer[8] = { + LEDDARONE_DEFAULT_ADDRESS, + LEDDARONE_MODOBUS_FUNCTION_CODE, + 0, + LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS, // 20: Address of first register to read + 0, + LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER, // 10: The number of consecutive registers to read + 0x30, // CRC Lo + 0x09 // CRC Hi + }; };