diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 90e9f31832..6ef2751463 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -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 #include @@ -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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 548ecf7c32..675c7a00be 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -78,6 +78,7 @@ public: Lanbao = 26, BenewakeTF03 = 27, VL53L1X_Short = 28, + LeddarVu8_Serial = 29, }; enum class Function { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp new file mode 100644 index 0000000000..4ad4f7da0f --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -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 . + */ + +#include "AP_RangeFinder_LeddarVu8.h" + +#include +#include + +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; +} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h new file mode 100644 index 0000000000..e1300f4267 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h @@ -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 +}; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 1e98288ee9..06925a997b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -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),