AP_RangeFinder: Benewake TF02 and TFmini lidar driver #8504

rmackay9 · 2018-05-26T05:36:52Z

This PR add supports for the Benewake TF02 and TFmini lidars.

The same driver is used for both because the communication packets (sent over serial) are nearly identical. The only difference that we care about is that byte 6 provides a signal strength on the TF02 and a range-mode on the TFmini (either short or long).

I've tested the driver with both sensors and it seems to work.

Wiki pages for the two sensors are available below. Note that the TYPE parameter setting described needs to be updated on the wiki after this PR goes in:

rmackay9 · 2018-05-28T11:24:37Z

rebased to fix conflicts with the WASP200 lidar

OXINARF · 2018-05-28T13:28:31Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                        // no signal byte from TFmini
+                        sig_ok = true;
+                    } else {
+                        // TF02 provides signal strength (good = 7 or 8)


Both provide signal strength, only TF02 provides reliability (which is what the code is checking).

ok, comments fixed in follow up commit.

OXINARF · 2018-05-28T13:31:50Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                    // TFmini has short distance mode (mm)
+                    if ((model_type == BENEWAKE_TFmini)) {
+                        if (linebuf[6] == 0x02) {
+                            dist *= 10;


Should be divide by 10 right? In this case it's in mm and we want cm.

oops. thanks. distance is divided by 10 in follow-up commit.

OXINARF · 2018-05-28T13:32:59Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                // if checksum matches extract contents
+                if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
+                    // calculate distance and add to sum
+                    uint16_t dist = be16toh(((uint16_t)linebuf[2] << 8) | linebuf[3]);


Datasheet says it's in little endian, so I think this should be le16toh.

Also, we should check for 0xFFFF because, according that datasheet, that's an invalid distance.

ok, added check for 0xFFFF in follow up commit.

OXINARF · 2018-05-28T13:51:27Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                        sig_ok = true;
+                    } else {
+                        // TF02 provides signal strength (good = 7 or 8)
+                        // we interpret a single good strength as confidence in the average distance


I would propose something different:

set signal_ok to true after checking that checksum is valid (so line 110)

set sig_ok as is (probably rename to dist_reliable or something like that), but:

only add to sum_cm and increase count if sig_ok is true

reset sig_ok to false when linebuf_len is reset to 0

This means two things:

even if we don't receive a good distance, it won't be signaled as no data, but as out-of-range

it won't add values that aren't good, even if just one was

OXINARF · 2018-05-28T13:59:49Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                // calculate checksum
+                uint8_t checksum = 0;
+                for (uint8_t i=0; i<BENEWAKE_FRAME_LENGTH-1; i++) {
+                    checksum += linebuf[i];


This doesn't look correct. According to the datasheet, the checksum is the low 8-bit of the sum of the 8 first bytes. By making checksum just uint8_t we might be overflowing. It should be a uint16_t and then the comparison below should be if (uint8_t(checksum & 0xFF) == ...).

I don't think there's actually a difference is there? Either way the top bits are thrown away.. I've changed it in a follow-up commit but the checksum hasn't been failing..

OXINARF · 2018-05-29T03:10:15Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

-                    if ((model_type == BENEWAKE_TFmini)) {
-                        if (linebuf[6] == 0x02) {
-                            dist *= 10;
+                    uint16_t dist = le16toh(((uint16_t)linebuf[3] << 8) | linebuf[2]);


I did a terrible job explaining here, sorry. We have two options:

we either do what's between parenthesis: ((uint16_t)linebuf[3] << 8) | linebuf[2]);, OR

we call le16toh with a le16_t value: le16toh(*((le16_t*)&linebuf[2]))

Doing both operations isn't needed. The second option is more useful where packets are defined by a packed structure and a buffer is just copied into it, here the first option seems the better.

Ok, I don't know what to change. It works at the moment but if I flip around [3] and [2] it doesn't work...

It should be uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2]; . There is no point in calling the method because we are already doing the operation to make it correct.

@OXINARF, cool thanks. I've pushed fixes now. We should squash all these together before merging probably. If you give the LGTM I can do that..

Txs for the reivew, all issues addressed.

muramura

I think that it is an abnormal value when a value more than the performance is obtained. Because manufacturer does not guarantee.

muramura · 2018-05-29T16:11:43Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+// byte 5               STRENGTH_H      Strength high 8 bits
+// byte 6 (TF02)        SIG             Reliability in 8 levels, 7 & 8 means reliable
+// byte 6 (TFmini)      Distance Mode   0x02 for short distance (mm), 0x07 for long distance (cm)
+// byte 7 (TF02 only)   TIME            Exposure time in two levels 0x03 and 0x06


I think that it is better to know that it is 0 (without looking at the design sheet) for checksum check.
byte 7 (TFmini) 0x00

hmm.. this makes me think that we could actually auto detect if it's a TF02 or TFmini by looking at one of these fields..

After a benchtest, it seems like byte 7 is always zero on both the tfmini and the tf02.. so not useful for automatically determining which lidar is being used.

muramura · 2018-05-29T16:12:06Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+// byte 6 (TF02)        SIG             Reliability in 8 levels, 7 & 8 means reliable
+// byte 6 (TFmini)      Distance Mode   0x02 for short distance (mm), 0x07 for long distance (cm)
+// byte 7 (TF02 only)   TIME            Exposure time in two levels 0x03 and 0x06
+// byte 8               Check           Checksum parity bit, sum of byte 0 to byte 7


I think that parity bit is not written.
Design sheet:
Checksum is the low 8 bits of the cumulative sum of the numbers of the first 8 bytes.

I've updated the comments a bit to make it more clear I hope..

muramura · 2018-05-29T16:27:05Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+            // if buffer now has 9 items try to decode it
+            if (linebuf_len == BENEWAKE_FRAME_LENGTH) {
+                // calculate checksum
+                uint16_t checksum = 0;


On the design sheet the checksum is written as the lower 8 bits. I think that it is good to add 8 bits and 1 byte.

not quite sure what you mean. It seems to be working..

muramura · 2018-05-29T16:29:27Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                    checksum += linebuf[i];
+                }
+                // if checksum matches extract contents
+                if ((uint8_t)(checksum & 0xFF) == linebuf[BENEWAKE_FRAME_LENGTH-1]) {


If I do a checksum with 1 byte, I think that this expression will be simplified as well.

It was originally 1 byte, Francisco asked that we make it 2bytes to prevent overflow. It works either way so I think I will leave it as-is.

muramura · 2018-05-29T16:42:29Z

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

+                        // TFmini has short distance mode (mm)
+                        if ((model_type == BENEWAKE_TFmini)) {
+                            if (linebuf[6] == 0x02) {
+                                dist *= 0.1f;


In case of short range, 0 - 5 m
For long range, 1-12 m
I think that it is better to judge other values as abnormal values

I hope that it will return a bad value if it's out-of-range but I'm not sure yet. let's see how testing goes.

rmackay9 · 2018-05-30T02:16:32Z

Thanks for the reviews, merging!

rmackay9 added Enhancement Library labels May 26, 2018

rmackay9 requested a review from OXINARF May 26, 2018 05:38

rmackay9 force-pushed the benewake2 branch 2 times, most recently from 79c3a57 to 47541dc Compare May 28, 2018 11:24

OXINARF previously requested changes May 28, 2018

View reviewed changes

OXINARF reviewed May 29, 2018

View reviewed changes

muramura reviewed May 29, 2018

View reviewed changes

AP_RangeFinder: Benewake TF02 and TFmini lidar driver

160213e

rmackay9 force-pushed the benewake2 branch from e507729 to 160213e Compare May 29, 2018 23:49

rmackay9 merged commit e520d4f into ArduPilot:master May 30, 2018

rmackay9 deleted the benewake2 branch May 30, 2018 04:02

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_RangeFinder: Benewake TF02 and TFmini lidar driver #8504

AP_RangeFinder: Benewake TF02 and TFmini lidar driver #8504

rmackay9 commented May 26, 2018 •

edited

rmackay9 commented May 28, 2018

OXINARF May 28, 2018

rmackay9 May 29, 2018

OXINARF May 28, 2018

rmackay9 May 29, 2018

OXINARF May 28, 2018

rmackay9 May 29, 2018

OXINARF May 28, 2018

OXINARF May 28, 2018

rmackay9 May 29, 2018

OXINARF May 29, 2018

rmackay9 May 29, 2018

OXINARF May 29, 2018

rmackay9 May 29, 2018

muramura left a comment

muramura May 29, 2018

rmackay9 May 29, 2018

rmackay9 May 30, 2018

muramura May 29, 2018

rmackay9 May 29, 2018

muramura May 29, 2018

rmackay9 May 29, 2018

muramura May 29, 2018

rmackay9 May 29, 2018

muramura May 29, 2018

rmackay9 May 29, 2018

rmackay9 commented May 30, 2018

AP_RangeFinder: Benewake TF02 and TFmini lidar driver #8504

AP_RangeFinder: Benewake TF02 and TFmini lidar driver #8504

Conversation

rmackay9 commented May 26, 2018 • edited

rmackay9 commented May 28, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

muramura left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 commented May 30, 2018

rmackay9 commented May 26, 2018 •

edited