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

AP_RangeFinder: LeddarVu8 driver

This commit is contained in:
Randy Mackay 2020-01-13 14:48:57 +09:00 committed by Andrew Tridgell
parent e5c3a7f7b5
commit b068f15932
5 changed files with 301 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -42,6 +42,7 @@
#include "AP_RangeFinder_BLPing.h"
#include "AP_RangeFinder_UAVCAN.h"
#include "AP_RangeFinder_Lanbao.h"
#include "AP_RangeFinder_LeddarVu8.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
@ -496,6 +497,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++);
}
break;
case Type::LeddarVu8_Serial:
if (AP_RangeFinder_Lanbao::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++);
}
break;
default:
break;
}

1
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -78,6 +78,7 @@ public:
Lanbao = 26,
BenewakeTF03 = 27,
VL53L1X_Short = 28,
LeddarVu8_Serial = 29,
};
enum class Function {

204
libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp Normal file
View File

@ -0,0 +1,204 @@
/*
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_RangeFinder_LeddarVu8.h"
#include <AP_HAL/AP_HAL.h>
#include <ctype.h>
extern const AP_HAL::HAL& hal;
// LeddarVu8 uses the modbus RTU protocol
// https://autonomoustuff.com/product/leddartech-vu8/
#define LEDDARVU8_ADDR_DEFAULT 0x01 // modbus default device id
#define LEDDARVU8_DIST_MAX_CM 18500 // maximum possible distance reported by lidar
#define LEDDARVU8_OUT_OF_RANGE_ADD_CM 100 // add this many cm to out-of-range values
#define LEDDARVU8_TIMEOUT_MS 200 // timeout in milliseconds if no distance messages received
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_LeddarVu8::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
// check for timeout receiving messages
uint32_t now_ms = AP_HAL::millis();
if (((now_ms - last_distance_ms) > LEDDARVU8_TIMEOUT_MS) && ((now_ms - last_distance_request_ms) > LEDDARVU8_TIMEOUT_MS)) {
request_distances();
}
// variables for averaging results from multiple messages
float sum_cm = 0;
uint16_t count = 0;
uint16_t count_out_of_range = 0;
uint16_t latest_dist_cm = 0;
bool latest_dist_valid = false;
// read any available characters from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
int16_t r = uart->read();
if (r < 0) {
continue;
}
if (parse_byte((uint8_t)r, latest_dist_valid, latest_dist_cm)) {
if (latest_dist_valid) {
sum_cm += latest_dist_cm;
count++;
} else {
count_out_of_range++;
}
}
}
if (count > 0 || count_out_of_range > 0) {
// record time of successful read and request another reading
last_distance_ms = now_ms;
request_distances();
if (count > 0) {
// return average distance of readings
reading_cm = sum_cm / count;
} else {
// if only out of range readings return larger of
// driver defined maximum range for the model and user defined max range + 1m
reading_cm = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM);
}
return true;
}
// no readings so return false
return false;
}
// get sensor address from RNGFNDx_ADDR parameter
uint8_t AP_RangeFinder_LeddarVu8::get_sensor_address() const
{
if (params.address == 0) {
return LEDDARVU8_ADDR_DEFAULT;
}
return params.address;
}
// send request to device to provide distances
void AP_RangeFinder_LeddarVu8::request_distances()
{
uint8_t req_buf[] = {
get_sensor_address(), // address
(uint8_t)FunctionCode::READ_INPUT_REGISTER, // function code low
0, // function code high
(uint8_t)RegisterNumber::FIRST_DISTANCE0, // register number low
0, // register number high
8, // register count
0, // crc low
0 // crc high
};
const uint8_t req_buf_len = sizeof(req_buf);
// fill in crc bytes
uint16_t crc = calc_crc_modbus(req_buf, req_buf_len - 2);
req_buf[req_buf_len - 2] = LOWBYTE(crc);
req_buf[req_buf_len - 1] = HIGHBYTE(crc);
// send request to device
uart->write(req_buf, req_buf_len);
// record time of request
last_distance_request_ms = AP_HAL::millis();
}
// process one byte received on serial port
// returns true if successfully parsed a message
// if distances are valid, valid_readings is set to true and distance is stored in reading_cm
bool AP_RangeFinder_LeddarVu8::parse_byte(uint8_t b, bool &valid_reading, uint16_t &reading_cm)
{
// process byte depending upon current state
switch (parsed_msg.state) {
case ParseState::WAITING_FOR_ADDRESS: {
if (b == get_sensor_address()) {
parsed_msg.address = b;
parsed_msg.state = ParseState::WAITING_FOR_FUNCTION_CODE;
}
break;
}
case ParseState::WAITING_FOR_FUNCTION_CODE:
if (b == (uint8_t)FunctionCode::READ_INPUT_REGISTER) {
parsed_msg.function_code = b;
parsed_msg.state = ParseState::WAITING_FOR_PAYLOAD_LEN;
} else {
parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
}
break;
case ParseState::WAITING_FOR_PAYLOAD_LEN:
// only parse messages of the expected length
if (b == LEDDARVU8_PAYLOAD_LENGTH) {
parsed_msg.payload_len = b;
parsed_msg.payload_recv = 0;
parsed_msg.state = ParseState::WAITING_FOR_PAYLOAD;
} else {
parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
}
break;
case ParseState::WAITING_FOR_PAYLOAD:
if (parsed_msg.payload_recv < parsed_msg.payload_len) {
if (parsed_msg.payload_recv < ARRAY_SIZE(parsed_msg.payload)) {
parsed_msg.payload[parsed_msg.payload_recv] = b;
}
parsed_msg.payload_recv++;
}
if (parsed_msg.payload_recv == parsed_msg.payload_len) {
parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
}
break;
case ParseState::WAITING_FOR_CRC_LOW:
parsed_msg.crc = b;
parsed_msg.state = ParseState::WAITING_FOR_CRC_HIGH;
break;
case ParseState::WAITING_FOR_CRC_HIGH: {
parsed_msg.crc |= ((uint16_t)b << 8);
parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
// check crc
uint16_t expected_crc = calc_crc_modbus(&parsed_msg.address, 3+parsed_msg.payload_recv);
if (expected_crc == parsed_msg.crc) {
// calculate and return shortest distance
reading_cm = 0;
valid_reading = false;
for (uint8_t i=0; i<8; i++) {
uint8_t ix2 = i*2;
const uint16_t dist_cm = (uint16_t)parsed_msg.payload[ix2] << 8 | (uint16_t)parsed_msg.payload[ix2+1];
if ((dist_cm > 0) && (!valid_reading || (dist_cm < reading_cm))) {
reading_cm = dist_cm;
valid_reading = true;
}
}
return true;
}
break;
}
}
valid_reading = false;
return false;
}

89
libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h Normal file
View File

@ -0,0 +1,89 @@
#pragma once
#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend_Serial.h"
#define LEDDARVU8_PAYLOAD_LENGTH (8*2)
class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial
{
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
protected:
// baudrate used during object construction:
uint32_t initial_baudrate(uint8_t serial_instance) const override {
return 115200;
}
// return sensor type as laser
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_LASER;
}
// get a reading, distance returned in reading_cm
bool get_reading(uint16_t &reading_cm) override;
private:
// function codes
enum class FunctionCode : uint8_t {
READ_HOLDING_REGISTER = 0x03,
READ_INPUT_REGISTER = 0x04,
WRITE_HOLDING_REGISTER = 0x06,
WRITE_MULTIPLE_REGISTER = 0x10,
READ_WRITE_MULTIPLE_REGISTER = 0x17
};
// register numbers for reading input registers
enum class RegisterNumber : uint8_t {
REGISTER_STATUS = 1, // 0 = detections not ready, 1 = ready
NUMBER_OF_SEGMENTS = 2,
NUMBER_OF_DETECTIONS = 11,
PERCENTAGE_OF_LIGHT_SOURCE_POWER = 12,
TIMESTAMP_LOW = 14,
TIMESTAMP_HIGH = 15,
FIRST_DISTANCE0 = 16, // distance of first detection for 1st segment, zero if no detection
FIRST_AMPLITUDE0 = 24, // amplitude of first detection * 64 for 1st segment
FIRST_FLAG0 = 32, // flags of first detection for 1st segment. Bit0:Valid, Bit1:Result of object demerging, Bit2:Reserved, Bit3:Saturated
// registers exist for distance, amplitude and flags for subsequent detections but are not used in this driver
};
// parsing state
enum class ParseState : uint8_t {
WAITING_FOR_ADDRESS,
WAITING_FOR_FUNCTION_CODE,
WAITING_FOR_PAYLOAD_LEN,
WAITING_FOR_PAYLOAD,
WAITING_FOR_CRC_LOW,
WAITING_FOR_CRC_HIGH,
};
// get sensor address from RNGFNDx_ADDR parameter
uint8_t get_sensor_address() const;
// send request to device to provide distances
void request_distances();
// process one byte received on serial port
// returns true if successfully parsed a message
// if distances are valid, valid_readings is set to true and distance is stored in reading_cm
bool parse_byte(uint8_t b, bool &valid_reading, uint16_t &reading_cm);
// structure holding latest message contents
// the order of fields matches the incoming message so it can be used to calculate the crc
struct PACKED {
uint8_t address; // device address (required for calculating crc)
uint8_t function_code; // function code (always 0x04 but required for calculating crc)
uint8_t payload_len; // message payload length
uint8_t payload[LEDDARVU8_PAYLOAD_LENGTH]; // payload
uint16_t crc; // latest message's crc
uint16_t payload_recv; // number of message's payload bytes received so far
ParseState state; // state of incoming message processing
} parsed_msg;
uint32_t last_distance_ms; // system time of last successful distance sensor read
uint32_t last_distance_request_ms; // system time of last request to sensor to send distances
};

2
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini/Plus-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFmini/Plus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini/Plus-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFmini/Plus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial
// @User: Standard
AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0),