Skip to content

Commit

Permalink
added sf45 as a seperate rangefinder
Browse files Browse the repository at this point in the history
  • Loading branch information
Claudio-Chies committed Nov 11, 2024
1 parent 1076591 commit 4376433
Show file tree
Hide file tree
Showing 6 changed files with 64 additions and 36 deletions.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 1 addition & 0 deletions en/SUMMARY.md
Original file line number Diff line number Diff line change
Expand Up @@ -238,6 +238,7 @@
- [Barometers](sensor/barometer.md)
- [Distance Sensors \(Rangefinders\)](sensor/rangefinders.md)
- [Lightware Lidars (SF/LW)](sensor/sfxx_lidar.md)
- [Lightware SF45/B rotary Lidar](sensor/sf45_rotating_lidar.md)
- [Ainstein US-D1 Standard Radar Altimeter](sensor/ulanding_radar.md)
- [LeddarOne Lidar](sensor/leddar_one.md)
- [Benewake TFmini Lidar](sensor/tfmini.md)
Expand Down
42 changes: 10 additions & 32 deletions en/computer_vision/collision_prevention.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ _Collision Prevention_ may be used to automatically slow and stop a vehicle befo

It can be enabled for multicopter vehicles in [Position mode](../flight_modes_mc/position.md), and can use sensor data from an offboard companion computer, offboard rangefinders over MAVLink, a rangefinder attached to the flight controller, or any combination of the above.

Collision prevention may restrict vehicle maximum speed if the sensor range isn't large enough!
Collision prevention may restrict vehicle maximum speed if the sensor range isnt large enough!
It also prevents motion in directions where no sensor data is available (i.e. if you have no rear-sensor data, you will not be able to fly backwards).

:::tip
Expand All @@ -28,7 +28,7 @@ If multiple sources supply data for the _same_ orientation, the system uses the

The vehicle restricts the maximum velocity in order to slow down as it gets closer to obstacles, and will stop movement when it reaches the minimum allowed separation.
In order to move away from (or parallel to) an obstacle, the user must command the vehicle to move toward a setpoint that does not bring the vehicle closer to the obstacle.
The algorithm will make minor adjustments to the setpoint direction if it is determined that a "better" setpoint exists within a fixed margin on either side of the requested setpoint.
The algorithm will make minor adjustments to the setpoint direction if it is determined that a better setpoint exists within a fixed margin on either side of the requested setpoint.

Users are notified through _QGroundControl_ while _Collision Prevention_ is actively controlling velocity setpoints.

