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