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

AP_RangeFinder: change enum name, add "state" word

This commit is contained in:
Shingo Matsuura 2016-11-06 19:02:14 +09:00 committed by Randy Mackay
parent 98ba554fdc
commit 6abe6448d6
2 changed files with 35 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -56,54 +56,54 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
}
switch (modbus_status) {
case LEDDARONE_MODBUS_INIT:
case LEDDARONE_MODBUS_STATE_INIT:
// clear buffer and buffer_len
memset(read_buffer, 0, sizeof(read_buffer));
read_len = 0;
modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST;
modbus_status = LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST;
// FALL THROUGH
// no break to fall through to next state LEDDARONE_MODBUS_PRE_SEND_REQUEST immediately
// no break to fall through to next state LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST immediately
case LEDDARONE_MODBUS_PRE_SEND_REQUEST:
case LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST:
// send a request message for Modbus function 4
if (send_request() != LEDDARONE_OK) {
if (send_request() != LEDDARONE_STATE_OK) {
// TODO: handle LEDDARONE_ERR_SERIAL_PORT
break;
}
modbus_status = LEDDARONE_MODBUS_SENT_REQUEST;
modbus_status = LEDDARONE_MODBUS_STATE_SENT_REQUEST;
last_sending_request_ms = AP_HAL::millis();
break;
case LEDDARONE_MODBUS_SENT_REQUEST:
case LEDDARONE_MODBUS_STATE_SENT_REQUEST:
if (uart->available()) {
// change mod_bus status to read available buffer
modbus_status = LEDDARONE_MODBUS_AVAILABLE;
modbus_status = LEDDARONE_MODBUS_STATE_AVAILABLE;
} else {
if (AP_HAL::millis() - last_sending_request_ms > 200) {
// reset mod_bus status to read new buffer
// if read_len is zero, send request without initialize
modbus_status = (read_len == 0) ? LEDDARONE_MODBUS_PRE_SEND_REQUEST : LEDDARONE_MODBUS_INIT;
modbus_status = (read_len == 0) ? LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST : LEDDARONE_MODBUS_STATE_INIT;
}
}
break;
case LEDDARONE_MODBUS_AVAILABLE:
case LEDDARONE_MODBUS_STATE_AVAILABLE:
// parse a response message, set number_detections, detections and sum_distance
leddarone_status = parse_response(number_detections);
if (leddarone_status == LEDDARONE_OK) {
if (leddarone_status == LEDDARONE_STATE_OK) {
reading_cm = sum_distance / number_detections;
// reset mod_bus status to read new buffer
modbus_status = LEDDARONE_MODBUS_INIT;
modbus_status = LEDDARONE_MODBUS_STATE_INIT;
return true;
}
// if status is not reading buffer, reset mod_bus status to read new buffer
else if (leddarone_status != LEDDARONE_READING_BUFFER) {
else if (leddarone_status != LEDDARONE_STATE_READING_BUFFER) {
// if read_len is zero, send request without initialize
modbus_status = (read_len == 0) ? LEDDARONE_MODBUS_PRE_SEND_REQUEST : LEDDARONE_MODBUS_INIT;
modbus_status = (read_len == 0) ? LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST : LEDDARONE_MODBUS_STATE_INIT;
}
break;
}
@ -170,7 +170,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
while (nbytes-- > 0) {
uart->read();
if (++index > LEDDARONE_SERIAL_PORT_MAX) {
return LEDDARONE_ERR_SERIAL_PORT;
return LEDDARONE_STATE_ERR_SERIAL_PORT;
}
}
@ -191,7 +191,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
}
uart->flush();
return LEDDARONE_OK;
return LEDDARONE_STATE_OK;
}
/*
@ -227,7 +227,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
if (nbytes != 0) {
for (index=read_len; index<nbytes+read_len; index++) {
if (index >= LEDDARONE_READ_BUFFER_SIZE) {
return LEDDARONE_ERR_BAD_RESPONSE;
return LEDDARONE_STATE_ERR_BAD_RESPONSE;
}
read_buffer[index] = uart->read();
}
@ -235,18 +235,18 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
read_len += nbytes;
if (read_len < LEDDARONE_READ_BUFFER_SIZE) {
return LEDDARONE_READING_BUFFER;
return LEDDARONE_STATE_READING_BUFFER;
}
}
// 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;
return LEDDARONE_STATE_ERR_BAD_RESPONSE;
}
// CRC16
if (!CRC16(read_buffer, read_len-2, true)) {
return LEDDARONE_ERR_BAD_CRC;
return LEDDARONE_STATE_ERR_BAD_CRC;
}
// number of detections (index:10)
@ -254,7 +254,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
// if the number of detection is over or zero , it is false
if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) {
return LEDDARONE_ERR_NUMBER_DETECTIONS;
return LEDDARONE_STATE_ERR_NUMBER_DETECTIONS;
}
memset(detections, 0, sizeof(detections));
@ -268,5 +268,5 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
index_offset += LEDDARONE_DETECTION_DATA_OFFSET;
}
return LEDDARONE_OK;
return LEDDARONE_STATE_OK;
}

26
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -19,22 +19,22 @@
// 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,
LEDDARONE_ERR_SHORT_RESPONSE = -4,
LEDDARONE_ERR_SERIAL_PORT = -5,
LEDDARONE_ERR_NUMBER_DETECTIONS = -6
LEDDARONE_STATE_OK = 0,
LEDDARONE_STATE_READING_BUFFER = 1,
LEDDARONE_STATE_ERR_BAD_CRC = -1,
LEDDARONE_STATE_ERR_NO_RESPONSES = -2,
LEDDARONE_STATE_ERR_BAD_RESPONSE = -3,
LEDDARONE_STATE_ERR_SHORT_RESPONSE = -4,
LEDDARONE_STATE_ERR_SERIAL_PORT = -5,
LEDDARONE_STATE_ERR_NUMBER_DETECTIONS = -6
};
// LeddarOne Modbus status
enum LeddarOne_ModbusStatus {
LEDDARONE_MODBUS_INIT = 0,
LEDDARONE_MODBUS_PRE_SEND_REQUEST,
LEDDARONE_MODBUS_SENT_REQUEST,
LEDDARONE_MODBUS_AVAILABLE
LEDDARONE_MODBUS_STATE_INIT = 0,
LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST,
LEDDARONE_MODBUS_STATE_SENT_REQUEST,
LEDDARONE_MODBUS_STATE_AVAILABLE
};
class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
@ -71,7 +71,7 @@ private:
uint16_t detections[LEDDARONE_DETECTIONS_MAX];
uint32_t sum_distance;
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_INIT;
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT;
uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
uint32_t read_len;
};