K. Isom
356218e8a2
DearPyGui app that visualizes the Multi-ranger ToF sensors and builds a top-down map of the drone's surroundings. Handheld and experimental hardened-scan capture, Flow-deck drift correction (ZUPT), props-off dry-run mode, and optional 3D point-cloud streaming to CloudView. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
212 lines
12 KiB
Markdown
212 lines
12 KiB
Markdown
# Crazyflie RangeView
|
||
|
||
A small [DearPyGui](https://github.com/hoffstadt/DearPyGui) tool (same GUI
|
||
binding as the test-stand and mission clients) that visualises the
|
||
**Multi-ranger** deck's five Time-of-Flight sensors on a Bitcraze **Crazyflie**
|
||
and builds a top-down **map of the drone's immediate surroundings**.
|
||
|
||
It has two independent parts:
|
||
|
||
1. **Mapping** - each ToF distance is projected through the drone's position
|
||
estimate (Flow v2 deck) into a world-frame point cloud. This needs only
|
||
pose + range streaming, so it works **whether the drone is flying or you are
|
||
holding it in your hand and walking it around**.
|
||
2. **Flight (automated scan)** - an optional capture profile that flies the
|
||
drone itself: arm, lift off, then **ascend slowly while rotating in place** so
|
||
the sensors sweep a full circle hands-free.
|
||
|
||
```
|
||
┌─────────────────────────────┬──────────────────────────────────────┐
|
||
│ Connection [radio...][Scan][⏻] │ Map (top-down) extent +/- 3m │
|
||
│ ─────────────────────────── │ ┌────────────────────────────────┐ │
|
||
│ Status: Connected - ready │ │ · ·· ··· │ │
|
||
│ Battery 3.9V Height 0.00m │ │ · · │ │
|
||
│ Multi-ranger attached │ │ · ╲front / · │ │
|
||
│ Flow v2 attached │ │ · left●--●--●right · │ │
|
||
│ Pos x+0.0 y+0.0 z+0.0 Yaw 0 │ │ · (drone) · │ │
|
||
│ ─────────────────────────── │ │ · · │ │
|
||
│ ToF (m) F:1.8 B:2.1 L:0.9 R... │ │ ··· ·· · │ │
|
||
│ Up:2.4 Down:0.03 │ └────────────────────────────────┘ │
|
||
│ ─────────────────────────── │ │
|
||
│ Mapping [✓ Record] │ │
|
||
│ [Reset origin] [Clear map] │ │
|
||
│ Points: 1843 │ │
|
||
│ ─────────────────────────── │ │
|
||
│ Flight (automated scan) │ │
|
||
│ Start 0.40m Top 1.50m │ │
|
||
│ Climb 0.15m/s Yaw 45°/s │ │
|
||
│ [ ARM -> SCAN -> LAND ] │ │
|
||
│ [Disarm] [EMERGENCY (SPACE)] │ │
|
||
│ ─────────────────────────── │ │
|
||
│ Console ... │ │
|
||
└─────────────────────────────┴──────────────────────────────────────┘
|
||
```
|
||
|
||
## Hardware
|
||
|
||
- Crazyflie (brushless or standard) with current firmware.
|
||
- **Flow v2 deck** - supplies the position/altitude estimate the map is plotted
|
||
against, and is required to arm.
|
||
- **Multi-ranger deck** - the five VL53L1x ToF sensors (front / back / left /
|
||
right / up). The Flow deck's down-facing z-ranger provides the floor distance.
|
||
|
||
## Install & run
|
||
|
||
```sh
|
||
# with uv
|
||
uv run main.py
|
||
|
||
# or a plain venv
|
||
python -m venv .venv && . .venv/bin/activate
|
||
pip install -r requirements.txt
|
||
python -m rangeview # or: python main.py
|
||
|
||
# custom radio URI
|
||
CFLIB_URI=radio://0/80/2M/E7E7E7E7E7 python -m rangeview
|
||
```
|
||
|
||
## How the map is built
|
||
|
||
The Multi-ranger reports a distance per sensor in millimetres. Each horizontal
|
||
sensor points along a fixed body axis (front `+x`, back `−x`, left `+y`, right
|
||
`−y`). For every sample, RangeView rotates that body direction by the drone's
|
||
estimated yaw and offsets it from the estimated `(x, y)` position, giving the
|
||
world-frame coordinate of the obstacle the beam hit:
|
||
|
||
```
|
||
yaw = radians(stabilizer.yaw)
|
||
wx = cos(yaw)·bx − sin(yaw)·by # body dir -> world dir
|
||
wy = sin(yaw)·bx + cos(yaw)·by
|
||
point = (stateEstimate.x + wx·d, stateEstimate.y + wy·d)
|
||
```
|
||
|
||
Points accumulate into the cloud while **Record** is on, but only once the drone
|
||
has actually moved (≥2 cm, ≥5 cm in height, or ≥2°), so a stationary drone never
|
||
dumps noise into the map. Readings at or beyond the sensor's ~4 m ceiling are
|
||
treated as "nothing there" and skipped, so empty directions don't pollute the
|
||
map. The world origin is wherever the estimator was last reset.
|
||
|
||
The in-app map is the 2D top-down **horizontal slice** (the four side sensors) —
|
||
a quick live diagnostic. The points are world-anchored: each is placed using the
|
||
drone's estimated pose *at the moment it was measured*, so the cloud stays fixed
|
||
in space while the drone marker moves through it (accuracy is bound by the
|
||
position estimate — drift smears it).
|
||
|
||
## 3D view (CloudView)
|
||
|
||
For a real 3D point cloud, RangeView streams the **full cloud** — horizontal
|
||
hits at the drone's height, plus ceiling/floor points from the up/down sensors —
|
||
to the standalone **[CloudView](../cloudview)** viewer over a socket. Tick
|
||
**Stream 3D to** and give the viewer's address (default
|
||
`tcp://127.0.0.1:9870`; `unix:///path` also works). Start the viewer with
|
||
`python -m cloudview` in the `cloudview/` project first, or any time after — the
|
||
publisher reconnects and replays the whole cloud, and the stream is best-effort
|
||
so it never blocks flight. The 2D map keeps working regardless; CloudView is a
|
||
generic viewer that other robots can feed too (see its README for the protocol).
|
||
|
||
## Drift correction
|
||
|
||
A Crazyflie with a Flow v2 deck **drifts even when it is sitting perfectly
|
||
still** - there's no fix in the firmware estimator for this. Two things cause it:
|
||
|
||
- The firmware Kalman filter integrates Flow-deck velocity continuously from the
|
||
moment it powers on. With nothing to correct it, a freshly-connected drone can
|
||
report a position **tens of metres** from where it actually is (on the bench
|
||
here it read +78.9 m, −33.7 m). RangeView therefore **resets the estimator
|
||
automatically on connect** so you always start from a clean `(0, 0)` origin -
|
||
and you can re-zero any time with **Reset origin**.
|
||
- Even after a reset, optical-flow noise keeps integrating into position and gyro
|
||
bias into yaw, so the estimate slowly wanders (~a few mm per second at rest).
|
||
|
||
To cancel the residual wander, RangeView applies a **zero-velocity update
|
||
(ZUPT)**, toggled by the **Drift correction (hold pose when still)** checkbox
|
||
(on by default). It detects "at rest" from the *raw* gyro rate and optical-flow
|
||
counts - signals that read ~0 when the drone is genuinely still, independent of
|
||
the drifting estimate - and while at rest it pins the corrected pose in place,
|
||
folding the wander into an offset instead of letting it move the map. When you
|
||
pick the drone up and move it, real motion passes straight through; when you set
|
||
it down or pause, the drift is re-absorbed. The **Motion** line shows the current
|
||
state and how much drift is being held off. Measured on the bench: over 15 s
|
||
stationary the raw estimate drifted ~0.4 cm while the corrected pose held at
|
||
0.0 cm.
|
||
|
||
### Workflow A - handheld mapping (no flight)
|
||
|
||
1. Connect.
|
||
2. Place/hold the drone where you want `(0, 0)` and press **Reset origin** (this
|
||
resets the Kalman estimator and clears the map).
|
||
3. Make sure **Record** is checked, then slowly carry the drone through the space
|
||
- keep it roughly level so the Flow deck tracks well. Walls fill in live.
|
||
|
||
> The Flow v2 estimate drifts over long, fast, or feature-poor moves (it needs
|
||
> texture under the down-facing camera and a return for the z-ranger). Move
|
||
> smoothly and reset the origin if the map starts to smear.
|
||
|
||
### Workflow B - automated scan (EXPERIMENTAL)
|
||
|
||
> **A continuously-rotating ascent on a Flow deck caused a flyaway** (the drone
|
||
> bounced off the walls, ceiling, and floor). A Flow deck cannot hold position
|
||
> while yaw-rotating: spinning the optical-flow sensor injects phantom motion,
|
||
> the firmware Kalman estimate diverges, and the position controller chases it
|
||
> into obstacles. The client-side **drift correction only fixes the displayed
|
||
> pose - the firmware flies on its own (diverging) estimate**, so it does *not*
|
||
> make flight safer.
|
||
>
|
||
> The scan was therefore redesigned to be as gentle as possible, but rotation on
|
||
> a Flow deck remains inherently unstable. **Bench-test with the props off,
|
||
> then tethered, before any free flight, and keep the area clear and SPACE
|
||
> (emergency stop) ready.**
|
||
|
||
The hardened profile never spins continuously:
|
||
|
||
1. Set **Start height**, **Top height**, **Climb rate**, and **Yaw rate** (the
|
||
rotation speed of each step, hard-capped at 30 °/s). Defaults are conservative.
|
||
2. Place the drone level on the floor with clear space all around.
|
||
3. Press **ARM**, then **SCAN (!)**. The drone:
|
||
- takes off with the firmware **high-level commander** (which holds position
|
||
autonomously, so a slow UI can't drop it) and **settles into a stable hover**
|
||
before doing anything else;
|
||
- rotates one full turn in discrete **~30° "stop-and-stare" steps**, pausing at
|
||
each bearing so the estimate re-converges and the sensors take a clean
|
||
static reading;
|
||
- climbs to the next ring (~0.3 m up) and repeats, up to the top height;
|
||
- lands automatically when done.
|
||
|
||
It **aborts to a controlled landing** if the estimate starts to diverge (any
|
||
sustained horizontal speed while it's supposed to be holding still), if a wall
|
||
comes within 25 cm, or if it loses the flying state, and **emergency-stops** on a
|
||
tumble/crash. The climb is also **capped below the ceiling** sensed by the
|
||
up-facing ToF. Press **LAND** any time to bring it down early; **SPACE** cuts the
|
||
motors instantly.
|
||
|
||
### Dry run (props off, no motors)
|
||
|
||
Tick **Dry run (props off ...)** before arming to validate the whole sequence on
|
||
the bench with **zero flight risk**. In dry run the controller walks the entire
|
||
scan state machine - takeoff, every stop-and-stare go-to bearing, ring climbs,
|
||
and landing - and logs each decision to the console (`[DRY RUN] ...`), but it
|
||
**never arms the motors and never sends a flight command**. Live sensor data
|
||
still drives the safety guards, so you can also confirm an abort fires (e.g.
|
||
hold a hand within 25 cm of a horizontal sensor and watch it abort). Run this
|
||
with the props off first; only after the sequence looks right should you remove
|
||
dry run and fly tethered.
|
||
|
||
## Safety
|
||
|
||
- The drone never spins up until you press **ARM**, and arming is refused
|
||
without a Flow v2 deck, on low battery, or if the supervisor reports the drone
|
||
tumbled/locked.
|
||
- **EMERGENCY STOP** (button or **SPACE**) cuts the motors instantly from any
|
||
state. A real emergency stop requires a reboot before flying again.
|
||
- All radio I/O runs on a dedicated worker thread that streams hover setpoints at
|
||
10 Hz, so a slow or frozen UI can't make the drone drift or fall. Closing the
|
||
window lands the drone (if airborne) and closes the link cleanly.
|
||
|
||
## Layout
|
||
|
||
- `rangeview/controller.py` - threaded Crazyflie link: connection, logging of
|
||
`range.*` + `stateEstimate.*` + supervisor status, the arm/scan/land flight
|
||
profile, and the world-frame map accumulation.
|
||
- `rangeview/app.py` - DearPyGui front-end: telemetry readouts, the mapping and
|
||
flight controls, the top-down map plot, and the firmware console.
|