AP_RangeFinder: Benewake TF02 and TFmini lidar driver #8504
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,165 @@ | ||
| /* | ||
| This program is free software: you can redistribute it and/or modify | ||
| it under the terms of the GNU General Public License as published by | ||
| the Free Software Foundation, either version 3 of the License, or | ||
| (at your option) any later version. | ||
|
|
||
| This program is distributed in the hope that it will be useful, | ||
| but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| GNU General Public License for more details. | ||
|
|
||
| You should have received a copy of the GNU General Public License | ||
| along with this program. If not, see <http://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| #include <AP_HAL/AP_HAL.h> | ||
| #include "AP_RangeFinder_Benewake.h" | ||
| #include <AP_SerialManager/AP_SerialManager.h> | ||
| #include <ctype.h> | ||
| #include <AP_HAL/utility/sparse-endian.h> | ||
|
|
||
| extern const AP_HAL::HAL& hal; | ||
|
|
||
| #define BENEWAKE_FRAME_HEADER 0x59 | ||
| #define BENEWAKE_FRAME_LENGTH 9 | ||
|
|
||
| // format of serial packets received from benewake lidar | ||
| // | ||
| // Data Bit Definition Description | ||
| // ------------------------------------------------ | ||
| // byte 0 Frame header 0x59 | ||
| // byte 1 Frame header 0x59 | ||
| // byte 2 DIST_L Distance (in cm) low 8 bits | ||
| // byte 3 DIST_H Distance (in cm) high 8 bits | ||
| // byte 4 STRENGTH_L Strength low 8 bits | ||
| // 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 | ||
| // byte 8 Checksum Checksum byte, sum of bytes 0 to bytes 7 | ||
|
|
||
| /* | ||
| The constructor also initialises the rangefinder. Note that this | ||
| constructor is not called until detect() returns true, so we | ||
| already know that we should setup the rangefinder | ||
| */ | ||
| AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state, | ||
| AP_SerialManager &serial_manager, | ||
| uint8_t serial_instance, | ||
| benewake_model_type model) : | ||
| AP_RangeFinder_Backend(_state), | ||
| model_type(model) | ||
| { | ||
| uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); | ||
| if (uart != nullptr) { | ||
| uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)); | ||
| } | ||
| } | ||
|
|
||
| /* | ||
| detect if a Benewake rangefinder is connected. We'll detect by | ||
| trying to take a reading on Serial. If we get a result the sensor is | ||
| there. | ||
| */ | ||
| bool AP_RangeFinder_Benewake::detect(AP_SerialManager &serial_manager, uint8_t serial_instance) | ||
| { | ||
| return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; | ||
| } | ||
|
|
||
| // 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) | ||
| { | ||
| if (uart == nullptr) { | ||
| return false; | ||
| } | ||
|
|
||
| float sum_cm = 0; | ||
| uint16_t count = 0; | ||
| bool dist_reliable = false; | ||
|
|
||
| // read any available lines from the lidar | ||
| int16_t nbytes = uart->available(); | ||
| while (nbytes-- > 0) { | ||
| char c = uart->read(); | ||
| // if buffer is empty and this byte is 0x59, add to buffer | ||
| if (linebuf_len == 0) { | ||
| if (c == BENEWAKE_FRAME_HEADER) { | ||
| linebuf[linebuf_len++] = c; | ||
| } | ||
| } else if (linebuf_len == 1) { | ||
| // if buffer has 1 element and this byte is 0x59, add it to buffer | ||
| // if not clear the buffer | ||
| if (c == BENEWAKE_FRAME_HEADER) { | ||
| linebuf[linebuf_len++] = c; | ||
| } else { | ||
| linebuf_len = 0; | ||
| dist_reliable = false; | ||
| } | ||
| } else { | ||
| // add character to buffer | ||
| linebuf[linebuf_len++] = c; | ||
| // if buffer now has 9 items try to decode it | ||
| if (linebuf_len == BENEWAKE_FRAME_LENGTH) { | ||
| // calculate checksum | ||
| uint16_t checksum = 0; | ||
|
There was a problem hiding this comment. 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. There was a problem hiding this comment. not quite sure what you mean. It seems to be working.. |
||
| for (uint8_t i=0; i<BENEWAKE_FRAME_LENGTH-1; i++) { | ||
| checksum += linebuf[i]; | ||
| } | ||
| // if checksum matches extract contents | ||
| if ((uint8_t)(checksum & 0xFF) == linebuf[BENEWAKE_FRAME_LENGTH-1]) { | ||
|
There was a problem hiding this comment. If I do a checksum with 1 byte, I think that this expression will be simplified as well. There was a problem hiding this comment. 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. |
||
| // 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 != 0xFFFF) { | ||
| // TFmini has short distance mode (mm) | ||
| if ((model_type == BENEWAKE_TFmini)) { | ||
| if (linebuf[6] == 0x02) { | ||
| dist *= 0.1f; | ||
|
There was a problem hiding this comment. In case of short range, 0 - 5 m There was a problem hiding this comment. 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. |
||
| } | ||
| // 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) { | ||
| sum_cm += dist; | ||
| count++; | ||
| } | ||
| } | ||
| } | ||
| // clear buffer | ||
| linebuf_len = 0; | ||
| dist_reliable = false; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| if (count == 0) { | ||
| return false; | ||
| } | ||
| reading_cm = sum_cm / count; | ||
| return true; | ||
| } | ||
|
|
||
| /* | ||
| update the state of the sensor | ||
| */ | ||
| void AP_RangeFinder_Benewake::update(void) | ||
| { | ||
| bool signal_ok; | ||
| if (get_reading(state.distance_cm, signal_ok)) { | ||
| // 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); | ||
| } | ||
| } else if (AP_HAL::millis() - last_reading_ms > 200) { | ||
| set_status(RangeFinder::RangeFinder_NoData); | ||
| } | ||
| } | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,45 @@ | ||
| #pragma once | ||
|
|
||
| #include "RangeFinder.h" | ||
| #include "RangeFinder_Backend.h" | ||
|
|
||
| class AP_RangeFinder_Benewake : public AP_RangeFinder_Backend | ||
| { | ||
|
|
||
| public: | ||
|
|
||
| enum benewake_model_type { | ||
| BENEWAKE_TF02 = 0, | ||
| BENEWAKE_TFmini = 1 | ||
| }; | ||
|
|
||
| // constructor | ||
| AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state, | ||
| AP_SerialManager &serial_manager, | ||
| uint8_t serial_instance, | ||
| benewake_model_type model); | ||
|
|
||
| // static detection function | ||
| static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance); | ||
|
|
||
| // update state | ||
| void update(void); | ||
|
|
||
| protected: | ||
|
|
||
| virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { | ||
| return MAV_DISTANCE_SENSOR_LASER; | ||
| } | ||
|
|
||
| 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); | ||
|
|
||
| AP_HAL::UARTDriver *uart = nullptr; | ||
| benewake_model_type model_type; | ||
| uint32_t last_reading_ms; | ||
| char linebuf[10]; | ||
| uint8_t linebuf_len; | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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..
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.