Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SF45 fixes to restart the state machine if sensor does not init correctly #23565

Merged
merged 5 commits into from
Sep 23, 2024
Merged

SF45 fixes to restart the state machine if sensor does not init correctly #23565

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
cd73c5c
fixes to restart the state machine if sensor does not init correctly
dirksavage88 Aug 18, 2024
e6ffb69
fixes
dirksavage88 Aug 18, 2024
1e30e04
increase fail count
dirksavage88 Aug 18, 2024
3145295
remove extra flush, switch from warn to debug, add enum states for se…
dirksavage88 Sep 21, 2024
8dfbd5a
remove dead code, decrease restart fail count metric, break out of lo…
dirksavage88 Sep 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
99 changes: 87 additions & 12 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,33 +164,76 @@ int SF45LaserSerial::collect()
/* read from the sensor (uart buffer) */
const hrt_abstime timestamp_sample = hrt_absolute_time();

if (_consecutive_fail_count > 50 && !_sensor_ready) {
alexcekay marked this conversation as resolved.
Show resolved Hide resolved
PX4_ERR("Restarting the state machine");
start();
}
alexcekay marked this conversation as resolved.
Show resolved Hide resolved

if (_sensor_state == 1) {

ret = ::read(_fd, &readbuf[0], 22);

if (ret < 0) {
PX4_ERR("Read err state 1: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
return ret;
}

sf45_request_handle(ret, readbuf);
ScheduleDelayed(_interval * 2);
ScheduleDelayed(_interval * 3);

} else if (_sensor_state == 2) {

ret = ::read(_fd, &readbuf[0], 7);

if (ret < 0) {
PX4_ERR("Read err state 2: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
return ret;
}

if (readbuf[3] == SF_UPDATE_RATE) {
sf45_request_handle(ret, readbuf);
ScheduleDelayed(_interval * 5);
ScheduleDelayed(_interval * 3);
}

} else if (_sensor_state == 3) {

ret = ::read(_fd, &readbuf[0], 8);

if (ret < 0) {
PX4_ERR("Read err state 3: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
return ret;
}

if (readbuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(ret, readbuf);
ScheduleDelayed(_interval * 5);
ScheduleDelayed(_interval * 3);
}

} else {

ret = ::read(_fd, &readbuf[0], 10);

if (ret < 0) {
PX4_ERR("Read err state 4: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
return ret;
}

uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);

if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
Expand All @@ -207,6 +250,14 @@ int SF45LaserSerial::collect()

ret = ::read(_fd, &readbuf[0], 10);

if (ret < 0) {
PX4_ERR("Read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
return ret;
}
}
}

Expand All @@ -218,6 +269,8 @@ int SF45LaserSerial::collect()
/* only throw an error if we time out */
if (read_elapsed > (_interval * 2)) {
PX4_DEBUG("Timing out...");
// Data received but not read
tcflush(_fd, TCIFLUSH);
return ret;

} else {
Expand Down Expand Up @@ -328,6 +381,7 @@ void SF45LaserSerial::Run()
/* 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);
Expand All @@ -336,7 +390,7 @@ void SF45LaserSerial::Run()
}

if (OK != collect_ret) {
/* we know the sensor needs about four seconds to initialize */
/* we know the sensor needs about five seconds to initialize */
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
PX4_ERR("collection error #%u", _consecutive_fail_count);
}
Expand Down Expand Up @@ -407,7 +461,12 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
_parsed_state = 0;
restart_flag = true;
_calc_crc = 0;
PX4_INFO("INFO: start of packet not valid");
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
PX4_WARN("Start of packet not valid: %d", _sensor_state);
_consecutive_fail_count++;
break;
} // end else
} // end case 0
Expand All @@ -426,10 +485,15 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)

// Check payload length against known max value
if (rx_field.data_len > 17) {
PX4_INFO("INFO: payload length invalid, restarting data request");
_parsed_state = 0;
restart_flag = true;
_calc_crc = 0;
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
PX4_WARN("Payload length error: %d", _sensor_state);
_consecutive_fail_count++;
break;

} else {
Expand Down Expand Up @@ -463,6 +527,12 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
_consecutive_fail_count++;
PX4_WARN("Unknown message ID: %d", _sensor_state);
break;

}
Expand Down Expand Up @@ -526,12 +596,17 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
break;

} else {
PX4_INFO("INFO: CRC mismatch");

_crc_valid = false;
_init_complete = false;
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
perf_count(_comms_errors);
perf_end(_sample_perf);
// Data received but not read
tcflush(_fd, TCIFLUSH);
PX4_WARN("CRC mismatch: %d", _sensor_state);
break;
}
}
Expand Down Expand Up @@ -633,22 +708,22 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
packet_len = (uint8_t)len;

// DEBUG
for (uint8_t i = 0; i < packet_len; ++i) {
/*for (uint8_t i = 0; i < packet_len; ++i) {
PX4_INFO("INFO: Send byte: %d", packet_buff[i]);
}
}*/
alexcekay marked this conversation as resolved.
Show resolved Hide resolved

ret = ::write(_fd, packet_buff, packet_len);

if (ret != packet_len) {
perf_count(_comms_errors);
PX4_DEBUG("write fail %d", ret);
//return ret;
PX4_ERR("write fail %d", ret);
// Flush data written, not transmitted
tcflush(_fd, TCOFLUSH);
}
}

void SF45LaserSerial::sf45_process_replies(float *distance_m)
{

switch (rx_field.msg_id) {
case SF_DISTANCE_DATA_CM: {

Expand Down
2 changes: 1 addition & 1 deletion src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
uint8_t _parsed_state{0};
bool _sop_valid{false};
uint16_t _calc_crc{0};
uint8_t _num_retries{2};
uint8_t _num_retries{0};
int32_t _yaw_cfg{0};
int32_t _orient_cfg{0};
int32_t _collision_constraint{0};
Expand Down
Loading