Expand Down Expand Up @@ -66,8 +66,8 @@ If you wish to move freely into directions without sensor coverage, this can be
Delay, both in the vehicle tracking velocity setpoints and in receiving sensor data from external sources, is conservatively estimated via the [CP_DELAY](#CP_DELAY) parameter.
This should be [tuned](#delay_tuning) to the specific vehicle.

If the sectors adjacent to the commanded sectors are 'better' by a significant margin, the direction of the requested input can be modified by up to the angle specified in [CP_GUIDE_ANG](#CP_GUIDE_ANG).
This helps to fine-tune user input to 'guide' the vehicle around obstacles rather than getting stuck against them.
If the sectors adjacent to the commanded sectors are better by a significant margin, the direction of the requested input can be modified by up to the angle specified in [CP_GUIDE_ANG](#CP_GUIDE_ANG).
This helps to fine-tune user input to guide the vehicle around obstacles rather than getting stuck against them.

### Range Data Loss

Expand Down Expand Up @@ -106,51 +106,29 @@ Depending on the vehicle, type of environment and pilot skill different amounts
Setting the [CP_GUIDE_ANG](#CP_GUIDE_ANG) parameter to 0 will disable the guidance, resulting in the vehicle only moving exactly in the directions commanded.
Increasing this parameter will let the vehicle choose optimal directions to avoid obstacles, making it easier to fly through tight gaps and to keep the minimum distance exactly while going around objects.

If this parameter is too small the vehicle may feel 'stuck' when close to obstacles, because only movement away from obstacles at minimum distance are allowed.
If the parameter is too large the vehicle may feel like it 'slides' away from obstacles in directions not commanded by the operator.
If this parameter is too small the vehicle may feel stuck when close to obstacles, because only movement away from obstacles at minimum distance are allowed.
If the parameter is too large the vehicle may feel like it slides away from obstacles in directions not commanded by the operator.
From testing, 30 degrees is a good balance, although different vehicles may have different requirements.

::: info
The guidance feature will never direct the vehicle in a direction without sensor data.
If the vehicle feels 'stuck' with only a single distance sensor pointing forwards, this is probably because the guidance cannot safely adapt the direction due to lack of information.
If the vehicle feels stuck with only a single distance sensor pointing forwards, this is probably because the guidance cannot safely adapt the direction due to lack of information.
:::

## PX4 Distance Sensor {#rangefinder}

### Lanbao PSK-CM8JL65-CC5
### Lanbao PSK-CM8JL65-CC5 [EOL]

At time of writing PX4 allows you to use the [Lanbao PSK-CM8JL65-CC5](../sensor/cm8jl65_ir_distance_sensor.md) IR distance sensor for collision prevention "out of the box", with minimal additional configuration:
At time of writing PX4 allows you to use the [Lanbao PSK-CM8JL65-CC5](../sensor/cm8jl65_ir_distance_sensor.md) IR distance sensor for collision prevention out of the box, with minimal additional configuration:

- First [attach and configure the sensor](../sensor/cm8jl65_ir_distance_sensor.md), and enable collision prevention (as described above, using [CP_DIST](#CP_DIST)).
- Set the sensor orientation using [SENS_CM8JL65_R_0](../advanced_config/parameter_reference.md#SENS_CM8JL65_R_0).

### LightWare LiDAR SF45 Rotating Lidar

PX4 v1.14 (and later) supports the [LightWare LiDAR SF45](https://www.lightwarelidar.com/shop/sf45-b-50-m/) rotating lidar which provides 320 degree sensing.
PX4 v1.14 (and later) supports the [LightWare LiDAR SF45](../sensor/sf45_rotating_lidar.md) rotating lidar which provides 320 degree sensing.

The SF45 must be connected via a UART/serial port and configured as described below (In addition to the [collision prevention setup](#px4-software-setup)).

[LightWare Studio](https://www.lightwarelidar.com/resources-software) configuration:

- In the LightWare Studio app enable scanning, set the scan angle, and change the baud rate to `921600`.

PX4 Configuration:

- Add the [lightware_sf45_serial](../modules/modules_driver_distance_sensor.md#lightware-sf45-serial) driver in [menuconfig](../hardware/porting_guide_config.md#px4-menuconfig-setup):
- Under **drivers > Distance sensors** select `lightware_sf45_serial`.
- Recompile and upload to the flight controller.
- [Set the following parameters](../advanced_config/parameters.md) via QGC:
- [SENS_EN_SF45_CFG](../advanced_config/parameter_reference.md#SENS_EN_SF45_CFG): Set to the serial port you have the sensor connected to.
Make sure GPS or Telemetry are not enabled on this port.
- [SF45_ORIENT_CFG](../advanced_config/parameter_reference.md#SF45_ORIENT_CFG): Set the orientation of the sensor (facing up or down)
- [SF45_UPDATE_CFG](../advanced_config/parameter_reference.md#SF45_UPDATE_CFG): Set the update rate
- [SF45_YAW_CFG](../advanced_config/parameter_reference.md#SF45_YAW_CFG): Set the yaw orientation

In QGroundControl you should see an [OBSTACLE_DISTANCE](https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE) message in the [MAVLink console](../debug/mavlink_shell.md#qgroundcontrol-mavlink-console) if collision prevention is configured correctly and active.

The obstacle overlay in QGC will look like this:

![sf45](../../assets/sf45/sf45_obstacle_map.png)

### Rangefinder Support

Expand Down
5 changes: 4 additions & 1 deletion en/sensor/cm8jl65_ir_distance_sensor.md
Original file line number Diff line number Diff line change
@@ -1,4 +1,7 @@
# Lanbao PSK-CM8JL65-CC5 ToF Infrared Distance Measuring Sensor
# Lanbao PSK-CM8JL65-CC5 ToF Infrared Distance Measuring Sensor [EOL]

:::warning
The Lanbao Lidar is End of Life, and not manufactured anymore

The [Lanbao PSK-CM8JL65-CC5](https://www.seeedstudio.com/PSK-CM8JL65-CC5-Infrared-Distance-Measuring-Sensor-p-4028.html) is a very small IR distance sensor with a 0.17m-8m range and millimeter resolution.
It must be connected to a UART/serial bus.
Expand Down
6 changes: 3 additions & 3 deletions en/sensor/rangefinders.md
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,14 @@ The rangefinders are enabled using the parameter [SENS_EN_MB12XX](../advanced_co

### Lightware LIDARs

[Lightware SFxx Lidar](../sensor/sfxx_lidar.md) provide a broad range of lightweight "laser altimeters" that are suitable for many drone applications.
[Lightware SFxx Lidar](../sensor/sfxx_lidar.md) provide a broad range of lightweight laser altimeters that are suitable for many drone applications.

PX4 supports: SF11/c and SF/LW20.
PX4 can also be used with the following discontinued models: SF02, SF10/a, SF10/b, SF10/c.

Others may be supported via the [RaccoonLab Cyphal and DroneCAN Rangefinder Adapter](#raccoonlab-cyphal-and-dronecan-rangefinder-adapter) described below.

PX4 also supports the [LightWare LiDAR SF45 Rotating Lidar](https://www.lightwarelidar.com/shop/sf45-b-50-m/) for [collision prevention](../computer_vision/collision_prevention.md#lightware-lidar-sf45-rotating-lidar) applications.
PX4 also supports the [LightWare LiDAR SF45 Rotating Lidar](../sensor/sf45_rotating_lidar.md) for [collision prevention](../computer_vision/collision_prevention.md) applications.

### TeraRanger Rangefinders

Expand All @@ -67,7 +67,7 @@ It has a sensing range from 1cm to 40m and needs to be connected to a UART/seria

The [Benewake TFmini Lidar](../sensor/tfmini.md) is a tiny, low cost, and low power LIDAR with 12m range.

### PSK-CM8JL65-CC5
### PSK-CM8JL65-CC5 [EOL]

The [Lanbao PSK-CM8JL65-CC5 ToF Infrared Distance Measuring Sensor](../sensor/cm8jl65_ir_distance_sensor.md) is a very small (38 mm x 18mm x 7mm, <10g) IR distance sensor with a 0.17m-8m range and millimeter resolution.
It must be connected to a UART/serial bus.
Expand Down
46 changes: 46 additions & 0 deletions en/sensor/sf45_rotating_lidar.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
# LightWare SF45/B (50m) rotating Lidar


![LightWare SF45 rotating Lidar](../../assets/hardware/sensors/lidar_lightware/sf45.png)


## Basics

This sensor is implemented in PX4 so that it published s [ObstacleDistance UORB](../msg_docs/ObstacleDistance.md) Message which then can be used for [Collision Prevention](../computer_vision/collision_prevention.md).

The measurements in each sector will correspond to the lowest measurement the sensor had in that corresponding sector.

## LightWare Studio Setup

In the [LightWare Studio](https://www.lightwarelidar.com/resources-software) app Set following settings:
| Parameter| Description|
| -------- | ---------- |
| Baud rate| 921600|

Then also make sure the scan angles are set so that nothing on the drone interferes with the measurements.
The Driver and Collision Prevention automatically handle angles different from the maximum angles.

## PX4 Setup:
:::info
The lidar driver is not included in the default build. make sure to add it when flashing PX4
### Add the driver to the PX4 build

for this add the [lightware_sf45_serial](../modules/modules_driver_distance_sensor.md#lightware-sf45-serial) driver in [menuconfig](../hardware/porting_guide_config.md#px4-menuconfig-setup):
1. Under **drivers > Distance sensors** select `lightware_sf45_serial`.
2. Recompile and upload to the flight controller.

### Set the following parameters via QGC:


| Parameter| Description|
| -------- | ---------- |
| <a id="SENS_EN_SF45_CFG"></a>[SENS_EN_SF45_CFG](../advanced_config/parameter_reference.md#SENS_EN_SF45_CFG) | Set to the serial port you have the sensor connected to. |
| <a id="SF45_ORIENT_CFG"></a>[SF45_ORIENT_CFG](../advanced_config/parameter_reference.md#SF45_ORIENT_CFG) | Set the orientation of the sensor (facing up or down) |
| <a id="SF45_UPDATE_CFG"></a>[SF45_UPDATE_CFG](../advanced_config/parameter_reference.md#SF45_UPDATE_CFG) | Set the update rate |
| <a id="SF45_YAW_CFG"></a>[SF45_YAW_CFG](../advanced_config/parameter_reference.md#SF45_YAW_CFG) | Set the yaw orientation |

In QGroundControl you should see an [OBSTACLE_DISTANCE](https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE)

The obstacle overlay in QGC will look like this:

![sf45](../../assets/sf45/sf45_obstacle_map.png)

0 comments on commit 4376433

Please sign in to comment.