From f4df4298a01bbcc4a85e7a9524607d3930233849 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 22 Jun 2022 11:37:17 +1000 Subject: [PATCH] AP_RangeFinder: make LeddarOne calculations more efficient --- libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp | 8 ++++---- libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 8c7122c559..e7e8b83cfd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -72,7 +72,7 @@ bool AP_RangeFinder_LeddarOne::get_reading(float &reading_m) leddarone_status = parse_response(number_detections); if (leddarone_status == LEDDARONE_STATE_OK) { - reading_m = (sum_distance * 0.01f) / number_detections; + reading_m = (sum_distance_mm * 0.001f) / number_detections; // reset mod_bus status to read new buffer modbus_status = LEDDARONE_MODBUS_STATE_INIT; @@ -171,10 +171,10 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect return LEDDARONE_STATE_ERR_NUMBER_DETECTIONS; } - sum_distance = 0; + sum_distance_mm = 0; for (uint8_t index=0; index(read_buffer[index_offset])*256 + read_buffer[index_offset+1]) / 10; + // construct data word from two bytes + sum_distance_mm += read_buffer[index_offset]<<8 | read_buffer[index_offset+1]; // add index offset (4) to read next detection data index_offset += LEDDARONE_DETECTION_DATA_OFFSET; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 13e6e2a27a..b8c41ddb73 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -69,7 +69,7 @@ private: uint32_t last_sending_request_ms; uint32_t last_available_ms; - uint32_t sum_distance; + uint32_t sum_distance_mm; LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT; uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];