zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_RangeFinder: fixed LeddarOne busy wait

master
Shingo Matsuura 8 years ago committed by Tom Pittenger
parent
c843fedd77
commit
4b127edd5e
3 changed files with 103 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 121
      libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
  2. 15
      libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
  3. 4
      libraries/AP_RangeFinder/RangeFinder.cpp

121
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
Unescape View File

@ -48,36 +48,67 @@ bool AP_RangeFinder_LeddarOne::detect(RangeFinder &_ranger, uint8_t instance, AP @@ -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;
}
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
return false;
break;
}
modbus_status = LEDDARONE_MODBUS_SENT_REQUEST;
last_sending_request_ms = AP_HAL::millis();
break;
uint32_t start_ms = AP_HAL::millis();
while (!uart->available()) {
// wait up to 200ms
if (AP_HAL::millis() - start_ms > 200) {
return false;
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
// 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;
}
leddarone_status = parse_response(number_detections);
// calculate average distance
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;
}
return false;
}
/*
@ -130,33 +161,35 @@ bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCh @@ -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) @@ -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) {
if (nbytes != 0) {
for (index=read_len; index<nbytes+read_len; index++) {
if (index >= 25) {
return LEDDARONE_ERR_BAD_RESPONSE;
}
data_buffer[i] = uart->read();
read_buffer[index] = uart->read();
}
start_ms = AP_HAL::millis();
len += nbytes;
read_len += nbytes;
if (read_len < 25) {
return LEDDARONE_READING_BUFFER;
}
}
if (len != 25) {
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 @@ -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
Unescape View File

@ -2,16 +2,17 @@ @@ -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 { @@ -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: @@ -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
Unescape View File

@ -634,15 +634,11 @@ void RangeFinder::detect_instance(uint8_t instance) @@ -634,15 +634,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)

Write Preview
Loading…
Cancel
Save