Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFilnder: add clear buffer at INIT state and change send_buffer to member variable to reduce CRC computation for sending request

Browse Source
This commit is contained in:
ShingoMatsuura 2016-11-21 20:45:34 +09:00 committed by Randy Mackay
parent dad1432e9e
commit 714ae5213d
2 changed files with 32 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -32,6 +32,18 @@ AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder &_ranger, uint8_t
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
// Modbus send request buffer
// read input register (function code 0x04)
send_request_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
send_request_buffer[1] = LEDDARONE_MODOBUS_FUNCTION_CODE;
send_request_buffer[2] = 0;
send_request_buffer[3] = LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS; // 20: Address of first register to read
send_request_buffer[4] = 0;
send_request_buffer[5] = LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER; // 10: The number of consecutive registers to read
// CRC16
CRC16(send_request_buffer, 6, false);
}
}
@ -50,6 +62,8 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
{
uint8_t number_detections;
LeddarOne_Status leddarone_status;
uint8_t index = 0;
uint32_t nbytes;
if (uart == nullptr) {
return false;
@ -57,6 +71,15 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
switch (modbus_status) {
case LEDDARONE_MODBUS_STATE_INIT:
// clear read buffer
nbytes = uart->available();
while (nbytes-- > 0) {
uart->read();
if (++index > LEDDARONE_SERIAL_PORT_MAX) {
// LEDDARONE_STATE_ERR_SERIAL_PORT
return false;
}
}
// clear buffer and buffer_len
memset(read_buffer, 0, sizeof(read_buffer));
read_len = 0;
@ -67,13 +90,11 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
case LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST:
// send a request message for Modbus function 4
if (send_request() != LEDDARONE_STATE_OK) {
// TODO: handle LEDDARONE_ERR_SERIAL_PORT
break;
}
send_request();
modbus_status = LEDDARONE_MODBUS_STATE_SENT_REQUEST;
last_sending_request_ms = AP_HAL::millis();
break;
// FALL THROUGH
case LEDDARONE_MODBUS_STATE_SENT_REQUEST:
if (uart->available()) {
@ -159,28 +180,10 @@ bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCh
/*
send a request message to execute ModBus function 0x04
*/
LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
void AP_RangeFinder_LeddarOne::send_request(void)
{
uint8_t send_buffer[8] = {0};
// Modbus read input register (function code 0x04)
send_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
send_buffer[1] = LEDDARONE_MODOBUS_FUNCTION_CODE;
send_buffer[2] = 0;
send_buffer[3] = LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS; // 20: Address of first register to read
send_buffer[4] = 0;
send_buffer[5] = LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER; // 10: The number of consecutive registers to read
// CRC16
CRC16(send_buffer, 6, false);
// write buffer data with CRC16 bits
for (uint8_t index=0; index<8; index++) {
uart->write(send_buffer[index]);
}
uart->flush();
return LEDDARONE_STATE_OK;
// write send request buffer that is initialized in constructor
uart->write(send_request_buffer, 8);
}
/*

4
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -59,7 +59,7 @@ private:
bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck);
// send a request message to execute ModBus function
LeddarOne_Status send_request(void);
void send_request(void);
// parse a response message from ModBus
LeddarOne_Status parse_response(uint8_t &number_detections);
@ -74,4 +74,6 @@ private:
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT;
uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
uint32_t read_len;
uint8_t send_request_buffer[8] = {0};
};