/*
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"
#if AP_RANGEFINDER_LEDDARVU8_ENABLED
#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(float &reading_m)
{
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
for (auto i=0; i<8192; i++) {
uint8_t b;
if (!uart->read(b)) {
break;
}
if (parse_byte(b, 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_m = (sum_cm * 0.01f) / 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_m = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM)*0.01f;
}
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;
}
#endif // AP_RANGEFINDER_LEDDARVU8_ENABLED