From 4df28ddc200b74646070b4bbfb370ffe4c44e8a7 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Tue, 3 Dec 2024 10:37:15 +0100 Subject: [PATCH] Fix startup problems, increase frequency, robust parser, use nonblocking reads --- .../lightware_sf45_serial.cpp | 526 ++++++++---------- .../lightware_sf45_serial.hpp | 32 +- .../lightware_sf45_serial/module.yaml | 2 +- 3 files changed, 232 insertions(+), 328 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index d5a4625a665c..b3ac5db49654 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -37,10 +37,11 @@ #include #include #include +#include #include -#include -#include + +using namespace time_literals; /* Configuration Constants */ #define SF45_SCALE_FACTOR 0.01f @@ -65,8 +66,6 @@ SF45LaserSerial::SF45LaserSerial(const char *port) : device_id.devid_s.bus = bus_num; } - _num_retries = 2; - // populate obstacle map members _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; @@ -78,7 +77,6 @@ SF45LaserSerial::SF45LaserSerial(const char *port) : for (uint32_t i = 0 ; i < BIN_COUNT; i++) { _obstacle_map_msg.distances[i] = UINT16_MAX; } - } SF45LaserSerial::~SF45LaserSerial() @@ -95,43 +93,36 @@ int SF45LaserSerial::init() param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); - _interval = 10000; start(); - return PX4_OK; } int SF45LaserSerial::measure() { - int rate = (int)_update_rate; - _data_output = 0x101; // raw distance + yaw readings + int32_t rate = (int32_t)_update_rate; + _data_output = 0x101; // raw distance (first return) + yaw readings _stream_data = 5; // enable constant streaming - // send some packets so the sensor starts scanning + // send packets to the sensor depending on the state switch (_sensor_state) { - // sensor should now respond case STATE_UNINIT: - while (_num_retries--) { - sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); - _sensor_state = STATE_UNINIT; - } - - _sensor_state = STATE_SEND_PRODUCT_NAME; + // Used to probe if the sensor is alive + sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); break; - case STATE_SEND_PRODUCT_NAME: + case STATE_ACK_PRODUCT_NAME: // Update rate default to 50 readings/s sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t)); - _sensor_state = STATE_SEND_UPDATE_RATE; break; - case STATE_SEND_UPDATE_RATE: + case STATE_ACK_UPDATE_RATE: + // Configure the data that the sensor shall output sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output)); - _sensor_state = STATE_SEND_DISTANCE_DATA; break; - case STATE_SEND_DISTANCE_DATA: + case STATE_ACK_DISTANCE_OUTPUT: + // Configure the sensor to automatically output data at the configured update rate sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data)); _sensor_state = STATE_SEND_STREAM; break; @@ -145,136 +136,90 @@ int SF45LaserSerial::measure() int SF45LaserSerial::collect() { - perf_begin(_sample_perf); - - int ret; float distance_m = -1.0f; - if (_sensor_state == STATE_SEND_PRODUCT_NAME) { + if (_sensor_state == STATE_UNINIT) { + perf_begin(_sample_perf); const int payload_length = 22; - ret = ::read(_fd, &_linebuf[0], payload_length); - if (ret < 0) { - PX4_ERR("ERROR (ack from sending product name cmd): %d", ret); - perf_count(_comms_errors); + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME); + + if (_crc_valid) { + _sensor_state = STATE_ACK_PRODUCT_NAME; perf_end(_sample_perf); - return ret; + return PX4_OK; } - sf45_request_handle(_linebuf); - ScheduleDelayed(_interval * 3); + return -EAGAIN; - } else if (_sensor_state == STATE_SEND_UPDATE_RATE) { + } else if (_sensor_state == STATE_ACK_PRODUCT_NAME) { + perf_begin(_sample_perf); const int payload_length = 7; - ret = ::read(_fd, &_linebuf[0], payload_length); - if (ret < 0) { - PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - if (ret == payload_length && _linebuf[3] == SF_UPDATE_RATE) { - sf45_request_handle(_linebuf); - ScheduleDelayed(_interval * 3); - } + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE); - } else if (_sensor_state == STATE_SEND_DISTANCE_DATA) { - - const int payload_length = 8; - ret = ::read(_fd, &_linebuf[0], payload_length); - - if (ret < 0) { - PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret); - perf_count(_comms_errors); + if (_crc_valid) { + _sensor_state = STATE_ACK_UPDATE_RATE; perf_end(_sample_perf); - return ret; + return PX4_OK; } - if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) { - sf45_request_handle(_linebuf); - ScheduleDelayed(_interval * 3); - } + return -EAGAIN; + } else if (_sensor_state == STATE_ACK_UPDATE_RATE) { - } else { - // Stream data from sensor + perf_begin(_sample_perf); const int payload_length = 10; - size_t max_read = sizeof(_linebuf) - _linebuf_size; - ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); - _linebuf_size += ret; + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT); - if (ret < 0) { - PX4_ERR("ERROR (ack from streaming distance data): %d", ret); - _linebuf_size = 0; - perf_count(_comms_errors); + if (_crc_valid) { + _sensor_state = STATE_ACK_DISTANCE_OUTPUT; perf_end(_sample_perf); - return ret; - } - - // Not enough data to parse a complete packet. Gather more data in the next cycle. - if (_linebuf_size < payload_length) { - return -EAGAIN; + return PX4_OK; } - int index = _linebuf_size - payload_length; - - while (index >= 0) { - if (_linebuf[index] == 0xAA) { - uint8_t flags_payload = (_linebuf[index + 1] >> 6) | (_linebuf[index + 2] << 2); + return -EAGAIN; - // Process the incoming distance data - if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { - sf45_request_handle(&_linebuf[index]); + } else { - if (_init_complete && _crc_valid) { - sf45_process_replies(&distance_m); - _linebuf_size = 0; - break; - } - } - } + // Stream data from sensor + perf_begin(_sample_perf); + const int payload_length = 10; - index--; - } + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM); - // The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again. - if (_linebuf_size >= sizeof(_linebuf)) { - _linebuf_size = 0; - perf_count(_comms_errors); + if (_crc_valid) { + sf45_process_replies(&distance_m); + PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); + perf_end(_sample_perf); + return PX4_OK; } - } - if (_consecutive_fail_count > 35 && !_sensor_ready) { - PX4_ERR("Restarting the state machine"); - return PX4_ERROR; - } - - _last_read = hrt_absolute_time(); - - if (!_crc_valid) { return -EAGAIN; } - - PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); - - perf_end(_sample_perf); - - return PX4_OK; } void SF45LaserSerial::start() { - /* reset the report ring and state machine */ + /* reset the sensor state */ + _sensor_state = STATE_UNINIT; + + /* reset the report ring */ _collect_phase = false; /* reset the UART receive buffer size */ _linebuf_size = 0; + /* reset the fail counter */ + _last_received_time = hrt_absolute_time(); + /* schedule a cycle to start things */ ScheduleNow(); } @@ -289,8 +234,7 @@ void SF45LaserSerial::Run() /* fds initialized? */ if (_fd < 0) { /* open fd: non-blocking read mode*/ - - _fd = ::open(_port, O_RDWR | O_NOCTTY); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); if (_fd < 0) { PX4_ERR("serial open failed (%i)", errno); @@ -304,34 +248,11 @@ void SF45LaserSerial::Run() /* fill the struct for the new configuration */ tcgetattr(_fd, &uart_config); - uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8; - - uart_config.c_cflag |= (CLOCAL | CREAD); - - // no parity, 1 stop bit, flow control disabled - uart_config.c_cflag &= ~(PARENB | PARODD); - - uart_config.c_cflag |= 0; - - uart_config.c_cflag &= ~CSTOPB; - - uart_config.c_cflag &= ~CRTSCTS; - - uart_config.c_iflag &= ~IGNBRK; - - uart_config.c_iflag &= ~ICRNL; + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - - // echo and echo NL off, canonical mode off (raw mode) - // extended input processing off, signal chars off - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - uart_config.c_oflag = 0; - - uart_config.c_cc[VMIN] = 0; - - uart_config.c_cc[VTIME] = 1; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); unsigned speed = B921600; @@ -347,51 +268,37 @@ void SF45LaserSerial::Run() if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { PX4_ERR("baud %d ATTR", termios_state); } - } if (_collect_phase) { - - /* perform collection */ - int collect_ret = collect(); - - - if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ - ScheduleDelayed(1042 * 8); - + if (hrt_absolute_time() - _last_received_time >= 1_s) { + start(); return; } - if (OK != collect_ret) { - // Too many packet errors in init, restart the consecutive fail count - _consecutive_fail_count = 0; - - /* restart the measurement state machine */ - start(); - return; + /* perform collection */ + if (collect() != PX4_OK && errno != EAGAIN) { + PX4_DEBUG("collect error"); + } - } else { - /* apparently success */ - _consecutive_fail_count = 0; + if (_sensor_state != STATE_SEND_STREAM) { + /* next phase is measurement */ + _collect_phase = false; } - /* next phase is measurement */ - _collect_phase = false; - } + } else { + /* measurement phase */ - /* measurement phase */ + if (measure() != PX4_OK) { + PX4_DEBUG("measure error"); + } - if (OK != measure()) { - PX4_DEBUG("measure error"); + /* next phase is collection */ + _collect_phase = true; } - /* next phase is collection */ - _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(_interval); - } void SF45LaserSerial::print_info() @@ -400,7 +307,7 @@ void SF45LaserSerial::print_info() perf_print_counter(_comms_errors); } -void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf) +void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id) { // SF45 protocol // Start byte is 0xAA and is the start of packet @@ -410,172 +317,178 @@ void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf) // ID byte precedes the data in the payload // CRC comprised of 16-bit checksum (not included in checksum calc.) - uint16_t recv_crc = 0; - bool restart_flag = false; + int ret; + size_t max_read = sizeof(_linebuf) - _linebuf_size; + ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); - while (restart_flag != true) { + if (ret < 0 && errno != EAGAIN) { + PX4_ERR("ERROR (ack from streaming distance data): %d", ret); + _linebuf_size = 0; + perf_count(_comms_errors); + perf_end(_sample_perf); + return; + } - switch (_parsed_state) { - case 0: { - if (input_buf[0] == 0xAA) { - // start of frame is valid, continue - _sop_valid = true; - _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); - _parsed_state = 1; - break; + if (ret > 0) { + _last_received_time = hrt_absolute_time(); + _linebuf_size += ret; + } - } else { - _sop_valid = false; - _crc_valid = false; - _parsed_state = 0; - restart_flag = true; - _calc_crc = 0; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("Start of packet not valid: %d", _sensor_state); - _consecutive_fail_count++; - break; - } // end else - } // end case 0 + // Not enough data to parse a complete packet. Gather more data in the next cycle. + if (_linebuf_size < payload_length) { + return; + } - case 1: { - rx_field.flags_lo = input_buf[1]; - _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); - _parsed_state = 2; - break; - } + int index = 0; - case 2: { - rx_field.flags_hi = input_buf[2]; - rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); - _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); - - // Check payload length against known max value - if (rx_field.data_len > 17) { - _parsed_state = 0; - restart_flag = true; - _calc_crc = 0; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("Payload length error: %d", _sensor_state); - _consecutive_fail_count++; - break; + while (index <= _linebuf_size - payload_length && _crc_valid == false) { + bool restart_flag = false; - } else { - _parsed_state = 3; - break; - } - } - - case 3: { + while (restart_flag != true) { + switch (_parsed_state) { + case 0: { + if (_linebuf[index] == 0xAA) { + // start of frame is valid, continue + _sop_valid = true; + _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); + _parsed_state = 1; + break; - rx_field.msg_id = input_buf[3]; + } else { + _sop_valid = false; + _crc_valid = false; + _parsed_state = 0; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Start of packet not valid: %d", _sensor_state); + break; + } // end else + } // end case 0 - if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT - || rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) { + case 1: { + rx_field.flags_lo = _linebuf[index + 1]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); + _parsed_state = 2; + break; + } - if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) { - _sensor_ready = true; + case 2: { + rx_field.flags_hi = _linebuf[index + 2]; + rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); + + // Check payload length against known max value + if (rx_field.data_len > 17) { + _parsed_state = 0; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Payload length error: %d", _sensor_state); + break; } else { - _sensor_ready = false; + _parsed_state = 3; + break; } - - _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); - - _parsed_state = 4; - break; } - // Ignore message ID's that aren't defined - else { - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - perf_count(_comms_errors); - perf_end(_sample_perf); - _consecutive_fail_count++; - PX4_DEBUG("Unknown message ID: %d", _sensor_state); - break; + case 3: { + rx_field.msg_id = _linebuf[index + 3]; + if (rx_field.msg_id == msg_id) { + _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); + _parsed_state = 4; + break; + } + + // Ignore message ID's that aren't searched + else { + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + PX4_DEBUG("Non needed message ID: %d", _sensor_state); + break; + } } - } - // Data - case 4: { - // Process commands with & w/out data bytes - if (rx_field.data_len > 1) { - for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { + // Data + case 4: { + // Process commands with & w/out data bytes + if (rx_field.data_len > 1) { + for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { - rx_field.data[_data_bytes_recv] = input_buf[i]; - _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); - _data_bytes_recv = _data_bytes_recv + 1; + rx_field.data[_data_bytes_recv] = _linebuf[index + i]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); + _data_bytes_recv = _data_bytes_recv + 1; - } // end for - } //end if + } // end for + } //end if - else { + else { + + _parsed_state = 5; + _data_bytes_recv = 0; + _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); + } _parsed_state = 5; _data_bytes_recv = 0; - _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); - + break; } - _parsed_state = 5; - _data_bytes_recv = 0; - break; - } - - // CRC low byte - case 5: { - rx_field.crc[0] = input_buf[3 + rx_field.data_len]; - _parsed_state = 6; - break; - } - - // CRC high byte - case 6: { - rx_field.crc[1] = input_buf[4 + rx_field.data_len]; - recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; - - // Check the received crc bytes from the sf45 against our own CRC calcuation - // If it matches, we can check if sensor ready - // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete - if (recv_crc == _calc_crc) { - _crc_valid = true; + // CRC low byte + case 5: { + rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len]; + _parsed_state = 6; + break; + } - // Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM - if (_sensor_ready) { - _init_complete = true; + // CRC high byte + case 6: { + rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len]; + uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; + + // Check the received crc bytes from the sf45 against our own CRC calcuation + // If it matches, we can check if sensor ready + // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete + if (recv_crc == _calc_crc) { + _crc_valid = true; + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + break; } else { - _init_complete = false; + + _crc_valid = false; + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + perf_count(_comms_errors); + PX4_DEBUG("CRC mismatch: %d", _sensor_state); + break; } + } + } // end switch + } //end while - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - break; + index++; + } - } else { + // If we parsed successfully, remove the parsed part from the buffer if it is still large enough + if (_crc_valid && index + payload_length < _linebuf_size) { + unsigned next_after_index = index + payload_length; + memmove(&_linebuf[0], &_linebuf[next_after_index], _linebuf_size - next_after_index); + _linebuf_size -= next_after_index; + } - _crc_valid = false; - _init_complete = false; - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("CRC mismatch: %d", _sensor_state); - break; - } - } - } // end switch - } //end while + // The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again. + if ((unsigned)_linebuf_size >= sizeof(_linebuf)) { + _linebuf_size = 0; + perf_count(_comms_errors); + } } -void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len) +void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len) { uint16_t crc_val = 0; uint8_t packet_buff[SF45_MAX_PAYLOAD]; @@ -612,7 +525,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d if (msg_id == SF_DISTANCE_OUTPUT) { uint8_t data_convert = data[0] & 0x00FF; // write data bytes to the output buffer - packet_buff[data_inc] = data_convert; + packet_buff[data_inc] = data_convert; // Add data bytes to crc add function crc_val = sf45_format_crc(crc_val, data_convert); data_inc = data_inc + 1; @@ -650,12 +563,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d // Add data bytes to crc add function crc_val = sf45_format_crc(crc_val, data[0]); data_inc = data_inc + 1; - } else { // Product Name - PX4_INFO("INFO: Product name"); + PX4_DEBUG("DEBUG: Product name"); } uint8_t crc_lo = crc_val & 0xFF; @@ -694,7 +606,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float if (raw_yaw > 32000) { raw_yaw = raw_yaw - 65535; - } // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle @@ -802,7 +713,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { - uint8_t mapped_sector = 0; float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset); mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment); diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index 4f75b3285201..de7af5468485 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -56,9 +56,9 @@ enum SF_SERIAL_STATE { STATE_UNINIT = 0, - STATE_SEND_PRODUCT_NAME = 1, - STATE_SEND_UPDATE_RATE = 2, - STATE_SEND_DISTANCE_DATA = 3, + STATE_ACK_PRODUCT_NAME = 1, + STATE_ACK_UPDATE_RATE = 2, + STATE_ACK_DISTANCE_OUTPUT = 3, STATE_SEND_STREAM = 4, }; @@ -80,8 +80,8 @@ class SF45LaserSerial : public px4::ScheduledWorkItem int init(); void print_info(); - void sf45_request_handle(uint8_t *value); - void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); + void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id); + void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); void sf45_process_replies(float *data); uint8_t sf45_convert_angle(const int16_t yaw); @@ -104,30 +104,26 @@ class SF45LaserSerial : public px4::ScheduledWorkItem char _port[20] {}; - int _interval{10000}; + int _interval{2000}; bool _collect_phase{false}; int _fd{-1}; uint8_t _linebuf[SF45_MAX_PAYLOAD] {}; - unsigned _linebuf_size{0}; - hrt_abstime _last_read{0}; + int _linebuf_size{0}; // SF45/B uses a binary protocol to include header,flags // message ID, payload, and checksum bool _is_sf45{false}; - bool _init_complete{false}; - bool _sensor_ready{false}; - uint8_t _sensor_state{0}; + SF_SERIAL_STATE _sensor_state{STATE_UNINIT}; int _baud_rate{0}; - int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int _stream_data{0}; - int32_t _update_rate{1}; - int _data_output{0}; + int32_t _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + int32_t _stream_data{0}; + int32_t _update_rate{0}; + int32_t _data_output{0}; const uint8_t _start_of_frame{0xAA}; uint16_t _data_bytes_recv{0}; uint8_t _parsed_state{0}; bool _sop_valid{false}; uint16_t _calc_crc{0}; - uint8_t _num_retries{0}; int32_t _yaw_cfg{0}; int32_t _orient_cfg{0}; uint8_t _previous_bin{0}; @@ -135,9 +131,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem // end of SF45/B data members - unsigned _consecutive_fail_count; - + hrt_abstime _last_received_time{0}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - }; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml index 4dbdf25c9083..36eddddfa0d1 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -32,7 +32,7 @@ parameters: 12: 5000hz reboot_required: true num_instances: 1 - default: 1 + default: 5 SF45_ORIENT_CFG: description: