Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: fixed LeddarOne busy wait

Browse Source
This commit is contained in:
Shingo Matsuura 2016-11-04 23:15:31 +09:00 committed by Randy Mackay
parent df8cc895b3
commit 47d8e96f06
3 changed files with 105 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -48,36 +48,67 @@ bool AP_RangeFinder_LeddarOne::detect(RangeFinder &_ranger, uint8_t instance, AP
// read - return last value measured by sensor
bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
{
uint8_t number_detections;
LeddarOne_Status leddarone_status;
if (uart == nullptr) {
return false;
}
// send a request message for Modbus function 4
if (send_request() != LEDDARONE_OK) {
// TODO: handle LEDDARONE_ERR_SERIAL_PORT
return false;
switch (modbus_status) {
case LEDDARONE_MODBUS_PRE_SEND_REQUEST:
// clear buffer and buffer_len
memset(read_buffer, 0, sizeof(read_buffer));
read_len = 0;
// send a request message for Modbus function 4
if (send_request() != LEDDARONE_OK) {
// TODO: handle LEDDARONE_ERR_SERIAL_PORT
break;
}
modbus_status = LEDDARONE_MODBUS_SENT_REQUEST;
last_sending_request_ms = AP_HAL::millis();
break;
case LEDDARONE_MODBUS_SENT_REQUEST:
if (uart->available()) {
// change mod_bus status to read available buffer
modbus_status = LEDDARONE_MODBUS_AVAILABLE;
} else {
if (AP_HAL::millis() - last_sending_request_ms > 200) {
// reset mod_bus status to read new buffer
modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
}
}
break;
case LEDDARONE_MODBUS_AVAILABLE:
// parse a response message, set number_detections, detections and sum_distance
leddarone_status = parse_response(number_detections);
if (leddarone_status == LEDDARONE_OK) {
reading_cm = sum_distance / number_detections;
// reset mod_bus status to read new buffer
modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
return true;
}
// keep reading next buffer
else if (leddarone_status == LEDDARONE_READING_BUFFER) {
// not change mod_bus status, keep reading
break;
}
// leddarone_status is LEDDARONE_ERR_*
else {
// reset mod_bus status to read new buffer
modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
}
break;
}
uint32_t start_ms = AP_HAL::millis();
while (!uart->available()) {
// wait up to 200ms
if (AP_HAL::millis() - start_ms > 200) {
return false;
}
}
// parse a response message, set number_detections, detections and sum_distance
// must be signed to handle errors
uint8_t number_detections;
if (parse_response(number_detections) != LEDDARONE_OK) {
// TODO: when (not LEDDARONE_OK) handle LEDDARONE_ERR_
return false;
}
// calculate average distance
reading_cm = sum_distance / number_detections;
return true;
return false;
}
/*
@ -130,33 +161,35 @@ bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCh
*/
LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
{
uint8_t data_buffer[10] = {0};
uint8_t i = 0;
uint8_t send_buffer[10] = {0};
uint8_t index = 0;
uint32_t nbytes = uart->available();
// clear buffer
while (nbytes-- > 0) {
uart->read();
if (++i > 250) {
if (++index > 250) {
return LEDDARONE_ERR_SERIAL_PORT;
}
}
// Modbus read input register (function code 0x04)
data_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
data_buffer[1] = 0x04;
data_buffer[2] = 0;
data_buffer[3] = 20;
data_buffer[4] = 0;
data_buffer[5] = 10;
// send_buffer[3] = 20: Address of first register to read
// send_buffer[5] = 10: The number of consecutive registers to read
send_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
send_buffer[1] = 0x04;
send_buffer[2] = 0;
send_buffer[3] = 20;
send_buffer[4] = 0;
send_buffer[5] = 10;
// CRC16
CRC16(data_buffer, 6, false);
CRC16(send_buffer, 6, false);
// write buffer data with CRC16 bits
for (i=0; i<8; i++) {
uart->write(data_buffer[i]);
for (index=0; index<8; index++) {
uart->write(send_buffer[index]);
}
uart->flush();
@ -168,40 +201,38 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
*/
LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detections)
{
uint8_t data_buffer[25] = {0};
uint32_t start_ms = AP_HAL::millis();
uint32_t len = 0;
uint8_t i;
uint8_t index_offset = 11;
uint8_t index;
uint8_t index_offset = LEDDARONE_DATA_INDEX_OFFSET;
// read serial
while (AP_HAL::millis() - start_ms < 10) {
uint32_t nbytes = uart->available();
if (len == 25 && nbytes == 0) {
break;
} else {
for (i=len; i<nbytes+len; i++) {
if (i >= 25) {
return LEDDARONE_ERR_BAD_RESPONSE;
}
data_buffer[i] = uart->read();
uint32_t nbytes = uart->available();
if (nbytes != 0) {
for (index=read_len; index<nbytes+read_len; index++) {
if (index >= 25) {
return LEDDARONE_ERR_BAD_RESPONSE;
}
start_ms = AP_HAL::millis();
len += nbytes;
read_buffer[index] = uart->read();
}
read_len += nbytes;
if (read_len < 25) {
return LEDDARONE_READING_BUFFER;
}
}
if (len != 25) {
return LEDDARONE_ERR_BAD_RESPONSE;
if (read_len != 25 || read_buffer[1] != 0x04) {
return LEDDARONE_ERR_BAD_RESPONSE;
}
// CRC16
if (!CRC16(data_buffer, len-2, true)) {
if (!CRC16(read_buffer, read_len-2, true)) {
return LEDDARONE_ERR_BAD_CRC;
}
// number of detections
number_detections = data_buffer[10];
number_detections = read_buffer[10];
// if the number of detection is over or zero , it is false
if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) {
@ -210,10 +241,10 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
memset(detections, 0, sizeof(detections));
sum_distance = 0;
for (i=0; i<number_detections; i++) {
for (index=0; index<number_detections; index++) {
// construct data word from two bytes and convert mm to cm
detections[i] = (static_cast<uint16_t>(data_buffer[index_offset])*256 + data_buffer[index_offset+1]) / 10;
sum_distance += detections[i];
detections[index] = (static_cast<uint16_t>(read_buffer[index_offset])*256 + read_buffer[index_offset+1]) / 10;
sum_distance += detections[index];
index_offset += 4;
}

15
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -2,16 +2,17 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#include <GCS_MAVLink/GCS.h>
#define LEDDARONE_DETECTIONS_MAX 3
// default slave address
#define LEDDARONE_DEFAULT_ADDRESS 0x01
#define LEDDARONE_DATA_INDEX_OFFSET 11
// LeddarOne status
enum LeddarOne_Status {
LEDDARONE_OK = 0,
LEDDARONE_READING_BUFFER = 1,
LEDDARONE_ERR_BAD_CRC = -1,
LEDDARONE_ERR_NO_RESPONSES = -2,
LEDDARONE_ERR_BAD_RESPONSE = -3,
@ -20,6 +21,13 @@ enum LeddarOne_Status {
LEDDARONE_ERR_NUMBER_DETECTIONS = -6
};
// LeddarOne Modbus status
enum LeddarOne_ModbusStatus {
LEDDARONE_MODBUS_PRE_SEND_REQUEST = 0,
LEDDARONE_MODBUS_SENT_REQUEST,
LEDDARONE_MODBUS_AVAILABLE
};
class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
{
@ -49,7 +57,12 @@ private:
AP_HAL::UARTDriver *uart = nullptr;
uint32_t last_reading_ms;
uint32_t last_sending_request_ms;
uint16_t detections[LEDDARONE_DETECTIONS_MAX];
uint32_t sum_distance;
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
uint8_t read_buffer[25];
uint32_t read_len;
};

4
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -538,15 +538,11 @@ void RangeFinder::detect_instance(uint8_t instance)
}
}
if (type == RangeFinder_TYPE_LEDDARONE) {
#if 0
if (AP_RangeFinder_LeddarOne::detect(*this, instance, serial_manager)) {
state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_LeddarOne(*this, instance, state[instance], serial_manager);
return;
}
#else
hal.console->printf("LEDDARONE driver disabled\n");
#endif
}
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO)