diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp new file mode 100644 index 0000000000..d738cdc43c --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp @@ -0,0 +1,125 @@ +/* + 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 . + */ +/* + driver for Lanbao PSK-CM8JL65-CC5 Lidar + */ +#include +#include "AP_RangeFinder_Lanbao.h" +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +/* + this sensor has no way of reporting "out of range", it will keep + reporting distances at of around 7 to 8 meters even when pointed at + the sky. For this reason we limit the max range to 6 meters as + otherwise we may be giving false data + */ +#define LANBAO_MAX_RANGE_CM 600 + +/* + 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_Lanbao::AP_RangeFinder_Lanbao(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, + uint8_t serial_instance) : + AP_RangeFinder_Backend(_state, _params) +{ + const AP_SerialManager &serial_manager = AP::serialmanager(); + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); + if (uart != nullptr) { + // always 115200 + uart->begin(115200); + } +} + +/* + detect if a 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_Lanbao::detect(uint8_t serial_instance) +{ + return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; +} + + +// read - return last value measured by sensor +bool AP_RangeFinder_Lanbao::get_reading(uint16_t &reading_cm) +{ + if (uart == nullptr) { + return false; + } + + float sum_range = 0; + uint32_t count = 0; + + // format is: [ 0xA5 | 0x5A | distance-MSB-mm | distance-LSB-mm | crc16 ] + + // read any available lines from the lidar + int16_t nbytes = uart->available(); + while (nbytes-- > 0) { + int16_t b = uart->read(); + if (b == -1) { + break; + } + if (buf_len == 0 && b != 0xA5) { + // discard + continue; + } + if (buf_len == 1 && b != 0x5A) { + // discard + if (b == 0xA5) { + buf[0] = b; + } else { + buf_len = 0; + } + continue; + } + buf[buf_len++] = b; + if (buf_len == sizeof(buf)) { + buf_len = 0; + uint16_t crc = (buf[5]<<8) | buf[4]; + if (crc != calc_crc_modbus(buf, 4)) { + // bad CRC, discard + continue; + } + sum_range += float((buf[2]<<8) | buf[3]) * 0.001; + count++; + } + } + if (count > 0) { + reading_cm = (sum_range / count) * 100; + return reading_cm <= LANBAO_MAX_RANGE_CM?true:false; + } + return false; +} + +/* + update the state of the sensor +*/ +void AP_RangeFinder_Lanbao::update(void) +{ + if (get_reading(state.distance_cm)) { + // update range_valid state based on distance measured + state.last_reading_ms = AP_HAL::millis(); + update_status(); + } else if (AP_HAL::millis() - state.last_reading_ms > 200) { + set_status(RangeFinder::RangeFinder_NoData); + } +} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h new file mode 100644 index 0000000000..63f3ff3b6e --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h @@ -0,0 +1,34 @@ +#pragma once + +#include "RangeFinder.h" +#include "RangeFinder_Backend.h" + +class AP_RangeFinder_Lanbao : public AP_RangeFinder_Backend +{ + +public: + // constructor + AP_RangeFinder_Lanbao(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, + uint8_t serial_instance); + + // static detection function + static bool detect(uint8_t serial_instance); + + // update state + void update(void) override; + +protected: + + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_LASER; + } + +private: + // get a reading + bool get_reading(uint16_t &reading_cm); + + AP_HAL::UARTDriver *uart = nullptr; + uint8_t buf[6]; + uint8_t buf_len = 0; +}; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index feb5cd4074..5ce6a9f8e7 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -39,6 +39,7 @@ #include "AP_RangeFinder_PWM.h" #include "AP_RangeFinder_BLPing.h" #include "AP_RangeFinder_UAVCAN.h" +#include "AP_RangeFinder_Lanbao.h" #include #include @@ -512,6 +513,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) drivers[instance] = new AP_RangeFinder_BLPing(state[instance], params[instance], serial_instance++); } break; + case RangeFinder_TYPE_Lanbao: + if (AP_RangeFinder_Lanbao::detect(serial_instance)) { + drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++); + } + break; default: break; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 914f40ca73..eb0912dd5a 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -72,6 +72,7 @@ public: RangeFinder_TYPE_BLPing = 23, RangeFinder_TYPE_UAVCAN = 24, RangeFinder_TYPE_BenewakeTFminiPlus = 25, + RangeFinder_TYPE_Lanbao = 26, }; enum RangeFinder_Function {