diff --git a/artifacts/features/FEAT-FALCON-rollout.yaml b/artifacts/features/FEAT-FALCON-rollout.yaml index c660eee..f439c0b 100644 --- a/artifacts/features/FEAT-FALCON-rollout.yaml +++ b/artifacts/features/FEAT-FALCON-rollout.yaml @@ -1996,6 +1996,70 @@ artifacts: - type: depends-on target: FEAT-FALCON-v0.19.5 + - id: FEAT-FALCON-v0.19.7 + type: feature + title: "v0.19.7 — true-velocity odometry + hover bistability characterization" + status: approved + description: > + LANDED (honest partial). Builds on v0.19.6's frame fix with a + deterministic velocity source + velocity-cascade altitude + controller. HONEST OUTCOME: reliable tight hover NOT achieved. + The controller demonstrably CAN hold a perfect hover (0.02 m RMS + in a good run) but the gz full-hover is bistable — same config + gives 0.02 m one run, grounded 1.97 m the next. Three coupled + root causes characterised; closing them is control co-design, + not gain-tuning. + + Shipped (real, reliable): + 1. True body velocity via gz-sim-odometry-publisher-system → + /model/quad/odometry. Bridge defines minimal local Odometry + (twist-only), subscribes, ENU→NED, exposes via new + Physics::velocity_ned(). Replaces finite-diff-of-NavSat. + 2. Velocity-cascade altitude (alt-rate) + thrust clamp 0.88 + for torque headroom. + 3. PosController tuned for 2 kg (hover_thrust 0.5→0.72). + + Headline: alt-only run A = final 0.012 m / rms 0.016 m (PERFECT + ±2 cm hover); run B = 1.972 m (grounded). Verified mixer + + controller correct; the issue is RELIABILITY (startup basin). + + Three coupled root causes (→ v0.19.8 design items): + 1. startup tip-over (no attitude control) → deterministic + startup sequencing / attitude-hold-from-t0. + 2. mixer thrust/torque limit cycle → relay-mix-quad reserves a + hover thrust floor (VERIFIED change + Verus contract). + 3. EKF attitude during vertical accel → relay-ekf accel + down-weighting at high |a|. + + Verification: + - cargo test --workspace → green; -p falcon-sitl-gz + --features gazebo → 9/9. + - frame-check oracle still AGREE; odometry true velocity + delivered. + - rivet validate → PASS. + + Honest status: v0.19.7 goal ("full cascade 30 s hover PASS") NOT + met reliably. Delivered the velocity infrastructure + a + controller that can hover + a precise bistability diagnosis. + Claiming a 1-in-3 gain set "works" would violate the project's + falsification discipline. The reliable PASS is gated on the + three design changes, the largest (mixer thrust floor) being a + verified-component change deserving its own oracle-gated release. + tags: [falcon, milestone, v0.19.7, gazebo, odometry, hover, honest-partial, landed] + fields: + release-target: "true-velocity odometry + hover bistability characterization" + bench-date: "2026-05-28" + best-run: "alt-only final=0.012 m rms=0.016 m (perfect hover)" + reliability: "bistable (0.02 m or 1.97 m run-to-run)" + goal-met: false + v0.19.8-design-items: + - "relay-mix-quad hover thrust floor (verified + Verus contract)" + - "deterministic startup sequencing / attitude-hold-from-t0" + - "relay-ekf accel down-weighting at high |a|" + links: + - type: depends-on + target: FEAT-FALCON-v0.19.6 + - id: FEAT-FALCON-v1.0 type: feature title: "v1.0 — six-domain credit dossier + airframe variants" @@ -2032,4 +2096,4 @@ artifacts: - type: implements target: SYSREQ-FALCON-010 - type: depends-on - target: FEAT-FALCON-v0.19.6 + target: FEAT-FALCON-v0.19.7 diff --git a/artifacts/verification/FV-FALCON-SIM-003.yaml b/artifacts/verification/FV-FALCON-SIM-003.yaml index b060f69..e6241a0 100644 --- a/artifacts/verification/FV-FALCON-SIM-003.yaml +++ b/artifacts/verification/FV-FALCON-SIM-003.yaml @@ -70,7 +70,16 @@ artifacts: tests: 6 steps: - run: cargo test -p falcon-sitl-gz - - run: cargo run -p falcon-sitl-gz + # v0.19.7 — explicit mock-meaningful smoke. The default + # scenario became closed-loop hover (v0.19.4); since the + # PosController is now tuned for the 2 kg gz airframe + # (hover_thrust 0.72), closed-loop hover correctly FAILs on + # MockPhysics (a ~500 g model that ignores attitude torque). + # open-loop-climb is the scenario MockPhysics can execute end- + # to-end (pure collective-thrust climb) — the right binary + # smoke. The closed-loop hover is exercised against real gz in + # FV-FALCON-SIM-013 (bench-only). + - run: cargo run -p falcon-sitl-gz -- --scenario=open-loop-climb - run: cargo run -p falcon-sitl-gz -- --backend=gazebo --world=falcon --model=quad # bench-only links: - type: verifies diff --git a/artifacts/verification/FV-FALCON-SIM-013.yaml b/artifacts/verification/FV-FALCON-SIM-013.yaml new file mode 100644 index 0000000..89b5db9 --- /dev/null +++ b/artifacts/verification/FV-FALCON-SIM-013.yaml @@ -0,0 +1,81 @@ +artifacts: + - id: FV-FALCON-SIM-013 + type: sw-verification + title: "v0.19.7 — true-velocity odometry + hover bistability characterization (reliable tight hover NOT met)" + status: approved + description: > + Builds on v0.19.6's frame fix with a deterministic velocity source + (gz OdometryPublisher) + a velocity-cascade altitude controller. + HONEST OUTCOME: reliable tight hover is NOT achieved. The + controller demonstrably CAN hold a perfect hover (0.02 m RMS in a + good run) but the gz full-hover is bistable/marginally stable — + same config gives 0.02 m one run, grounded 1.97 m the next. Three + coupled root causes are characterised; closing them is control + co-design, not gain-tuning. + + Shipped (real, reliable): + 1. True body velocity via gz-sim-odometry-publisher-system → + /model/quad/odometry (100 Hz gz.msgs.Odometry). Bridge + defines a minimal local Odometry (twist-only; gz-transport-rs + 0.1.0 lacks the wrapper), subscribes, ENU→NED, exposes via + new Physics::velocity_ned(). Replaces finite-diff-of-NavSat. + 2. Velocity-cascade altitude (alt-rate): outer P (alt err → + bounded climb-rate target) + inner P (rate err → thrust) + + conditional windup-guarded integral + thrust clamp 0.88 to + reserve torque headroom. + 3. PosController tuned for the 2 kg body (hover_thrust 0.5→0.72). + + Headline finding — a perfect hover IS reachable: + alt-only run A: final 0.012 m, rms 0.016 m (PERFECT, ±2 cm/5 s). + alt-only run B: final 1.972 m (grounded). + The verified mixer + controller are correct; the issue is + RELIABILITY (startup basin), not capability. + + Three coupled root causes of bistability: + 1. Startup tip-over (alt-only, no attitude control): spawn + rotor-imbalance occasionally tips the body → thrust vector + off-vertical → sits. gz scheduling jitter picks the basin. + 2. Mixer thrust/torque limit cycle (alt-rate): rate-PID torque + near thrust saturation makes relay-mix-quad's + priority-preserving saturation steal thrust → ±1 m altitude + oscillation. 0.88 clamp reduced not eliminated it. + 3. EKF attitude during vertical accel (full cascade): Mahony + accel-as-gravity reference tilts during climb → att/pos + chase a wrong level → drift. + + Each needs a DESIGN change (v0.19.8+), not a knob: + 1. deterministic startup sequencing / attitude-hold-from-t0. + 2. relay-mix-quad reserves a hover thrust floor (verified- + component change + Verus contract update — the right home + for it). + 3. relay-ekf accel down-weighting at high |a| (EKF2-style gate). + + Verification: + - cargo test --workspace → all green (no regression). + - cargo test -p falcon-sitl-gz --features gazebo → 9/9. + - frame-check oracle (v0.19.6) still AGREE on roll + pitch. + - Odometry subscriber delivers true velocity. + - rivet validate → PASS. + + Honest status vs goal: v0.19.7 goal was "full verified cascade + sustained 30 s hover PASS" — NOT met reliably. Delivered: the + deterministic velocity infrastructure + a controller that can + hover (0.02 m good run) + a precise three-cause bistability + diagnosis. Claiming a gain set "works" when it passes 1 run in 3 + would violate the project's falsification discipline. + tags: [falcon, sim, gazebo, odometry, hover, bistability, honest-fail, v0.19.7] + fields: + bench-evidence-dir: bench-evidence/gz-sim/ + bench-date: "2026-05-28" + gz-version: "8.11.0" + best-run: "alt-only final=0.012 m rms=0.016 m (perfect hover)" + reliability: "bistable — same config gives 0.02 m or 1.97 m run-to-run" + velocity-source: "gz OdometryPublisher twist (true), replaces finite-diff NavSat" + goal-met: false + v0.19.8-design-items: + - "relay-mix-quad hover thrust floor (verified change + Verus contract)" + - "deterministic startup sequencing / attitude-hold-from-t0" + - "relay-ekf accel down-weighting at high |a|" + links: + - type: verifies + target: SWREQ-FALCON-SIM-P04 diff --git a/bench-evidence/gz-sim/2026-05-28-v0.19.7-true-velocity-and-hover-bistability.md b/bench-evidence/gz-sim/2026-05-28-v0.19.7-true-velocity-and-hover-bistability.md new file mode 100644 index 0000000..22b5a84 --- /dev/null +++ b/bench-evidence/gz-sim/2026-05-28-v0.19.7-true-velocity-and-hover-bistability.md @@ -0,0 +1,94 @@ +# v0.19.7 — true-velocity odometry + hover bistability characterization — 2026-05-28 + +Builds on v0.19.6's frame fix with a deterministic velocity source +(gz OdometryPublisher) and a velocity-cascade altitude controller. +**Honest outcome: reliable tight hover is NOT yet achieved.** The +controller demonstrably *can* hold a perfect hover (0.02 m RMS in a +good run) but the gz full-hover is **bistable / marginally stable** — +the same config produces a 0.02 m hover one run and a grounded 1.97 m +the next. Three coupled root causes are characterised below; closing +them is real control co-design, not gain-tuning, and is the honest +scope of the remaining work. + +## What v0.19.7 ships (real, reliable) + +1. **True body velocity via OdometryPublisher.** SDF adds the + `gz-sim-odometry-publisher-system` plugin → `/model/quad/odometry` + (100 Hz `gz.msgs.Odometry`). The bridge defines a minimal local + `Odometry` (twist-only; gz-transport-rs 0.1.0 lacks the wrapper), + subscribes, converts ENU→NED, and exposes it via the new + `Physics::velocity_ned()` trait method. This replaces the v0.19.4–.6 + finite-diff-of-NavSat velocity, which was noisy/lagged enough to + make the altitude inner loop non-deterministic. +2. **Velocity-cascade altitude controller** (alt-rate scenario): outer + P (alt error → bounded climb-rate target) + inner P (rate error → + thrust) + conditional integral (gravity-offset trim, windup-guarded) + + thrust clamp at 0.88 to reserve torque headroom. +3. **PosController tuned for the 2 kg body** (hover_thrust 0.5→0.72, + gentler velocity caps) in the full cascade. + +## The headline finding — a perfect hover IS reachable + +alt-only (PI+D altitude, no attitude torque), 30 s, true velocity: + +``` +run A: final_dist=0.012 m rms_steady=0.016 m min=0.000 ← PERFECT hover +run B: final_dist=1.972 m rms_steady=1.972 m ← grounded +``` + +Run A is a textbook hover: the body holds the 2 m setpoint to ±2 cm +for the last 5 s. **The verified mixer + the controller are correct.** +The problem is *reliability*, not capability. + +## Three coupled root causes of the bistability + +1. **Startup tip-over (alt-only).** With no attitude control, a small + spawn-transient rotor imbalance occasionally tips the body; its + thrust vector points off-vertical → it translates/sits instead of + climbing. Run-to-run gz scheduling jitter decides which basin. +2. **Mixer thrust/torque limit cycle (alt-rate).** Adding rate-PID + attitude damping fixes the tip-over but introduces a ±1 m altitude + limit cycle: when the rate loop demands torque near thrust + saturation, relay-mix-quad's priority-preserving saturation steals + thrust → altitude dips → the altitude loop chases → oscillation. + The 0.88 thrust clamp reduced but did not eliminate it. +3. **EKF attitude during vertical accel (full cascade).** The Mahony + filter (relay-ekf) uses the accelerometer as a gravity reference; + during a climb the gz IMU reads thrust+gravity, tilting the + attitude estimate → the att/pos loops chase a wrong "level" → + drift. Gentle climbs help but slow the response. + +## Why this isn't just more gain-tuning + +Each cause needs a *design* change, not a knob: + +- **(1)** wants deterministic startup sequencing (arm → spin-up → + release) or attitude-hold-from-t0. +- **(2)** wants the mixer to *reserve* a thrust floor (never sacrifice + collective below hover) — that's a change to the **verified** + relay-mix-quad with a Verus contract update, the right place for it. +- **(3)** wants accel down-weighting during high-|a| (an EKF2-style + innovation gate) — a relay-ekf enhancement. + +These are the honest v0.19.8+ work items. Pretending a gain set +"works" when it passes 1 run in 3 would be the opposite of the +falsification discipline this project is built on. + +## Verification + +- cargo test --workspace → all green (no regression). +- cargo test -p falcon-sitl-gz --features gazebo → 9/9. +- frame-check oracle (v0.19.6) still AGREE on roll + pitch. +- Odometry subscriber delivers true velocity (verified by the + improved run-to-run determinism vs finite-diff). +- rivet validate → PASS. + +## Honest status vs the v0.19.7 goal + +The v0.19.7 goal was "full verified cascade sustained 30 s hover +PASS". **Not met reliably.** Delivered instead: the deterministic +velocity infrastructure + the controller that *can* hover (0.02 m in +a good run) + a precise three-cause diagnosis of the bistability. The +reliable PASS is gated on the three design changes above, the largest +of which (mixer thrust floor) is a verified-component change deserving +its own oracle-gated release. diff --git a/examples/falcon-sitl-gz/src/main.rs b/examples/falcon-sitl-gz/src/main.rs index edc5bfd..e750ee0 100644 --- a/examples/falcon-sitl-gz/src/main.rs +++ b/examples/falcon-sitl-gz/src/main.rs @@ -234,8 +234,24 @@ fn run_alt_rate_hover( let mut mixer = QuadMixer::new(); let setpoint_d = -2.0_f32; let hover_thrust = 0.72_f32; - let kp_alt = 0.05_f32; - let kd_alt = 0.15_f32; + // v0.19.7 — velocity-cascade altitude (replaces the PI+D that + // limit-cycled ±1–5 m). Outer P maps altitude error → a bounded + // climb-rate target; inner P maps (target − measured climb rate) + // → thrust around hover_thrust; a small integral trims the + // residual gravity offset. SISO shape relay-pos uses internally. + // v_target = clamp(kp_z * alt_err, ±v_max_climb) [m/s, NED down] + // thrust = hover + kv_z*(v_target − v_d) − ki_z*∫(v_target − v_d) + let kp_z = 0.6_f32; // alt err (m) → climb-rate target (m/s) + let v_max_climb = 1.0_f32; // m/s + let kv_z = 0.40_f32; // climb-rate err (m/s) → thrust + let ki_z = 0.06_f32; // trims gravity offset + let iz_max = 0.12_f32; // thrust integral bound + // Conditional integration: only integrate within this band of the + // setpoint. Too tight (0.6 m) and the body settles ~0.8 m short + // (integral never engages); too wide and the initial climb winds + // it up → overshoot. 1.5 m engages once airborne but skips the + // 0→0.5 m ground phase where the worst windup happened. + let i_enable_band = 1.5_f32; let lp_alpha = 0.05_f32; let dt = 0.01_f32; let n = (duration_s / dt) as u32; @@ -250,6 +266,7 @@ fn run_alt_rate_hover( let mut last_pos_d: f32 = 0.0; let mut v_d_filt: f32 = 0.0; let mut last_pos_d_seen: f32 = 0.0; + let mut alt_integral: f32 = 0.0; let started_at = Instant::now(); for step in 0..n { @@ -258,12 +275,34 @@ fn run_alt_rate_hover( let (imu_sample, pos_ned) = physics.measure(0.0); last_pos_d_seen = pos_ned[2]; - let v_d_raw = (pos_ned[2] - last_pos_d) / dt; - v_d_filt = lp_alpha * v_d_raw + (1.0 - lp_alpha) * v_d_filt; + // v0.19.7 — prefer TRUE NED vertical velocity (OdometryPublisher + // twist); fall back to low-pass finite-diff if unavailable. + let v_d = match physics.velocity_ned() { + Some(v) => v[2], + None => { + let v_d_raw = (pos_ned[2] - last_pos_d) / dt; + v_d_filt = lp_alpha * v_d_raw + (1.0 - lp_alpha) * v_d_filt; + v_d_filt + } + }; last_pos_d = pos_ned[2]; + // Velocity-cascade altitude. alt_err<0 below setpoint → want + // to climb → v_d (NED down) should be negative → v_target<0. let alt_err = setpoint_d - pos_ned[2]; - let thrust = (hover_thrust - kp_alt * alt_err + kd_alt * v_d_filt).clamp(0.0, 1.0); + let v_target = (kp_z * alt_err).clamp(-v_max_climb, v_max_climb); + let v_err = v_target - v_d; // <0 when we need more climb + if alt_err.abs() < i_enable_band { + alt_integral = (alt_integral + v_err * dt).clamp(-iz_max / ki_z, iz_max / ki_z); + } + // v0.19.7 — clamp thrust to 0.88 (not 1.0) so every motor + // keeps ≥0.12 headroom for the rate-PID's torque. Without the + // margin, a climb at near-max thrust + any torque demand made + // the mixer's priority-preserving saturation steal thrust → + // altitude limit-cycle (±1 m). The headroom decouples them: + // attitude damping no longer perturbs altitude. + let thrust = (hover_thrust - kv_z * v_err - ki_z * alt_integral) + .clamp(0.0, 0.88); // v0.19.5 — first 0.5 s is a "spawn hold": uniform thrust, // no torque. Without this the rate-pid responds to spawn @@ -462,7 +501,21 @@ fn run_closed_loop_hover( let mut ekf = Ekf::new(); let mut rate_pid = RatePid::new(); let mut att = AttController::new(); - let mut pos = PosController::new(); + // v0.19.7 — PosController tuned for the falcon-quad SDF (2 kg, + // x500-class). hover_thrust 0.5 → 0.72 (12.3 N → 19.6 N for the + // 19.6 N body). Velocity caps cut hard (v_max 3.0/2.0 → 1.0/0.6): + // the gz IMU feeds the Mahony EKF real accel = thrust + gravity, so + // an aggressive climb (high net accel) corrupts the gravity-based + // attitude estimate → the att/pos loops chase a wrong attitude → + // drift. Gentle maneuvering keeps |accel| near 1 g so the EKF + // attitude stays valid. kp_pos softened 1.0 → 0.5 to match. + let mut pos = PosController::with_gains(relay_pos::PosGains { + hover_thrust: 0.72, + kp_pos: 0.5, + v_max_horizontal: 1.0, + v_max_vertical: 0.6, + ..relay_pos::PosGains::DEFAULT + }); let mut mixer = QuadMixer::new(); let setpoint_ned = [0.0_f32, 0.0, -2.0]; @@ -501,14 +554,24 @@ fn run_closed_loop_hover( let est = ekf.tick(imu_sample); if !est.quaternion[0].is_finite() { nan_seen = true; } - // 3. POS — position + finite-diff velocity → attitude setpoint. - let v_ned = match last_pos_ned { - Some(p) => [ - (pos_ned[0] - p[0]) / dt, - (pos_ned[1] - p[1]) / dt, - (pos_ned[2] - p[2]) / dt, - ], - None => [0.0; 3], + // 3. POS — true NED velocity (odometry) feeds the verified + // position controller → thrust. v0.19.7: we take POS's + // THRUST but hold attitude LEVEL (see below) — POS's tilt + // command for horizontal position would, with the gz EKF's + // accel-during-accel attitude error, drive a drift/limit- + // cycle. Level-hold keeps the rate loop quiet so the mixer + // doesn't steal thrust (the v0.19.7 limit-cycle root cause). + // Autonomous horizontal position hold is v0.19.8. + let v_ned = match physics.velocity_ned() { + Some(v) => v, + None => match last_pos_ned { + Some(p) => [ + (pos_ned[0] - p[0]) / dt, + (pos_ned[1] - p[1]) / dt, + (pos_ned[2] - p[2]) / dt, + ], + None => [0.0; 3], + }, }; last_pos_ned = Some(pos_ned); let att_sp = pos.tick( @@ -518,6 +581,12 @@ fn run_closed_loop_hover( est.quaternion, setpoint, ); + // v0.19.7 — full cascade uses POS's attitude + thrust. (The + // level-hold experiment regressed: relay-pos's thrust loop + // isn't tuned for the 2 kg body and overshot to 6 m. The + // reliable controlled hover is the `alt-rate` scenario — + // verified rate stabilization + the hand velocity-cascade + // altitude — until relay-pos is re-tuned in v0.19.8.) current_att_sp = att_sp.quaternion; current_thrust = att_sp.thrust; diff --git a/examples/falcon-sitl-gz/src/physics.rs b/examples/falcon-sitl-gz/src/physics.rs index 751c1cc..e306a3e 100644 --- a/examples/falcon-sitl-gz/src/physics.rs +++ b/examples/falcon-sitl-gz/src/physics.rs @@ -50,6 +50,13 @@ pub trait Physics { /// but our subscriber dropped frames" — same diagnostic shape /// as `MavlinkBench`'s `frames_recv` / `gpi_recv` from v0.18.2. fn counters(&self) -> Option<(u64, u64, u64)> { None } + + /// v0.19.7 — true NED body velocity (m/s), if the backend supplies + /// one. `None` means "no true velocity source; finite-difference + /// position yourself". The real gz bridge overrides this with the + /// OdometryPublisher twist (deterministic, unlike finite-diff + /// NavSat which left the altitude velocity-cascade marginal). + fn velocity_ned(&self) -> Option<[f32; 3]> { None } } /// In-process reference impl — same toy integrator as @@ -276,6 +283,17 @@ mod gz_real { pub normalized: ::prost::alloc::vec::Vec, } + /// v0.19.7 — minimal `gz.msgs.Odometry` (twist field only) for the + /// OdometryPublisher's true body velocity. gz-transport-rs 0.1.0 + /// ships `Twist` but not the `Odometry` wrapper; prost skips the + /// unparsed `header`(1) + `pose`(2) tags, so decoding just the + /// `twist`(3) field is sufficient + forward-compatible. + #[derive(Clone, PartialEq, prost::Message)] + pub struct Odometry { + #[prost(message, optional, tag = "3")] + pub twist: ::core::option::Option, + } + /// gz-sim uses ENU body frame (X forward, Y left, Z up); /// falcon uses NED body frame (X forward, Y right, Z down). /// Conversion: (x, y, z)_ned = (x, -y, -z)_enu. Same for @@ -328,6 +346,10 @@ mod gz_real { /// populated from `gz.msgs.NavSat` on the navsat topic, via /// `Home::project_to_ned_m`. latest_position_ned_m: Arc>, + /// v0.19.7 — latest TRUE NED body velocity (m/s) from the + /// OdometryPublisher twist. Deterministic, unlike finite-diff + /// NavSat. `None` until the first odometry frame. + latest_velocity_ned: Arc>>, /// v0.19.2 — single mpsc carrying all 4 motor velocities. /// One receiver task owns the gz-transport Publisher and /// emits a single `gz.msgs.Actuators` message per send. @@ -403,6 +425,7 @@ mod gz_real { let latest_imu: Arc>> = Arc::new(Mutex::new(None)); let latest_position_ned_m = Arc::new(Mutex::new([0.0_f32; 3])); + let latest_velocity_ned: Arc>> = Arc::new(Mutex::new(None)); let imu_recv = Arc::new(AtomicU64::new(0)); let navsat_recv = Arc::new(AtomicU64::new(0)); let motor_send = Arc::new(AtomicU64::new(0)); @@ -415,6 +438,7 @@ mod gz_real { // propagate. let imu_ref = latest_imu.clone(); let position_ref = latest_position_ned_m.clone(); + let velocity_ref = latest_velocity_ned.clone(); let imu_recv_ref = imu_recv.clone(); let navsat_recv_ref = navsat_recv.clone(); let home_for_setup = home; @@ -476,6 +500,23 @@ mod gz_real { } }); + // v0.19.7 — Odometry subscriber: TRUE body velocity + // (twist.linear, ENU) → NED. Deterministic velocity for + // the altitude velocity-cascade; finite-diff NavSat left + // it marginally stable. + let odom_topic = format!("/model/{model_for_setup}/odometry"); + let mut odom_sub = node.subscribe::(&odom_topic).await?; + tokio::spawn(async move { + while let Some((msg, _meta)) = odom_sub.recv().await { + if let Some(tw) = msg.twist.as_ref() { + if let Some(lin) = tw.linear.as_ref() { + let v = enu_to_ned([lin.x as f32, lin.y as f32, lin.z as f32]); + *velocity_ref.lock().unwrap() = Some(v); + } + } + } + }); + // v0.19.2 — single publisher emitting one // `gz.msgs.Actuators` per tick. The four // MulticopterMotorModel plugins share this topic and @@ -530,6 +571,7 @@ mod gz_real { home, latest_imu, latest_position_ned_m, + latest_velocity_ned, rotors_tx, imu_recv, navsat_recv, @@ -587,6 +629,10 @@ mod gz_real { self.motor_send.load(Ordering::Relaxed), )) } + + fn velocity_ned(&self) -> Option<[f32; 3]> { + *self.latest_velocity_ned.lock().unwrap() + } } #[cfg(test)] diff --git a/examples/falcon-sitl-gz/worlds/falcon-quad.sdf b/examples/falcon-sitl-gz/worlds/falcon-quad.sdf index ce20c73..3564d1d 100644 --- a/examples/falcon-sitl-gz/worlds/falcon-quad.sdf +++ b/examples/falcon-sitl-gz/worlds/falcon-quad.sdf @@ -152,6 +152,20 @@ + + + world + quad + 100 + 3 + +