From 8432c5fb4a74e01c25b0128f7f99cef9e3bc147a Mon Sep 17 00:00:00 2001 From: ShingoMatsuura Date: Mon, 10 Oct 2016 17:14:39 +0900 Subject: [PATCH] AP_RangeFinder: remove number_detections member variable and add number_detections reference argument to parse_response --- libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp | 9 +++++---- libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 3 +-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 81bd786365..986e5b1335 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -69,8 +69,9 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm) // parse a response message, set number_detections, detections and sum_distance // must be signed to handle errors - if (parse_response() != LEDDARONE_OK) { - // TODO: when (number_detections < 0) handle LEDDARONE_ERR_ + uint8_t number_detections; + if (parse_response(number_detections) != LEDDARONE_OK) { + // TODO: when (not LEDDARONE_OK) handle LEDDARONE_ERR_ return false; } @@ -166,7 +167,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void) /* parse a response message from Modbus */ -LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(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(); @@ -204,7 +205,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(void) number_detections = data_buffer[10]; // if the number of detection is over or zero , it is false - if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections <= 0) { + if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) { return LEDDARONE_ERR_NUMBER_DETECTIONS; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 488d68a279..d08784c525 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -46,12 +46,11 @@ private: LeddarOne_Status send_request(void); // parse a response message from ModBus - LeddarOne_Status parse_response(void); + LeddarOne_Status parse_response(uint8_t &number_detections); AP_HAL::UARTDriver *uart = nullptr; uint32_t last_reading_ms; uint16_t detections[LEDDARONE_DETECTIONS_MAX]; uint32_t sum_distance; - uint8_t number_detections; };