mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Adding Light Ware GRF Serial Driver (#26404)
* Creating a base for grf lidar * Serial Drive is working, need to work out distance publish * WIP Getting Range Data in cm * Working Rand Distance Values for GRF 250 and GRF500 * Review Changes * Compiler fixes * Update to date * small update * Fix typo and remover unused libs * removing unused enum * Update to the Documentation * Fiving scaling issue * update to the logic * [Feature] Adding I2C driver for the GRF250 and GRF500 models (#26425) * Adding the GRF I2C driver * I2C Driver Working * Removing a lot of unnecessary code * fixing names * Changing the i2c Driver to be in the lightware laser * remove the old driver * formatting fix * Adding Ligthware GRF to documentation * Update to the Documentation * Ensuring sample_perf ends * Updating Docs * uavcannode: implement hardpoint commands (#26334) * implement cannode hardpoint commands Signed-off-by: dirksavage88 <dirksavage88@gmail.com> * Update src/drivers/uavcannode/Subscribers/HardpointCommand.hpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * Update src/drivers/uavcannode/Subscribers/HardpointCommand.hpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * add hardpoint sub to ark cannode, simplify handling of hardpoint broadcast Signed-off-by: dirksavage88 <dirksavage88@gmail.com> --------- Signed-off-by: dirksavage88 <dirksavage88@gmail.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * voxl_esc: Limit frequency of UART passthru writes to 20Hz * voxl2_io: Added UART passthru * docs: update link for px4 ros2 interface lib python api docs * estimator_interface: remove unused getter * gnss_checks: always run strict checks on ground With the goal to never take off if the GNSS solution is not fullfilling the configured requirements still not stopping to use it in case it degrades mid air. * ekf2 unit-tests: adapt to strict GNSS checks on ground * escCheck: rework online check to properly report offline ESCs previous to this 09d79b221f274523349a029e63ab4462e41d0c1c set `esc_online_flags` e.g. for UAVCAN ESCs which specific one is online and that then got compared to a mask where the first `esc_count` bits were set. So if only ESC 5 is mapped and online you get the message "ESC 156 offline" because `esc_online_flags = 0b1000` gets compared to `online_bitmask = 0b1` based on `esc_count = 1` and the motor index is `esc[0].actuator_function = 0` wrapped using `0 - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1 = 156`. * FailureDetector: consistent timestamp naming * FailureDetector: rework motor status check * FailureDetector: implement upper and lower current limit with offset * Update src/modules/commander/failure_detector/FailureDetector.cpp Prevent Buffer overflow * Update Format * Subedit * Shrink and rename image * Apply suggestion from @hamishwillee Sounds good Co-authored-by: Hamish Willee <hamishwillee@gmail.com> * Apply suggestion from @hamishwillee More universal approach Co-authored-by: Hamish Willee <hamishwillee@gmail.com> * Update to the Documentation * FailureDetector: rework motor status check * FailureDetector: implement upper and lower current limit with offset * Subedit * docs: update parameter reference metadata * Remove pregenerated files - that should all be tidied up next time this runs * remover GRF parameters * Documentation updates * Fixing Merge Conflicts * remove @ * Undo Changes to parameter_reference * remove the code that will be autogen-ed * Update the Camake File --------- Signed-off-by: dirksavage88 <dirksavage88@gmail.com> Co-authored-by: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Co-authored-by: Eric Katzfey <eric.katzfey@modalai.com> Co-authored-by: Beat Küng <beat-kueng@gmx.net> Co-authored-by: Matthias Grob <maetugr@gmail.com> Co-authored-by: Marco Hauswirth <marco.hauswirth@auterion.com> Co-authored-by: Nick <145654544+ttechnick@users.noreply.github.com> Co-authored-by: Hamish Willee <hamishwillee@gmail.com> Co-authored-by: Ramon Roche <mrpollo@gmail.com>
This commit is contained in:
Aaron1356
GitHub
parent
08dc2a776e
commit
066e8f7fea
@ -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
|
||||
|
||||
BIN
docs/assets/hardware/sensors/lidar_lightware/grf_500.png
Normal file
BIN
docs/assets/hardware/sensors/lidar_lightware/grf_500.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 108 KiB |
@ -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)
|
||||
|
||||
68
docs/en/sensor/grf_lidar.md
Normal file
68
docs/en/sensor/grf_lidar.md
Normal file
@ -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.
|
||||
|
||||
|
||||
|
||||
::: 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 |
|
||||
| -------------------------------------------------------------------------------------------------------- | -------------------------------------------------------- |
|
||||
| <a id="SENS_EN_GRF_CFG"></a>[SENS_EN_GRF_CFG](../advanced_config/parameter_reference.md#SENS_EN_GRF_CFG) | Set to the serial port the sensor is connected to. |
|
||||
| <a id="GRF_UPDATE_CFG"></a>[GRF_UPDATE_CFG](../advanced_config/parameter_reference.md#GRF_UPDATE_CFG) | Set the update rate. |
|
||||
| <a id="GRF_SENS_MODEL"></a>[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)
|
||||
@ -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
|
||||
|
||||
|
||||
46
src/drivers/distance_sensor/lightware_grf_serial/CMakeLists.txt
Executable file
46
src/drivers/distance_sensor/lightware_grf_serial/CMakeLists.txt
Executable file
@ -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
|
||||
)
|
||||
5
src/drivers/distance_sensor/lightware_grf_serial/Kconfig
Executable file
5
src/drivers/distance_sensor/lightware_grf_serial/Kconfig
Executable file
@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_GRF_SERIAL
|
||||
bool "lightware_grf_serial"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lightware_grf_serial
|
||||
91
src/drivers/distance_sensor/lightware_grf_serial/grf_commands.h
Executable file
91
src/drivers/distance_sensor/lightware_grf_serial/grf_commands.h
Executable file
@ -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;
|
||||
633
src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.cpp
Executable file
633
src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.cpp
Executable file
@ -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 <fcntl.h>
|
||||
#include <float.h>
|
||||
#include <inttypes.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <lib/crc/crc.h>
|
||||
|
||||
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;
|
||||
}
|
||||
134
src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.hpp
Executable file
134
src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial.hpp
Executable file
@ -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 <aaron.porter@ascendengineer.com>
|
||||
*
|
||||
* Serial Protocol driver for the Lightware GRF rangefinder series
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
|
||||
#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;
|
||||
};
|
||||
167
src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial_main.cpp
Executable file
167
src/drivers/distance_sensor/lightware_grf_serial/lightware_grf_serial_main.cpp
Executable file
@ -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 <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
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;
|
||||
}
|
||||
44
src/drivers/distance_sensor/lightware_grf_serial/module.yaml
Executable file
44
src/drivers/distance_sensor/lightware_grf_serial/module.yaml
Executable file
@ -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
|
||||
Loading…
x
Reference in New Issue
Block a user