From 184c7fe79d63a49164568292c617ff0c526f4883 Mon Sep 17 00:00:00 2001 From: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com> Date: Wed, 1 Mar 2023 16:16:25 -0500 Subject: [PATCH] drivers/distance_sensor: Lightware Lidar SF45/B rotating sensor serial driver (#19891) Signed-off-by: dirksavage88 --- .../lightware_sf45_serial/CMakeLists.txt | 50 ++ .../lightware_sf45_serial/Kconfig | 5 + .../lightware_sf45_serial.cpp | 761 ++++++++++++++++++ .../lightware_sf45_serial.hpp | 121 +++ .../lightware_sf45_serial_main.cpp | 167 ++++ .../lightware_sf45_serial/module.yaml | 63 ++ .../lightware_sf45_serial/sf45_commands.h | 89 ++ 7 files changed, 1256 insertions(+) create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/Kconfig create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/module.yaml create mode 100755 src/drivers/distance_sensor/lightware_sf45_serial/sf45_commands.h diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt new file mode 100755 index 0000000000..561b7755c7 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__distance_sensor__lightware_sf45_serial + MAIN lightware_sf45_serial + COMPILE_FLAGS + SRCS + lightware_sf45_serial.cpp + lightware_sf45_serial.hpp + lightware_sf45_serial_main.cpp + DEPENDS + drivers_rangefinder + px4_work_queue + MODULE_CONFIG + module.yaml + ) + +if(PX4_TESTING) + add_subdirectory(tests) +endif() diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/Kconfig b/src/drivers/distance_sensor/lightware_sf45_serial/Kconfig new file mode 100755 index 0000000000..b2ab01855d --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL + bool "lightware_sf45_serial" + default n + ---help--- + Enable support for lightware_sf45_serial 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 new file mode 100755 index 0000000000..0115dd98b5 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -0,0 +1,761 @@ +/************************************************************************** + * + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "lightware_sf45_serial.hpp" +#include "sf45_commands.h" + +#include +#include +#include +#include + +#include +#include +#include + +/* Configuration Constants */ +#define SF45_MAX_PAYLOAD 256 +#define SF45_SCALE_FACTOR 0.01f + +SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _px4_rangefinder(0, rotation), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) +{ + /* store port name */ + strncpy(_port, port, sizeof(_port) - 1); + + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; + + uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx' + + if (bus_num < 10) { + device_id.devid_s.bus = bus_num; + } + + _num_retries = 2; + _px4_rangefinder.set_device_id(device_id.devid); + _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); + + // populate obstacle map members + _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + _obstacle_map_msg.increment = 5; + _obstacle_map_msg.angle_offset = -2.5; + _obstacle_map_msg.min_distance = UINT16_MAX; + _obstacle_map_msg.max_distance = 5000; + +} + +SF45LaserSerial::~SF45LaserSerial() +{ + stop(); + + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int SF45LaserSerial::init() +{ + + param_get(param_find("SF45_UPDATE_CFG"), &_update_rate); + param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); + param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); + + /* SF45/B (50M) */ + _px4_rangefinder.set_min_distance(0.2f); + _px4_rangefinder.set_max_distance(50.0f); + _interval = 10000; + + start(); + + return PX4_OK; +} + +int SF45LaserSerial::measure() +{ + + int rate = (int)_update_rate; + _data_output = 0x101; // raw distance + yaw readings + _stream_data = 5; // enable constant streaming + + // send some packets so the sensor starts scanning + switch (_sensor_state) { + + // sensor should now respond + case 0: + while (_num_retries--) { + sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); + _sensor_state = 0; + } + + _sensor_state = 1; + break; + + case 1: + // Update rate default to 50 readings/s + sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t)); + _sensor_state = 2; + break; + + case 2: + sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output)); + _sensor_state = 3; + break; + + case 3: + sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data)); + _sensor_state = 4; + break; + + default: + break; + } + + return PX4_OK; +} + +int SF45LaserSerial::collect() +{ + perf_begin(_sample_perf); + + /* clear buffer if last read was too long ago */ + int64_t read_elapsed = hrt_elapsed_time(&_last_read); + int ret; + /* the buffer for read chars is buflen minus null termination */ + param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint); + uint8_t readbuf[SF45_MAX_PAYLOAD]; + + float distance_m = -1.0f; + + /* read from the sensor (uart buffer) */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (_sensor_state == 1) { + + ret = ::read(_fd, &readbuf[0], 22); + sf45_request_handle(ret, readbuf); + ScheduleDelayed(_interval * 2); + + } else if (_sensor_state == 2) { + + ret = ::read(_fd, &readbuf[0], 7); + + if (readbuf[3] == SF_UPDATE_RATE) { + sf45_request_handle(ret, readbuf); + ScheduleDelayed(_interval * 5); + } + + } else if (_sensor_state == 3) { + + ret = ::read(_fd, &readbuf[0], 8); + + if (readbuf[3] == SF_DISTANCE_OUTPUT) { + sf45_request_handle(ret, readbuf); + ScheduleDelayed(_interval * 5); + } + + } else { + + ret = ::read(_fd, &readbuf[0], 10); + uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2); + + if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { + + for (uint8_t i = 0; i < ret; ++i) { + sf45_request_handle(ret, readbuf); + } + + if (_init_complete) { + sf45_process_replies(&distance_m); + } // end if + + } else { + + ret = ::read(_fd, &readbuf[0], 10); + + } + } + + if (ret < 0) { + PX4_DEBUG("read err: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + + /* only throw an error if we time out */ + if (read_elapsed > (_interval * 2)) { + PX4_DEBUG("Timing out..."); + return ret; + + } else { + + return -EAGAIN; + } + + } else if (ret == 0) { + return -EAGAIN; + } + + _last_read = hrt_absolute_time(); + + if (!_crc_valid) { + return -EAGAIN; + } + + PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO")); + _px4_rangefinder.update(timestamp_sample, distance_m); + + perf_end(_sample_perf); + + return PX4_OK; +} + +void SF45LaserSerial::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + + /* schedule a cycle to start things */ + ScheduleNow(); +} + +void SF45LaserSerial::stop() +{ + ScheduleClear(); +} + +void SF45LaserSerial::Run() +{ + /* fds initialized? */ + if (_fd < 0) { + /* open fd: non-blocking read mode*/ + + _fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (_fd < 0) { + PX4_ERR("open failed (%i)", errno); + return; + } + + struct termios uart_config; + + int termios_state; + + /* 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; + + 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; + + unsigned speed = B921600; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d OSPD", termios_state); + } + + 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) { + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + PX4_ERR("collection error #%u", _consecutive_fail_count); + } + + _consecutive_fail_count++; + + /* restart the measurement state machine */ + start(); + return; + + } else { + /* apparently success */ + _consecutive_fail_count = 0; + } + + /* next phase is measurement */ + _collect_phase = false; + } + + /* 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() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf) +{ + + // SF45 protocol + // Start byte is 0xAA and is the start of packet + // Payload length sanity check (0-1023) bytes + // and represented by 16-bit integer (payload + + // read/write status) + // 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; + + while (restart_flag != true) { + + 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; + + } else { + _sop_valid = false; + _crc_valid = false; + _parsed_state = 0; + restart_flag = true; + _calc_crc = 0; + PX4_INFO("INFO: start of packet not valid"); + break; + } // end else + } // end case 0 + + case 1: { + rx_field.flags_lo = input_buf[1]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); + _parsed_state = 2; + break; + } + + 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) { + PX4_INFO("INFO: payload length invalid, restarting data request"); + _parsed_state = 0; + restart_flag = true; + _calc_crc = 0; + 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; + + } else { + _sensor_ready = false; + } + + _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; + 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] = input_buf[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 + + 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; + 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; + + // Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM + if (_sensor_ready) { + _init_complete = true; + + } else { + _init_complete = false; + } + + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + break; + + } else { + PX4_INFO("INFO: CRC mismatch"); + _crc_valid = false; + _init_complete = false; + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + break; + } + } + } // end switch + } //end while +} + +void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len) +{ + uint16_t crc_val = 0; + uint8_t packet_buff[SF45_MAX_PAYLOAD]; + uint8_t data_inc = 4; + int ret = 0; + uint8_t packet_len = 0; + // Include payload ID byte in payload len + uint16_t flags = (data_len + 1) << 6; + + // If writing to the device, lsb is 1 + if (write) { + flags |= 0x01; + } + + else { + flags |= 0x0; + } + + uint8_t flags_lo = flags & 0xFF; + uint8_t flags_hi = (flags >> 8) & 0xFF; + + // Add packet bytes to format into crc based on CRC-16-CCITT 0x1021/XMODEM + crc_val = sf45_format_crc(crc_val, _start_of_frame); + crc_val = sf45_format_crc(crc_val, flags_lo); + crc_val = sf45_format_crc(crc_val, flags_hi); + crc_val = sf45_format_crc(crc_val, msg_id); + + // Write the packet header contents + payload msg ID to the output buffer + packet_buff[0] = _start_of_frame; + packet_buff[1] = flags_lo; + packet_buff[2] = flags_hi; + packet_buff[3] = msg_id; + + 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; + // Add data bytes to crc add function + crc_val = sf45_format_crc(crc_val, data_convert); + data_inc = data_inc + 1; + data_convert = data[0] >> 8; + packet_buff[data_inc] = data_convert; + crc_val = sf45_format_crc(crc_val, data_convert); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + } + + else if (msg_id == SF_STREAM) { + packet_buff[data_inc] = data[0]; + //pad zeroes + crc_val = sf45_format_crc(crc_val, data[0]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + } + + else if (msg_id == SF_UPDATE_RATE) { + // Update Rate + packet_buff[data_inc] = (uint8_t)data[0]; + // 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"); + } + + uint8_t crc_lo = crc_val & 0xFF; + uint8_t crc_hi = (crc_val >> 8) & 0xFF; + + packet_buff[data_inc] = crc_lo; + data_inc = data_inc + 1; + packet_buff[data_inc] = crc_hi; + + size_t len = sizeof(packet_buff[0]) * (data_inc + 1); + packet_len = (uint8_t)len; + + // DEBUG + for (uint8_t i = 0; i < packet_len; ++i) { + PX4_INFO("INFO: Send byte: %d", packet_buff[i]); + } + + ret = ::write(_fd, packet_buff, packet_len); + + if (ret != packet_len) { + perf_count(_comms_errors); + PX4_DEBUG("write fail %d", ret); + //return ret; + } +} + +void SF45LaserSerial::sf45_process_replies(float *distance_m) +{ + + switch (rx_field.msg_id) { + case SF_DISTANCE_DATA_CM: { + + uint16_t obstacle_dist_cm = 0; + const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); + int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); + int16_t scaled_yaw = 0; + + // 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 + if (_orient_cfg == 1) { + raw_yaw = raw_yaw * -1; + } + + switch (_yaw_cfg) { + case 0: + break; + + case 1: + if (raw_yaw > 180) { + raw_yaw = raw_yaw - 180; + + } else { + raw_yaw = raw_yaw + 180; // rotation facing aft + } + + break; + + case 2: + raw_yaw = raw_yaw + 90; // rotation facing right + break; + + case 3: + raw_yaw = raw_yaw - 90; // rotation facing left + break; + + default: + break; + } + + scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; + + // Convert to meters for rangefinder update + *distance_m = raw_distance * SF45_SCALE_FACTOR; + obstacle_dist_cm = (uint16_t)raw_distance; + + uint8_t current_bin = sf45_convert_angle(scaled_yaw); + + // If we have moved to a new bin + + if (current_bin != _previous_bin) { + + // update the current bin to the distance sensor reading + // readings in cm + _obstacle_map_msg.distances[current_bin] = obstacle_dist_cm; + _obstacle_map_msg.timestamp = hrt_absolute_time(); + + } + + _previous_bin = current_bin; + + _obstacle_distance_pub.publish(_obstacle_map_msg); + + break; + } + + default: + // add case for future use + break; + } +} + +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); + + return mapped_sector; +} + +float SF45LaserSerial::sf45_wrap_360(float f) +{ + return matrix::wrap(f, 0.f, 360.f); +} + +uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val) +{ + uint32_t i; + const uint16_t poly = 0x1021u; + crc ^= (uint16_t)((uint16_t) data_val << 8u); + + for (i = 0; i < 8; i++) { + if (crc & (1u << 15u)) { + crc = (uint16_t)((crc << 1u) ^ poly); + + } else { + crc = (uint16_t)(crc << 1u); + } + } + + return crc; +} 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 new file mode 100755 index 0000000000..0236f86f3b --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file lightware_sf45_serial.hpp + * @author Andrew Brahim + * + * Serial Protocol driver for the Lightware SF45/B rangefinder series + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "sf45_commands.h" +class SF45LaserSerial : public px4::ScheduledWorkItem +{ +public: + SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + ~SF45LaserSerial() override; + + int init(); + void print_info(); + void sf45_request_handle(int val, uint8_t *value); + void sf45_send(uint8_t msg_id, bool r_w, int *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); + float sf45_wrap_360(float f); +protected: + obstacle_distance_s _obstacle_map_msg{}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ + +private: + + void start(); + void stop(); + void Run() override; + int measure(); + int collect(); + bool _crc_valid{false}; + PX4Rangefinder _px4_rangefinder; + + char _port[20] {}; + int _interval{10000}; + bool _collect_phase{false}; + int _fd{-1}; + int _linebuf[256] {}; + unsigned _linebuf_index{0}; + hrt_abstime _last_read{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}; + 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}; + 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{2}; + int32_t _yaw_cfg{0}; + int32_t _orient_cfg{0}; + int32_t _collision_constraint{0}; + uint16_t _previous_bin{0}; + + // end of SF45/B data members + + unsigned _consecutive_fail_count; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + +}; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp new file mode 100755 index 0000000000..f05417e053 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "lightware_sf45_serial.hpp" + +#include +#include + +namespace lightware_sf45 +{ + +SF45LaserSerial *g_dev{nullptr}; + +static int start(const char *port, uint8_t rotation) +{ + if (g_dev != nullptr) { + PX4_WARN("already started"); + return -1; + } + + if (port == nullptr) { + PX4_ERR("no device specified"); + return -1; + } + + /* create the driver */ + g_dev = new SF45LaserSerial(port, rotation); + + if (g_dev == nullptr) { + return -1; + } + + if (g_dev->init() != PX4_OK) { + delete g_dev; + g_dev = nullptr; + return -1; + } + + return 0; +} + +static int stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + return -1; + } + + return 0; +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return -1; + } + + g_dev->print_info(); + + return 0; +} + +static int usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial bus driver for the Lightware SF45/b Laser rangefinder. + +Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html + +### Examples + +Attempt to start driver on a specified serial device. +$ lightware_sf45_serial start -d /dev/ttyS1 +Stop driver +$ lightware_sf45_serial stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("lightware_sf45_serial", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) +{ + uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING; + const char *device_path = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (uint8_t)atoi(myoptarg); + break; + + case 'd': + device_path = myoptarg; + break; + + default: + lightware_sf45::usage(); + return -1; + } + } + + if (myoptind >= argc) { + lightware_sf45::usage(); + return -1; + } + + if (!strcmp(argv[myoptind], "start")) { + return lightware_sf45::start(device_path, rotation); + + } else if (!strcmp(argv[myoptind], "stop")) { + return lightware_sf45::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return lightware_sf45::status(); + } + + lightware_sf45::usage(); + return -1; +} diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml new file mode 100755 index 0000000000..6356411fbe --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -0,0 +1,63 @@ +module_name: Lightware SF45 Rangefinder (serial) +serial_config: + - command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV} + port_config_param: + name: SENS_EN_SF45_CFG + group: Sensors + default: TEL2 + num_instances: 1 + supports_networking: false + +parameters: + - group: Sensors + definitions: + SF45_UPDATE_CFG: + description: + short: Update rate in Hz + long: | + The SF45 sets the update rate in Hz to allow greater resolution + type: enum + values: + 1: 50hz + 2: 100hz + 3: 200hz + 4: 400hz + 5: 500hz + 6: 625hz + 7: 1000hz + 8: 1250hz + 9: 1538hz + 10: 2000hz + 11: 2500hz + 12: 5000hz + reboot_required: true + num_instances: 1 + default: 1 + + SF45_ORIENT_CFG: + description: + short: Orientation upright or facing downward + long: | + The SF45 mounted facing upward or downward on the frame + type: enum + values: + 0: Rotation upward + 1: Rotation downward + reboot_required: true + num_instances: 1 + default: 0 + + SF45_YAW_CFG: + description: + short: Sensor facing forward or backward + long: | + The usb port on the sensor indicates 180deg, opposite usb is forward facing + type: enum + values: + 0: Rotation forward + 1: Rotation backward + 2: Rotation right + 3: Rotation left + reboot_required: true + num_instances: 1 + default: 0 diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/sf45_commands.h b/src/drivers/distance_sensor/lightware_sf45_serial/sf45_commands.h new file mode 100755 index 0000000000..a543346064 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_sf45_serial/sf45_commands.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf45_commands.h + * @author Andrew Brahim + * + * Declarations of sf45 serial commands for the Lightware sf45/b series + */ + +#pragma once +#define SF45_MAX_PAYLOAD 256 +#define SF45_CRC_FIELDS 2 + +enum SF_SERIAL_CMD { + SF_PRODUCT_NAME = 0, + SF_HARDWARE_VERSION = 1, + SF_FIRMWARE_VERSION = 2, + SF_SERIAL_NUMBER = 3, + SF_TEXT_MESSAGE = 7, + SF_USER_DATA = 9, + SF_TOKEN = 10, + SF_SAVE_PARAMETERS = 12, + SF_RESET = 14, + SF_STAGE_FIRMWARE = 16, + SF_COMMIT_FIRMWARE = 17, + SF_DISTANCE_OUTPUT = 27, + SF_STREAM = 30, + SF_DISTANCE_DATA_CM = 44, + SF_DISTANCE_DATA_MM = 45, + SF_LASER_FIRING = 50, + SF_TEMPERATURE = 57, + SF_UPDATE_RATE = 66, + SF_NOISE = 74, + SF_ZERO_OFFSET = 75, + SF_LOST_SIGNAL_COUNTER = 76, + SF_BAUD_RATE = 79, + SF_I2C_ADDRESS = 80, + SF_SCAN_SPEED = 85, + SF_STEPPER_STATUS = 93, + SF_SCAN_ON_STARTUP = 94, + SF_SCAN_ENABLE = 96, + SF_SCAN_POSITION = 97, + SF_SCAN_LOW_ANGLE = 98, + SF_HIGH_ANGLE = 99 +}; + +// Store contents of rx'd frame +struct { + const uint8_t data_fields = 2; // useful for breaking crc's into byte separated fields + uint16_t data_len{0}; // last message payload length (1+ bytes in payload) + uint8_t data[SF45_MAX_PAYLOAD]; // payload size limited by posix serial + uint8_t msg_id{0}; // latest message's message id + uint8_t flags_lo{0}; // flags low byte + uint8_t flags_hi{0}; // flags high byte + uint16_t crc[SF45_CRC_FIELDS] = {0, 0}; + uint8_t crc_lo{0}; // crc low byte + uint8_t crc_hi{0}; // crc high byte +} rx_field;