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

AP_RangeFinder: change enum name, add "state" word

Browse Source
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) { switch (modbus_status) {
case LEDDARONE_MODBUS_INIT: case LEDDARONE_MODBUS_STATE_INIT:
// clear buffer and buffer_len // clear buffer and buffer_len
memset(read_buffer, 0, sizeof(read_buffer)); memset(read_buffer, 0, sizeof(read_buffer));
read_len = 0; read_len = 0;
modbus_status = LEDDARONE_MODBUS_PRE_SEND_REQUEST; modbus_status = LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST;
// FALL THROUGH // 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 // 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 // TODO: handle LEDDARONE_ERR_SERIAL_PORT
break; break;
} }
modbus_status = LEDDARONE_MODBUS_SENT_REQUEST; modbus_status = LEDDARONE_MODBUS_STATE_SENT_REQUEST;
last_sending_request_ms = AP_HAL::millis(); last_sending_request_ms = AP_HAL::millis();
break; break;
case LEDDARONE_MODBUS_SENT_REQUEST: case LEDDARONE_MODBUS_STATE_SENT_REQUEST:
if (uart->available()) { if (uart->available()) {
// change mod_bus status to read available buffer // change mod_bus status to read available buffer
modbus_status = LEDDARONE_MODBUS_AVAILABLE; modbus_status = LEDDARONE_MODBUS_STATE_AVAILABLE;
} else { } else {
if (AP_HAL::millis() - last_sending_request_ms > 200) { if (AP_HAL::millis() - last_sending_request_ms > 200) {
// reset mod_bus status to read new buffer // reset mod_bus status to read new buffer
// if read_len is zero, send request without initialize // 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; break;
case LEDDARONE_MODBUS_AVAILABLE: case LEDDARONE_MODBUS_STATE_AVAILABLE:
// parse a response message, set number_detections, detections and sum_distance // parse a response message, set number_detections, detections and sum_distance
leddarone_status = parse_response(number_detections); leddarone_status = parse_response(number_detections);
if (leddarone_status == LEDDARONE_OK) { if (leddarone_status == LEDDARONE_STATE_OK) {
reading_cm = sum_distance / number_detections; reading_cm = sum_distance / number_detections;
// reset mod_bus status to read new buffer // reset mod_bus status to read new buffer
modbus_status = LEDDARONE_MODBUS_INIT; modbus_status = LEDDARONE_MODBUS_STATE_INIT;
return true; return true;
} }
// if status is not reading buffer, reset mod_bus status to read new buffer // 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 // 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; break;
} }
@ -170,7 +170,7 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::send_request(void)
while (nbytes-- > 0) { while (nbytes-- > 0) {
uart->read(); uart->read();
if (++index > LEDDARONE_SERIAL_PORT_MAX) { 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(); 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) { if (nbytes != 0) {
for (index=read_len; index<nbytes+read_len; index++) { for (index=read_len; index<nbytes+read_len; index++) {
if (index >= LEDDARONE_READ_BUFFER_SIZE) { if (index >= LEDDARONE_READ_BUFFER_SIZE) {
return LEDDARONE_ERR_BAD_RESPONSE; return LEDDARONE_STATE_ERR_BAD_RESPONSE;
} }
read_buffer[index] = uart->read(); read_buffer[index] = uart->read();
} }
@ -235,18 +235,18 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect
read_len += nbytes; read_len += nbytes;
if (read_len < LEDDARONE_READ_BUFFER_SIZE) { 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 // 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) { 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 // CRC16
if (!CRC16(read_buffer, read_len-2, true)) { if (!CRC16(read_buffer, read_len-2, true)) {
return LEDDARONE_ERR_BAD_CRC; return LEDDARONE_STATE_ERR_BAD_CRC;
} }
// number of detections (index:10) // 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 the number of detection is over or zero , it is false
if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) { 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)); 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; 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 // LeddarOne status
enum LeddarOne_Status { enum LeddarOne_Status {
LEDDARONE_OK = 0, LEDDARONE_STATE_OK = 0,
LEDDARONE_READING_BUFFER = 1, LEDDARONE_STATE_READING_BUFFER = 1,
LEDDARONE_ERR_BAD_CRC = -1, LEDDARONE_STATE_ERR_BAD_CRC = -1,
LEDDARONE_ERR_NO_RESPONSES = -2, LEDDARONE_STATE_ERR_NO_RESPONSES = -2,
LEDDARONE_ERR_BAD_RESPONSE = -3, LEDDARONE_STATE_ERR_BAD_RESPONSE = -3,
LEDDARONE_ERR_SHORT_RESPONSE = -4, LEDDARONE_STATE_ERR_SHORT_RESPONSE = -4,
LEDDARONE_ERR_SERIAL_PORT = -5, LEDDARONE_STATE_ERR_SERIAL_PORT = -5,
LEDDARONE_ERR_NUMBER_DETECTIONS = -6 LEDDARONE_STATE_ERR_NUMBER_DETECTIONS = -6
}; };
// LeddarOne Modbus status // LeddarOne Modbus status
enum LeddarOne_ModbusStatus { enum LeddarOne_ModbusStatus {
LEDDARONE_MODBUS_INIT = 0, LEDDARONE_MODBUS_STATE_INIT = 0,
LEDDARONE_MODBUS_PRE_SEND_REQUEST, LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST,
LEDDARONE_MODBUS_SENT_REQUEST, LEDDARONE_MODBUS_STATE_SENT_REQUEST,
LEDDARONE_MODBUS_AVAILABLE LEDDARONE_MODBUS_STATE_AVAILABLE
}; };
class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
@ -71,7 +71,7 @@ private:
uint16_t detections[LEDDARONE_DETECTIONS_MAX]; uint16_t detections[LEDDARONE_DETECTIONS_MAX];
uint32_t sum_distance; 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]; uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
uint32_t read_len; uint32_t read_len;
}; };