cf-rangeview/README.md

# 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.