mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 22:32:11 +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
09d79b221f
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
@@ -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
|
||||
|
||||
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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_GRF_SERIAL
|
||||
bool "lightware_grf_serial"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lightware_grf_serial
|
||||
@@ -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;
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user