Skip to content

Commit

Permalink
added info and scripts for obstacle_distance debugging with plotjuggler
Browse files Browse the repository at this point in the history
  • Loading branch information
Claudio-Chies authored and hamishwillee committed Dec 11, 2024
1 parent c790496 commit 43c85a9
Show file tree
Hide file tree
Showing 2 changed files with 44 additions and 1 deletion.
Binary file not shown.
45 changes: 44 additions & 1 deletion en/sensor/sf45_rotating_lidar.md
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,53 @@ The [sensor driver](../modules/modules_driver_distance_sensor.md#lightware-sf45-
The measurements in each sector will correspond to the lowest measurement the sensor had in that corresponding sector.
The data is then published to the [OBSTACLE_DISTANCE](https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE) MAVLink message.

## Debugging / Common problems
## Debugging

### Common problems

Start-up issues, jerky movements, or a lot of coms errors displayed with `lightware_sf45_serial status` could mean that the sensor is not getting enough power.

According to its datasheet, the sensor needs 300 mA of current at 5V.
The supplied cable is fairly long and when connected to the 5V supply from the FMU like the `TELEM2` port, the voltage at the rangefinder may have dropped below the required level.
One mitigation/alternative is to power the SF45 via a separate step-down converter from battery voltage, ensuring that there are 5V across the rangefinder itself.

### Debugging with PlotJuggler

Below are two reactive scripts for Plotjuggler, with which you can plot the [OBSTACLE_DISTANCE](https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE) messaage in an xy plot.
which greatly helps debugging as you directly see the orientation of the measurement in respect of the drone.

<video src="../../assets/hardware/sensors/lidar_lightware/sf45_plotjuggler.mp4" width="720" controls></video>

For this you need to add follwing Reactive Scripts to Plotjuggler:

**Global code, executed once:**
```
obs_dist_xy = ScatterXY.new("obstacle_distance_xy")
```
**function(tracker_time)**
```
obs_dist_xy:clear()
i = 0
angle_offset = TimeseriesView.find("obstacle_distance/angle_offset")
increment = TimeseriesView.find("obstacle_distance/increment")
while(true) do
str = string.format("obstacle_distance/distances.%02d", i)
distance = TimeseriesView.find( str )
if distance == nil then break end
angle = angle_offset:atTime(tracker_time) + i * increment:atTime(tracker_time)
y = distance:atTime(tracker_time) * math.cos(math.rad(angle))
x = distance:atTime(tracker_time) * math.sin(math.rad(angle))
obs_dist_xy:push_back(x, y)
i = i + 1
end
```
if you then save it, and load new data, you will see a new timeseries called `obstacle_distance_xy`, which is what you see above.
to also see the fused obstacle_distance, you can just adapt the script to work on the `obstacle_distance_fused` message

0 comments on commit 43c85a9

Please sign in to comment.