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

AP_RangeFinder: benewake lidar returns max range + 1m when out-of-range 

also returns out-of-range when signal is weak
This commit is contained in:
Randy Mackay 2018-11-08 10:04:03 +09:00 committed by Andrew Tridgell
parent ce1b0fe533
commit 89f485f32e
2 changed files with 34 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -24,6 +24,7 @@ extern const AP_HAL::HAL& hal;
#define BENEWAKE_FRAME_HEADER 0x59
#define BENEWAKE_FRAME_LENGTH 9
#define BENEWAKE_DIST_MAX_CM 32768
#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
// format of serial packets received from benewake lidar
//
@ -69,7 +70,7 @@ bool AP_RangeFinder_Benewake::detect(AP_SerialManager &serial_manager, uint8_t s
}
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
@ -77,7 +78,7 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
float sum_cm = 0;
uint16_t count = 0;
bool dist_reliable = false;
uint16_t count_out_of_range = 0;
// read any available lines from the lidar
int16_t nbytes = uart->available();
@ -95,7 +96,6 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
linebuf[linebuf_len++] = c;
} else {
linebuf_len = 0;
dist_reliable = false;
}
} else {
// add character to buffer
@ -109,8 +109,6 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
}
// if checksum matches extract contents
if ((uint8_t)(checksum & 0xFF) == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
// tell caller we are receiving packets
signal_ok = true;
// calculate distance and add to sum
uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
if (dist < BENEWAKE_DIST_MAX_CM) {
@ -119,30 +117,45 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
if (linebuf[6] == 0x02) {
dist *= 0.1f;
}
// no signal byte from TFmini
dist_reliable = true;
} else {
// TF02 provides signal reliability (good = 7 or 8)
dist_reliable = (linebuf[6] >= 7);
}
if (dist_reliable) {
// no signal byte from TFmini so add distance to sum
sum_cm += dist;
count++;
} else {
// TF02 provides signal reliability (good = 7 or 8)
if (linebuf[6] >= 7) {
// add distance to sum
sum_cm += dist;
count++;
} else {
// this reading is out of range
count_out_of_range++;
}
}
} else {
// this reading is out of range
count_out_of_range++;
}
}
// clear buffer
linebuf_len = 0;
dist_reliable = false;
}
}
}
if (count == 0) {
return false;
if (count > 0) {
// return average distance of readings
reading_cm = sum_cm / count;
return true;
}
reading_cm = sum_cm / count;
return true;
if (count_out_of_range > 0) {
// if only out of range readings return max range + 1m
reading_cm = max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM;
return true;
}
// no readings so return false
return false;
}
/*
@ -150,16 +163,10 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
*/
void AP_RangeFinder_Benewake::update(void)
{
bool signal_ok;
if (get_reading(state.distance_cm, signal_ok)) {
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
last_reading_ms = AP_HAL::millis();
if (signal_ok) {
update_status();
} else {
// if signal is weak set status to out-of-range
set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
}
update_status();
} else if (AP_HAL::millis() - last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData);
}

4
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -34,8 +34,8 @@ protected:
private:
// get a reading
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool get_reading(uint16_t &reading_cm, bool &signal_ok);
// distance returned in reading_cm
bool get_reading(uint16_t &reading_cm);
AP_HAL::UARTDriver *uart = nullptr;
benewake_model_type model_type;