diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 3313e1f3aa..017e23ba78 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -17,7 +17,6 @@ #include #include "AP_RangeFinder_LeddarOne.h" #include -#include extern const AP_HAL::HAL& hal; @@ -56,6 +55,7 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm) // send a request message for Modbus function 4 if (send_request() < 0) { + // TODO: handle LEDDARONE_ERR_SERIAL_PORT return false; } @@ -103,10 +103,10 @@ void AP_RangeFinder_LeddarOne::update(void) bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck) { uint16_t crc = 0xFFFF; - uint32_t i = 0; - uint32_t j = 0; - uint8_t lCRCHi = 0; - uint8_t lCRCLo = 0; + uint32_t i; + uint32_t j; + uint8_t lCRCHi; + uint8_t lCRCLo; for (i=0; iavailable(); + uint32_t nbytes = uart->available(); // clear buffer while (nbytes-- > 0) { @@ -176,9 +176,9 @@ int8_t AP_RangeFinder_LeddarOne::parse_response(void) { uint8_t data_buffer[25] = {0}; uint32_t start_ms = AP_HAL::millis(); - uint32_t nbytes = 0; - uint32_t len =0; - uint8_t i = 0; + uint32_t nbytes; + uint32_t len = 0; + uint8_t i; uint8_t index_offset = 11; // read serial @@ -211,14 +211,14 @@ int8_t AP_RangeFinder_LeddarOne::parse_response(void) uint8_t number_detections = data_buffer[10]; // if the number of detection is over , it is false - if (number_detections > (sizeof(detections) / sizeof(detections[0]))) { + if (number_detections > LEDDARONE_DETECTIONS_MAX) { return LEDDARONE_ERR_NUMBER_DETECTIONS; } memset(detections, 0, sizeof(detections)); sum_distance = 0; for (i=0; i +#define LEDDARONE_DETECTIONS_MAX 3 + // default slave address #define LEDDARONE_DEFAULT_ADDRESS 0x01 @@ -44,8 +46,8 @@ private: int8_t parse_response(void); AP_HAL::UARTDriver *uart = nullptr; - uint32_t last_reading_ms = 0; + uint32_t last_reading_ms; - uint16_t detections[3]; - uint32_t sum_distance = 0; + uint16_t detections[LEDDARONE_DETECTIONS_MAX]; + uint32_t sum_distance; };