diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index 395151b8cc..2f9d8479b3 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -19,6 +19,7 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_GRF_SERIAL=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y diff --git a/docs/assets/hardware/sensors/lidar_lightware/grf_500.png b/docs/assets/hardware/sensors/lidar_lightware/grf_500.png new file mode 100644 index 0000000000..baccec33a3 Binary files /dev/null and b/docs/assets/hardware/sensors/lidar_lightware/grf_500.png differ diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index 261c8995d4..041f0955da 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -255,6 +255,7 @@ - [Lidar-Lite](sensor/lidar_lite.md) - [Lightware Lidars (SF/LW/GRF)](sensor/sfxx_lidar.md) - [Lightware SF45 Rotary Lidar](sensor/sf45_rotating_lidar.md) + - [Lightware GRF250/GRF500 Gimbal Lidar](sensor/grf_lidar.md) - [TeraRanger](sensor/teraranger.md) - [✘ Lanbao PSK-CM8JL65-CC5](sensor/cm8jl65_ir_distance_sensor.md) - [Avionics Anonymous Laser Altimeter UAVCAN Interface (CAN)](dronecan/avanon_laser_interface.md) diff --git a/docs/en/sensor/grf_lidar.md b/docs/en/sensor/grf_lidar.md new file mode 100644 index 0000000000..efbc4e2718 --- /dev/null +++ b/docs/en/sensor/grf_lidar.md @@ -0,0 +1,68 @@ +# Lightware GRF250/GRF500 Gimbal Lidar + +LightWare [GRF250](https://lightwarelidar.com/shop/grf-250/) and [GRF500](https://lightwarelidar.com/shop/grf-500/) are small and light Lidar modules with a range of 250m and 500m, respectively. + +![LightWare GRF250 Gimbal Lidar](../../assets/hardware/sensors/lidar_lightware/grf_500.png) + +::: info +The Lidar driver is not included in the default build of PX4. +You will need to [create and use a custom build](#add-the-driver-to-the-px4-build). +::: + +## Where to Buy + +Order these modules from: + +- [GRF250](https://lightwarelidar.com/shop/grf-250/) +- [GRF500](https://lightwarelidar.com/shop/grf-500/) + +## Hardware Setup + +The rangefinder can be connected to any unused serial port, such as `TELEM2`. +[Parameter Configuration](#parameter-configuration) explains how to configure the port to use and the other properties of the rangefinder. + +## PX4 Setup + +### Add the Driver to the PX4 Build + +The [lightware_grf_serial](../modules/modules_driver_distance_sensor.md#lightware-grf-serial) driver for this Lidar is not included in PX4 firmware by default. +In order to use these modules you will first need to update the firmware configuration to add the driver, and then build the firmware. + +1. Update the firmware configuration. You can use either of the following options: + - Menuconfig: + 1. Install and open [menuconfig](../hardware/porting_guide_config.md#px4-menuconfig-setup) + 2. In [menuconfig](../hardware/porting_guide_config.md#px4-menuconfig-setup), navigate to **Drivers > Distance sensors** + 3. Select/Enable `lightware_grf_serial` + 4. Save the configuration + + - Manually update `default.px4` to include the configuration key: + 1. Open the `default.px4board` config file that corresponds to the board you want to build for. + For example, to add the driver to `fmu-v6x` boards you would update [/boards/px4/fmu-v6x/default.px4board ](https://github.com/PX4/PX4-Autopilot/blob/main/boards/px4/fmu-v6x/default.px4board) + 2. Add the following line and save the file: + + ```txt + CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_GRF_SERIAL=y + ``` + +2. [Build PX4](../dev_setup/building_px4.md) for your flight controller target and upload the new firmware. + +### Parameter Configuration + +You will need to configure PX4 to indicate the serial port to which the sensor is connected (as per [Serial Port Configuration](../peripherals/serial_configuration.md)) and also the orientation and other properties of the sensor. + +The [parameters to change](../advanced_config/parameters.md) are listed in the table. + +| Parameter | Description | +| -------------------------------------------------------------------------------------------------------- | -------------------------------------------------------- | +| [SENS_EN_GRF_CFG](../advanced_config/parameter_reference.md#SENS_EN_GRF_CFG) | Set to the serial port the sensor is connected to. | +| [GRF_UPDATE_CFG](../advanced_config/parameter_reference.md#GRF_UPDATE_CFG) | Set the update rate. | +| [GRF_SENS_MODEL](../advanced_config/parameter_reference.md#GRF_SENS_MODEL) | Set the update rate. | + +## Testing + +You can confirm that the sensor is correctly configured by connecting QGroundControl, and observing that [DISTANCE_SENSOR](https://mavlink.io/en/messages/common.html#DISTANCE_SENSOR) is present in the [MAVLink Inspector](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/analyze_view/mavlink_inspector.html). + +Moving the sensor around at various distances from a surface will have the `current_distance` value change. + +## Troubleshooting +If you are having problems with connecting to the sensor you may need to unassign a the default serial port. [Unassign Default Serial Port](../peripherals/serial_configuration.md) diff --git a/docs/en/sensor/sfxx_lidar.md b/docs/en/sensor/sfxx_lidar.md index e225422805..2ca57d9972 100644 --- a/docs/en/sensor/sfxx_lidar.md +++ b/docs/en/sensor/sfxx_lidar.md @@ -15,8 +15,8 @@ The following models are supported by PX4, and can be connected to either the I2 | [LW20/C](https://lightware.co.za/products/lw20-c-100-m) | 100 | I2C bus | Waterproofed (IP67) with servo for sense-and-avoid applications | | [SF30/D](https://lightwarelidar.com/shop/sf30-d-200-m/) | 200 | I2C bus | Waterproofed (IP67) | | [SF45/B](../sensor/sf45_rotating_lidar.md) | 50 | Serial | Rotary Lidar (Used for [Collision Prevention](../computer_vision/collision_prevention.md)) | -| [GRF250](https://lightwarelidar.com/shop/grf-250/) | 250 | I2C | Gimbal Range Finder | -| [GRF500](https://lightwarelidar.com/shop/grf-500/) | 500 | I2C | Gimbal Range Finder | +| [GRF250](../sensor/grf_lidar.md) | 250 | Serial or I2C | Gimbal Range Finder | +| [GRF500](../sensor/grf_lidar.md) | 500 | Serial or I2C | Gimbal Range Finder | ::: details Discontinued @@ -69,6 +69,9 @@ VTOL vehicles may choose to also set [SF1XX_MODE](../advanced_config/parameter_r ::: tip [SF45/B](../sensor/sf45_rotating_lidar.md) setup is covered in the linked document. ::: +::: tip +[GRF250/GRF500](../sensor/grf_lidar.md) setup is covered in the linked document. +::: ### Hardware diff --git a/src/drivers/distance_sensor/lightware_grf_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_grf_serial/CMakeLists.txt new file mode 100755 index 0000000000..9a3c045be7 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2026 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_grf_serial + MAIN lightware_grf_serial + COMPILE_FLAGS + SRCS + lightware_grf_serial.cpp + lightware_grf_serial.hpp + lightware_grf_serial_main.cpp + DEPENDS + drivers_rangefinder + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/distance_sensor/lightware_grf_serial/Kconfig b/src/drivers/distance_sensor/lightware_grf_serial/Kconfig new file mode 100755 index 0000000000..3c89bf0974 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_GRF_SERIAL + bool "lightware_grf_serial" + default n + ---help--- + Enable support for lightware_grf_serial diff --git a/src/drivers/distance_sensor/lightware_grf_serial/grf_commands.h b/src/drivers/distance_sensor/lightware_grf_serial/grf_commands.h new file mode 100755 index 0000000000..c278a166c6 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/grf_commands.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 grf_commands.h + * @author Aaron Porter + * + * Declarations of grf serial commands for the Lightware grf series + */ + +#pragma once +#define GRF_MAX_PAYLOAD 256 +#define GRF_CRC_FIELDS 2 + +enum GRF_SERIAL_CMD { + GRF_PRODUCT_NAME = 0, + GRF_HARDWARE_VERSION = 1, + GRF_FIRMWARE_VERSION = 2, + GRF_SERIAL_NUMBER = 3, + GRF_TEXT_MESSAGE = 7, + GRF_USER_DATA = 9, + GRF_TOKEN = 10, + GRF_SAVE_PARAMETERS = 12, + GRF_RESET = 14, + GRF_STAGE_FIRMWARE = 16, + GRF_COMMIT_FIRMWARE = 17, + GRF_DISTANCE_OUTPUT = 27, + GRF_STREAM = 30, + GRF_DISTANCE_DATA_CM = 44, + GRF_DISTANCE_DATA_MM = 45, + GRF_LASER_FIRING = 50, + GRF_TEMPERATURE = 55, + GRF_AUTO_EXPOSURE = 70, + GRF_UPDATE_RATE = 74, + GRF_LOST_SIGNAL_COUNT = 78, + GRF_GPIO_MODE = 83, + GRF_GPIO_ALARM = 84, + GRF_MEDIAN_FILTER_EN = 86, + GRF_MEDIAN_FILETER_S = 87, + GRF_SMOOTH_FILTER_EN = 88, + GRF_SMOOTH_FACTOR = 89, + GRF_BAUD_RATE = 91, + GRF_I2C_ADDRESS = 92, + GRF_ROLL_AVG_EN = 93, + GRF_ROLL_AVG_SIZE = 94, + GRF_SLEEP_COMMAND = 98, + GRF_ZERO_OFFSET = 114 +}; + +// 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[GRF_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[GRF_CRC_FIELDS] = {0, 0}; + uint8_t crc_lo{0}; // crc low byte + uint8_t crc_hi{0}; // crc high byte +} rx_field; diff --git a/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.cpp b/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.cpp new file mode 100755 index 0000000000..403c61f562 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.cpp @@ -0,0 +1,633 @@ +/************************************************************************** + * + * Copyright (c) 2026 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_grf_serial.hpp" + +#include +#include +#include +#include + +#include + +using namespace time_literals; + +GRFLaserSerial::GRFLaserSerial(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; + } + + _px4_rangefinder.set_device_id(device_id.devid); + _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); + +} + +GRFLaserSerial::~GRFLaserSerial() +{ + stop(); + + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int GRFLaserSerial::init() +{ + param_get(param_find("GRF_RATE_CFG"), &_update_rate); + param_get(param_find("GRF_SENS_MODEL"), &_model_type); + + start(); + return PX4_OK; +} + +int GRFLaserSerial::measure() +{ + int32_t rate = (int32_t)_update_rate; + _data_output = 0x01; // raw distance (last return) + _stream_data = 5; // enable constant streaming + + // send packets to the sensor depending on the state + switch (_sensor_state) { + + case STATE_UNINIT: + // Used to probe if the sensor is alive + grf_send(GRF_PRODUCT_NAME, false, &_product_name[0], 0); + break; + + case STATE_ACK_PRODUCT_NAME: + // Update rate default to 5 readings/s + grf_send(GRF_UPDATE_RATE, true, &rate, sizeof(uint8_t)); + ScheduleDelayed(100_ms); + break; + + case STATE_ACK_UPDATE_RATE: + // Configure the data that the sensor shall output + grf_send(GRF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output)); + break; + + case STATE_ACK_DISTANCE_OUTPUT: + // Configure the sensor to automatically output data at the configured update rate + grf_send(GRF_STREAM, true, &_stream_data, sizeof(_stream_data)); + _sensor_state = STATE_SEND_STREAM; + break; + + default: + break; + } + + return PX4_OK; +} + +int GRFLaserSerial::collect() +{ + if (_sensor_state == STATE_UNINIT) { + + perf_begin(_sample_perf); + const int payload_length = 22; + + _crc_valid = false; + grf_get_and_handle_request(payload_length, GRF_PRODUCT_NAME); + + if (_crc_valid) { + _sensor_state = STATE_ACK_PRODUCT_NAME; + perf_end(_sample_perf); + return PX4_OK; + } + + perf_end(_sample_perf); + return -EAGAIN; + + } else if (_sensor_state == STATE_ACK_PRODUCT_NAME) { + + perf_begin(_sample_perf); + const int payload_length = 7; + + _crc_valid = false; + grf_get_and_handle_request(payload_length, GRF_UPDATE_RATE); + + if (_crc_valid) { + _sensor_state = STATE_ACK_UPDATE_RATE; + perf_end(_sample_perf); + return PX4_OK; + } + + perf_end(_sample_perf); + return -EAGAIN; + + } else if (_sensor_state == STATE_ACK_UPDATE_RATE) { + + perf_begin(_sample_perf); + const int payload_length = 10; + + _crc_valid = false; + grf_get_and_handle_request(payload_length, GRF_DISTANCE_OUTPUT); + + if (_crc_valid) { + _sensor_state = STATE_ACK_DISTANCE_OUTPUT; + perf_end(_sample_perf); + return PX4_OK; + } + + perf_end(_sample_perf); + return -EAGAIN; + + } else { + + // Stream data from sensor + perf_begin(_sample_perf); + const int payload_length = 10; + + _crc_valid = false; + grf_get_and_handle_request(payload_length, GRF_DISTANCE_DATA_CM); + + if (_crc_valid) { + grf_process_replies(); + perf_end(_sample_perf); + return PX4_OK; + } + + perf_end(_sample_perf); + return -EAGAIN; + } +} + +void GRFLaserSerial::start() +{ + /* 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(); + + /*Set Lidar Min/Max based on model*/ + switch (_model_type) { + case GRF250: { + _px4_rangefinder.set_min_distance(0.1f); + _px4_rangefinder.set_max_distance(250.0f); + break; + } + + case GRF500: { + _px4_rangefinder.set_min_distance(0.1f); + _px4_rangefinder.set_max_distance(500.0f); + break; + } + } + + /* schedule a cycle to start things */ + ScheduleNow(); +} + +void GRFLaserSerial::stop() +{ + ScheduleClear(); +} + +void GRFLaserSerial::Run() +{ + /* fds initialized? */ + if (_fd < 0) { + /* open fd: non-blocking read mode*/ + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (_fd < 0) { + PX4_ERR("serial open failed (%i)", errno); + return; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B115200; + + /* 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) { + if (hrt_absolute_time() - _last_received_time >= 1_s) { + start(); + return; + } + + /* 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; + } + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(_interval); +} + +void GRFLaserSerial::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +void GRFLaserSerial::grf_get_and_handle_request(const int payload_length, const GRF_SERIAL_CMD msg_id) +{ + // GRF 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.) + + int ret; + size_t max_read = sizeof(_linebuf) - _linebuf_size; + ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); + + 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; + } + + if (ret > 0) { + _last_received_time = hrt_absolute_time(); + _linebuf_size += ret; + } + + // Not enough data to parse a complete packet. Gather more data in the next cycle. + if (_linebuf_size < payload_length) { + return; + } + + int index = 0; + + while (index <= _linebuf_size - payload_length && _crc_valid == false) { + bool restart_flag = false; + + while (restart_flag != true) { + switch (_parsed_state) { + case GRF_PARSED_STATE::START: { + if (_linebuf[index] == 0xAA) { + // start of frame is valid, continue + _calc_crc = grf_format_crc(_calc_crc, _start_of_frame); + _parsed_state = GRF_PARSED_STATE::FLG_LOW; + break; + + } else { + _crc_valid = false; + _parsed_state = GRF_PARSED_STATE::START; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Start of packet not valid: %d", _sensor_state); + break; + } + } + + case GRF_PARSED_STATE::FLG_LOW: { + rx_field.flags_lo = _linebuf[index + 1]; + _calc_crc = grf_format_crc(_calc_crc, rx_field.flags_lo); + _parsed_state = GRF_PARSED_STATE::FLG_HIGH; + break; + } + + case GRF_PARSED_STATE::FLG_HIGH: { + rx_field.flags_hi = _linebuf[index + 2]; + rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); + _calc_crc = grf_format_crc(_calc_crc, rx_field.flags_hi); + + // Check payload length against known max value + if (rx_field.data_len > 17) { + _parsed_state = GRF_PARSED_STATE::START; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Payload length error: %d", _sensor_state); + break; + + } else { + _parsed_state = GRF_PARSED_STATE::ID; + break; + } + } + + case GRF_PARSED_STATE::ID: { + rx_field.msg_id = _linebuf[index + 3]; + + if (rx_field.msg_id == msg_id) { + _calc_crc = grf_format_crc(_calc_crc, rx_field.msg_id); + _parsed_state = GRF_PARSED_STATE::DATA; + break; + } + + // Ignore message ID's that aren't searched + else { + _parsed_state = GRF_PARSED_STATE::START; + _calc_crc = 0; + restart_flag = true; + PX4_DEBUG("Non needed message ID: %d", _sensor_state); + break; + } + } + + case GRF_PARSED_STATE::DATA: { + // 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 = grf_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); + _data_bytes_recv = _data_bytes_recv + 1; + + } + } + + else { + + _parsed_state = GRF_PARSED_STATE::CRC_LOW; + _data_bytes_recv = 0; + _calc_crc = grf_format_crc(_calc_crc, _data_bytes_recv); + } + + _parsed_state = GRF_PARSED_STATE::CRC_LOW; + _data_bytes_recv = 0; + break; + } + + case GRF_PARSED_STATE::CRC_LOW: { + rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len]; + _parsed_state = GRF_PARSED_STATE::CRC_HIGH; + break; + } + + case GRF_PARSED_STATE::CRC_HIGH: { + 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 grf 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 = GRF_PARSED_STATE::START; + _calc_crc = 0; + restart_flag = true; + break; + + } else { + + _crc_valid = false; + _parsed_state = GRF_PARSED_STATE::START; + _calc_crc = 0; + restart_flag = true; + perf_count(_comms_errors); + PX4_DEBUG("CRC mismatch: %d", _sensor_state); + break; + } + } + } + } + + 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 GRFLaserSerial::grf_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len) +{ + uint16_t crc_val = 0; + uint8_t packet_buff[GRF_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 = grf_format_crc(crc_val, _start_of_frame); + crc_val = grf_format_crc(crc_val, flags_lo); + crc_val = grf_format_crc(crc_val, flags_hi); + crc_val = grf_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 == GRF_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 = grf_format_crc(crc_val, data_convert); + data_inc = data_inc + 1; + data_convert = data[0] >> 8; + packet_buff[data_inc] = data_convert; + crc_val = grf_format_crc(crc_val, data_convert); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = grf_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = grf_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + } + + else if (msg_id == GRF_STREAM) { + packet_buff[data_inc] = data[0]; + //pad zeroes + crc_val = grf_format_crc(crc_val, data[0]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = grf_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = grf_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + packet_buff[data_inc] = 0; + crc_val = grf_format_crc(crc_val, packet_buff[data_inc]); + data_inc = data_inc + 1; + } + + else if (msg_id == GRF_UPDATE_RATE) { + // Update Rate + packet_buff[data_inc] = (uint8_t)data[0]; + // Add data bytes to crc add function + crc_val = grf_format_crc(crc_val, data[0]); + data_inc = data_inc + 1; + } + + else { + // Product Name + PX4_DEBUG("DEBUG: 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_DEBUG("DEBUG: Send byte: %d", packet_buff[i]); + } + + ret = ::write(_fd, packet_buff, packet_len); + + if (ret != packet_len) { + perf_count(_comms_errors); + PX4_ERR("serial write fail %d", ret); + // Flush data written, not transmitted + tcflush(_fd, TCOFLUSH); + } +} + +void GRFLaserSerial::grf_process_replies() +{ + float distance_m = -1.0f; + hrt_abstime now = hrt_absolute_time(); + + switch (rx_field.msg_id) { + case GRF_DISTANCE_DATA_CM: { + const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8) | (rx_field.data[2] << 16); + distance_m = raw_distance * GRF_SCALE_FACTOR; + _px4_rangefinder.update(now, distance_m); + break; + } + + default: + // add case for future use + break; + } +} + +uint16_t GRFLaserSerial::grf_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_grf_serial/lightware_grf_serial.hpp b/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.hpp new file mode 100755 index 0000000000..17c73c4d54 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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_grf_serial.hpp + * @author Aaron Porter + * + * Serial Protocol driver for the Lightware GRF rangefinder series + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "grf_commands.h" + +enum GRF_SERIAL_STATE { + STATE_UNINIT = 0, + STATE_ACK_PRODUCT_NAME = 1, + STATE_ACK_UPDATE_RATE = 2, + STATE_ACK_DISTANCE_OUTPUT = 3, + STATE_SEND_STREAM = 4, +}; + +enum GRF_PARSED_STATE { + START = 0, + FLG_LOW, + FLG_HIGH, + ID, + DATA, + CRC_LOW, + CRC_HIGH +}; + +enum MODEL { + Disable = 0, + GRF250 = 1, + GRF500 = 2 +}; + +using namespace time_literals; +class GRFLaserSerial : public px4::ScheduledWorkItem +{ +public: + GRFLaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + ~GRFLaserSerial() override; + + int init(); + void print_info(); + void grf_get_and_handle_request(const int payload_length, const GRF_SERIAL_CMD msg_id); + void grf_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); + uint16_t grf_format_crc(uint16_t crc, uint8_t data_value); + void grf_process_replies(); + +private: + + distance_sensor_s _distance{}; + static constexpr uint64_t GRF_MEAS_TIMEOUT{100_ms}; + static constexpr float GRF_SCALE_FACTOR = 0.1f; + + void start(); + void stop(); + void Run() override; + int measure(); + int collect(); + bool _crc_valid{false}; + + PX4Rangefinder _px4_rangefinder; + char _port[20] {}; + int _interval{200000}; + bool _collect_phase{false}; + int _fd{-1}; + uint8_t _linebuf[GRF_MAX_PAYLOAD] {}; + int _linebuf_size{0}; + + // GRF uses a binary protocol to include header,flags + // message ID, payload, and checksum + GRF_SERIAL_STATE _sensor_state{STATE_UNINIT}; + int _baud_rate{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 _model_type{0}; + int32_t _data_output{0}; + const uint8_t _start_of_frame{0xAA}; + uint16_t _data_bytes_recv{0}; + uint8_t _parsed_state{0}; + uint16_t _calc_crc{0}; + + // end of GRF data members + + hrt_abstime _last_received_time{0}; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; +}; diff --git a/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial_main.cpp b/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial_main.cpp new file mode 100755 index 0000000000..08e912e2c8 --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial_main.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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_grf_serial.hpp" + +#include +#include + +namespace lightware_grf +{ + +GRFLaserSerial *g_dev{nullptr}; + +static int start(const char *port) +{ + 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 GRFLaserSerial(port); + + 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 GRF Laser rangefinder. + +### Configuration +https://docs.px4.io/main/en/sensor/grf_lidar + +### Parameters +https://docs.px4.io/main/en/advanced_config/parameter_reference#GRF_SENS_MODEL +https://docs.px4.io/main/en/advanced_config/parameter_reference#GRF_RATE_CFG +https://docs.px4.io/main/en/advanced_config/parameter_reference#SENS_EN_GRF_CFG + +### Examples + +Attempt to start driver on a specified serial device. +$ lightware_grf_serial start -d /dev/ttyS1 +Stop driver +$ lightware_grf_serial stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("lightware_grf_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_COMMAND_DESCR("stop", "Stop driver"); + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int lightware_grf_serial_main(int argc, char *argv[]) +{ + const char *device_path = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_path = myoptarg; + break; + + default: + lightware_grf::usage(); + return -1; + } + } + + if (myoptind >= argc) { + lightware_grf::usage(); + return -1; + } + + if (!strcmp(argv[myoptind], "start")) { + return lightware_grf::start(device_path); + + } else if (!strcmp(argv[myoptind], "stop")) { + return lightware_grf::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return lightware_grf::status(); + } + + lightware_grf::usage(); + return -1; +} diff --git a/src/drivers/distance_sensor/lightware_grf_serial/module.yaml b/src/drivers/distance_sensor/lightware_grf_serial/module.yaml new file mode 100755 index 0000000000..7765f9802c --- /dev/null +++ b/src/drivers/distance_sensor/lightware_grf_serial/module.yaml @@ -0,0 +1,44 @@ +module_name: Lightware GRF Rangefinder (serial) +serial_config: + - command: lightware_grf_serial start -d ${SERIAL_DEV} + port_config_param: + name: SENS_EN_GRF_CFG + group: Sensors + num_instances: 1 + supports_networking: false + +parameters: + - group: Sensors + definitions: + GRF_RATE_CFG: + description: + short: Lightware GRF lidar update rate. + long: | + The Lightware GRF distance sensor can increase the update rate to enable greater resolution. + type: enum + values: + 1: 1 Hz + 2: 2 Hz + 3: 4 Hz + 4: 5 Hz + 5: 10 Hz + 6: 20 Hz + 7: 30 Hz + 8: 40 Hz + 9: 50 Hz + reboot_required: true + num_instances: 1 + default: 4 + GRF_SENS_MODEL: + description: + short: GRF Sensor model + long: | + GRF Sensor Model used to distinush between the GRF250 and GRF500 since both have different max distance range. + type: enum + values: + 0: disable + 1: GRF250 + 2: GRF500 + reboot_required: true + num_instances: 1 + default: 0