Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

RangeFinder: benewake buffer uses uint8_t

Browse Source
This commit is contained in:
Randy Mackay 2018-11-19 10:31:24 +09:00
parent 4b1857b3aa
commit f5a9fdaf42
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -85,7 +85,11 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
// read any available lines from the lidar // read any available lines from the lidar
int16_t nbytes = uart->available(); int16_t nbytes = uart->available();
while (nbytes-- > 0) { while (nbytes-- > 0) {
char c = uart->read(); int16_t r = uart->read();
if (r < 0) {
continue;
}
uint8_t c = (uint8_t)r;
// if buffer is empty and this byte is 0x59, add to buffer // if buffer is empty and this byte is 0x59, add to buffer
if (linebuf_len == 0) { if (linebuf_len == 0) {
if (c == BENEWAKE_FRAME_HEADER) { if (c == BENEWAKE_FRAME_HEADER) {
@ -107,10 +111,10 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
// calculate checksum // calculate checksum
uint8_t checksum = 0; uint8_t checksum = 0;
for (uint8_t i=0; i<BENEWAKE_FRAME_LENGTH-1; i++) { for (uint8_t i=0; i<BENEWAKE_FRAME_LENGTH-1; i++) {
checksum += (uint8_t)linebuf[i]; checksum += linebuf[i];
} }
// if checksum matches extract contents // if checksum matches extract contents
if (checksum == (uint8_t)linebuf[BENEWAKE_FRAME_LENGTH-1]) { if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
// calculate distance // calculate distance
uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2]; uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
if (dist >= BENEWAKE_DIST_MAX_CM) { if (dist >= BENEWAKE_DIST_MAX_CM) {

2
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -39,6 +39,6 @@ private:
AP_HAL::UARTDriver *uart = nullptr; AP_HAL::UARTDriver *uart = nullptr;
benewake_model_type model_type; benewake_model_type model_type;
char linebuf[10]; uint8_t linebuf[10];
uint8_t linebuf_len; uint8_t linebuf_len;
}; };