Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: make LeddarOne calculations more efficient

This commit is contained in:
Peter Barker 2022-06-22 11:37:17 +10:00 committed by Peter Barker
parent 90362a4849
commit f4df4298a0
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -72,7 +72,7 @@ bool AP_RangeFinder_LeddarOne::get_reading(float &reading_m)
leddarone_status = parse_response(number_detections); leddarone_status = parse_response(number_detections);
if (leddarone_status == LEDDARONE_STATE_OK) { 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 // reset mod_bus status to read new buffer
modbus_status = LEDDARONE_MODBUS_STATE_INIT; 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; return LEDDARONE_STATE_ERR_NUMBER_DETECTIONS;
} }
sum_distance = 0; sum_distance_mm = 0;
for (uint8_t index=0; index<number_detections; index++) { for (uint8_t index=0; index<number_detections; index++) {
// construct data word from two bytes and convert mm to cm // construct data word from two bytes
sum_distance += (static_cast<uint16_t>(read_buffer[index_offset])*256 + read_buffer[index_offset+1]) / 10; sum_distance_mm += read_buffer[index_offset]<<8 | read_buffer[index_offset+1];
// add index offset (4) to read next detection data // add index offset (4) to read next detection data
index_offset += LEDDARONE_DETECTION_DATA_OFFSET; index_offset += LEDDARONE_DETECTION_DATA_OFFSET;

2
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -69,7 +69,7 @@ private:
uint32_t last_sending_request_ms; uint32_t last_sending_request_ms;
uint32_t last_available_ms; uint32_t last_available_ms;
uint32_t sum_distance; uint32_t sum_distance_mm;
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT; LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT;
uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE]; uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];