Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: change magic numbers to define

This commit is contained in:
Shingo Matsuura 2016-11-05 10:19:32 +09:00 committed by Tom Pittenger
parent 6254787186
commit 4ca1eefd50
2 changed files with 27 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -167,18 +167,18 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
// clear buffer
while (nbytes-- > 0) {
uart->read();
if (++index > 250) {
if (++index > LEDDARONE_SERIAL_PORT_MAX) {
return LEDDARONE_ERR_SERIAL_PORT;
}
}
// Modbus read input register (function code 0x04)
send_buffer[0] = LEDDARONE_DEFAULT_ADDRESS;
send_buffer[1] = 0x04;
send_buffer[1] = LEDDARONE_MODOBUS_FUNCTION_CODE;
send_buffer[2] = 0;
send_buffer[3] = 20; // Address of first register to read
send_buffer[3] = LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS; // 20: Address of first register to read
send_buffer[4] = 0;
send_buffer[5] = 10; // The number of consecutive registers to read
send_buffer[5] = LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER; // 10: The number of consecutive registers to read
// CRC16
CRC16(send_buffer, 6, false);
@ -217,14 +217,14 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detections)
{
uint8_t index;
uint8_t index_offset = LEDDARONE_DATA_INDEX_OFFSET;
uint8_t index_offset = LEDDARONE_DETECTION_DATA_INDEX_OFFSET;
// read serial
uint32_t nbytes = uart->available();
if (nbytes != 0) {
for (index=read_len; index<nbytes+read_len; index++) {
if (index >= 25) {
if (index >= LEDDARONE_READ_BUFFER_SIZE) {
return LEDDARONE_ERR_BAD_RESPONSE;
}
read_buffer[index] = uart->read();
@ -232,12 +232,13 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
read_len += nbytes;
if (read_len < 25) {
if (read_len < LEDDARONE_READ_BUFFER_SIZE) {
return LEDDARONE_READING_BUFFER;
}
}
if (read_len != 25 || read_buffer[1] != 0x04) {
// lead_len is not 25 byte or function code is not 0x04
if (read_len != LEDDARONE_READ_BUFFER_SIZE || read_buffer[1] != LEDDARONE_MODOBUS_FUNCTION_CODE) {
return LEDDARONE_ERR_BAD_RESPONSE;
}
@ -246,8 +247,8 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
return LEDDARONE_ERR_BAD_CRC;
}
// number of detections
number_detections = read_buffer[10];
// number of detections (index:10)
number_detections = read_buffer[LEDDARONE_DETECTION_DATA_NUMBER_INDEX];
// if the number of detection is over or zero , it is false
if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) {
@ -260,7 +261,9 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
// construct data word from two bytes and convert mm to cm
detections[index] = (static_cast<uint16_t>(read_buffer[index_offset])*256 + read_buffer[index_offset+1]) / 10;
sum_distance += detections[index];
index_offset += 4;
// add index offset (4) to read next detection data
index_offset += LEDDARONE_DETECTION_DATA_OFFSET;
}
return LEDDARONE_OK;

18
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -3,11 +3,19 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#define LEDDARONE_DETECTIONS_MAX 3
// default slave address
// defines
#define LEDDARONE_DEFAULT_ADDRESS 0x01
#define LEDDARONE_DATA_INDEX_OFFSET 11
#define LEDDARONE_MODOBUS_FUNCTION_CODE 0x04
#define LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS 20
#define LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER 10
#define LEDDARONE_SERIAL_PORT_MAX 250
#define LEDDARONE_READ_BUFFER_SIZE 25
#define LEDDARONE_DETECTIONS_MAX 3
#define LEDDARONE_DETECTION_DATA_NUMBER_INDEX 10
#define LEDDARONE_DETECTION_DATA_INDEX_OFFSET 11
#define LEDDARONE_DETECTION_DATA_OFFSET 4
// LeddarOne status
enum LeddarOne_Status {
@ -64,6 +72,6 @@ private:
uint32_t sum_distance;
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_INIT;
uint8_t read_buffer[25];
uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
uint32_t read_len;
};