fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 06:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

SENS: RNG: SF45:Fix startup problems, increase frequency, robust parser, use nonblocking reads

Browse Source
This commit is contained in:
Alexander Lerach
2024-12-16 10:49:13 +01:00
committed by Silvan Fuhrer
parent 88d771e3e5
commit f34b22907c
3 changed files with 246 additions and 342 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+232 -322
View File
@@ -37,10 +37,11 @@
#include <fcntl.h>
#include <termios.h>
#include <lib/crc/crc.h>
#include <lib/mathlib/mathlib.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
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);
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE);
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_UPDATE_RATE) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}
return -EAGAIN;
} else if (_sensor_state == STATE_SEND_DISTANCE_DATA) {
} else if (_sensor_state == STATE_ACK_UPDATE_RATE) {
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);
perf_end(_sample_perf);
return ret;
}
if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}
} 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;
return PX4_OK;
}
// Not enough data to parse a complete packet. Gather more data in the next cycle.
if (_linebuf_size < payload_length) {
return -EAGAIN;
return -EAGAIN;
} else {
// Stream data from sensor
perf_begin(_sample_perf);
const int payload_length = 10;
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
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;
}
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);
// Process the incoming distance data
if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
sf45_request_handle(&_linebuf[index]);
if (_init_complete && _crc_valid) {
sf45_process_replies(&distance_m);
_linebuf_size = 0;
break;
}
}
}
index--;
}
// 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 (_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;
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
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;
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);
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 */
if (hrt_absolute_time() - _last_received_time >= 1_s) {
start();
return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
}
/* next phase is measurement */
_collect_phase = false;
/* perform collection */
if (collect() != PX4_OK && errno != EAGAIN) {
PX4_DEBUG("collect error");
}
if (_sensor_state != STATE_SEND_STREAM) {
/* next phase is measurement */
_collect_phase = false;
}
} else {
/* measurement phase */
if (measure() != PX4_OK) {
PX4_DEBUG("measure error");
}
/* next phase is collection */
_collect_phase = true;
}
/* measurement phase */
if (OK != measure()) {
PX4_DEBUG("measure error");
}
/* 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);
while (index <= _linebuf_size - payload_length && _crc_valid == false) {
bool restart_flag = false;
// 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;
} else {
_parsed_state = 3;
break;
}
}
case 3: {
rx_field.msg_id = input_buf[3];
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) {
if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) {
_sensor_ready = true;
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;
} else {
_sensor_ready = false;
_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
case 1: {
rx_field.flags_lo = _linebuf[index + 1];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
_parsed_state = 2;
break;
}
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 {
_parsed_state = 3;
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;
}
_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;
}
}
// 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;
// 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] = _linebuf[index + i];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
_data_bytes_recv = _data_bytes_recv + 1;
// 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) {
} // end for
} //end if
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;
else {
} // end for
} //end if
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] = _linebuf[index + 3 + rx_field.data_len];
_parsed_state = 6;
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] = _linebuf[index + 4 + rx_field.data_len];
uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
// 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;
// Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM
if (_sensor_ready) {
_init_complete = true;
// 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;
}
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
break;
} else {
_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
} // end switch
} //end while
index++;
}
// 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;
}
// 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);
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
+13 -19
View File
@@ -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 @@ public:
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 @@ private:
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 @@ private:
// 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;
};
src/drivers/distance_sensor/lightware_sf45_serial/module.yaml
+1 -1
View File
@@ -32,7 +32,7 @@ parameters:
12: 5000hz
reboot_required: true
num_instances: 1
default: 1
default: 5
SF45_ORIENT_CFG:
description: