Fixed wing: quaternion orientation hold — inverted flight, knife edge, prop hang, figure sequencer (RFC, testers wanted)Feature/quaternion attitude hold#11695
Draft
swissembedded wants to merge 100 commits into
Conversation
…e edge / prop hang) New USE_ORIENTATION_HOLD feature: singularity free attitude hold for arbitrary target attitudes, using the existing orientation quaternion and quaternion math. - Error formed in the rotation group (rotation vector of q_target^-1 * q_est), valid for large error angles, shortest path handling at the 180 deg antipode, defined at pitch +/-90 where the Euler based pidLevel() is singular - Heading is always left free via swing/twist decomposition about the earth vertical axis (matches ANGLE mode behaviour in normal flight, free body roll at prop hang) - New boxes INVERTED / KNIFE EDGE LEFT / KNIFE EDGE RIGHT / PROP HANG, airplanes only, priority ANGLE > HORIZON > ORIENTATION HOLD > ANGLEHOLD - Reuses PID_LEVEL P gain, rate limits and PT1 smoothing of pidLevel(), feeds the existing, unchanged rate loop on all three axes; sticks remain live as rate commands
The swing-twist decomposition about earth Z is degenerate for every inverted attitude: w^2 + z^2 vanishes for all headings, so the extracted twist direction is driven by noise. Near roll 180 with a small pitch offset this produced large phantom body-yaw errors (found by the SITL closed-loop bench: 152 deg error for two attitudes 1.1 deg apart). Regulate the direction of the earth vertical in the body frame instead (reduced attitude control): well defined everywhere, heading-free by construction (heading in normal/inverted/knife flight, body roll at prop hang), exact at large angles, deterministic axis choice at the 180 deg antipode. Host convention tests 17/17 (incl. new regressions for the inverted degeneracy), SITL closed loop: all targets, antipode starts and the pitch-90 crossing pass.
New ALT FLOOR box (permanentId 73): while active and armed above floor + margin once, a predicted floor breach flies an automatic recovery (shortest-path upright + climb pitch via the orientation hold controller) until back above the floor and climbing. Plain switch semantics: box off = off, so the aircraft can land. - Predictive engage z + vz * 3s < floor: the lookahead must cover the Z estimator lag under sustained sink (~vz * 2-3 s with default baro weighting), not just the roll-to-upright time - Arms only after climbing above floor + margin once (switching the box on on the ground never grabs the aircraft during takeoff) - Priority: failsafe/nav auto-ANGLE > floor recovery > pilot modes; inactive in MANUAL passthrough - Settings alt_floor_altitude / alt_floor_margin / alt_floor_climb_pitch (PG_ALTITUDE_FLOOR_CONFIG), new helper navIsAltitudeEstimateTrusted() - SITL closed loop: dive from 67 m at -50 deg pitch caught at 54-61 m (floor 30 m), landing descent with the box off stays untouched
New INPUT_TVC_ROLL/PITCH/YAW servo mixer sources (61-63): same stabilized commands as the control surfaces, but with a thrust dependent gain. Vectoring vane / tilt motor torque scales with thrust, so the deflection is compensated inversely (capped below 25% thrust) to keep the control loop gain roughly constant -- full authority in a prop hang, no overcontrol at full power. Map TVC servos to these sources instead of statically coupling them to the surface outputs. Settings tvc_gain (overall %, at full thrust) and tvc_thrust_comp (0 = plain coupling, 100 = full 1/thrust), PG_THRUST_VECTORING_CONFIG. SITL verified: TVC/surface deflection ratio 1.00 at full throttle, 3.85 near idle (theoretical cap 4.0).
Inverted flight needs a down-elevator bias to hold altitude, knife edge a few degrees of nose above the horizon (doc: fuselage lift). New settings ohold_inverted_pitch_trim / ohold_knife_pitch_trim (deg), applied as the Euler pitch of the hold target before the attitude's roll -- positive is always 'nose above the horizon', in every attitude and for both knife edge sides (PG_ORIENTATION_HOLD_CONFIG). Host tests 18/18 (trim shifts the target exactly); SITL end-to-end: controller output ~0 on the trimmed target, clearly nonzero 10 deg off (I-term-reset measurement with frozen attitude).
The body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other knife edge side: the required trim is left/right = shared fuselage-lift part +/- prop part, so one shared value cannot trim both sides. Reversed prop rotation swaps the sides. ohold_knife_pitch_trim -> ohold_knife_left_pitch_trim / ohold_knife_right_pitch_trim. Host tests 19/19 (new per-side check).
Aerobatic figures as time parameterized orientation-hold targets. The heading-free reduced attitude controller makes figures trivially invariant: no attitude capture needed, a roll always rotates about the current heading, a loop flies in the current heading plane. Boxes FIGURE ROLL / FIGURE LOOP / FIGURE 4PT ROLL (permanentId 74-76): figure starts when the box goes active, holds level when complete, re-arms on release. Altitude assist: a PID on altitude/climb rate adds an earth referenced nose-above-horizon offset to the figure target; the controller distributes it to elevator and rudder as the roll phase demands (the classic slow-roll coordination, for free from the error geometry), blended out with cos(pitch) toward nose-vertical where altitude belongs to the thrust axis. Settings fig_roll_rate / fig_loop_rate / fig_point_dwell / fig_assist_z_gain / fig_assist_vz_gain / fig_assist_max (PG_FIGURE_SEQUENCER_CONFIG). Keep the vz gain low: the climb rate estimate lags and a strong damping term fights fast figures. SITL closed loop: roll and loop complete through inverted; with assist the roll ends 0.4 m from entry altitude vs 4.3 m stuck low without.
FIGURE SEQ box (permanentId 77) flies a programmable chain of up to 16 segments (PG_FIGURE_SEQUENCE, MSP2_INAV_FIGURE_SEQUENCE 0x2240 / MSP2_INAV_SET_FIGURE_SEQUENCE 0x2241): ROLL/PITCH rotations are cumulative on the running attitude baseline (Immelmann = PITCH +180 then ROLL +180), HOLD holds an absolute attitude, WAIT_ALT gates the chain on reaching a target altitude (wings level, climbing/descending via the assist mechanism), WAIT_TIME dwells. A position wait is reserved -- it needs heading control / nav coupling. Altitude assist fix: the offset now raises the NOSE ELEVATION -- multiplied by cos(pitch), which both blends it out toward nose-vertical and corrects the sign when the accumulated pitch parameter is past +/-90 (base pitch 180 after a half loop acted inverted before). SITL: WAIT_ALT 40m -> Immelmann -> hold plays through with the gate respected (figure starts at 38.6 m) and ends upright. KNOWN ISSUE: the plane noses down ~17 deg for several seconds after the figure before recovering -- looks like slow AHRS recovery after the fast maneuver in the bench sensor model, under investigation.
The accumulated rate-loop I trims the holding load of the CURRENT attitude (propwash authority in a prop hang, rudder load in knife edge). On a target switch (e.g. prop hang -> knife edge) that charge is wrong for the new attitude and would discharge as a disturbance into the entry. Reset the accumulators exactly once per edge: mode entry, preset/figure/floor source switch, and mode exit (back to the pilot's manual flying). Within a figure (continuous trajectory) the source is stable and the I-term is kept. Together with pid_iterm_limit_percent (default 33%) this bounds the knife-edge saturation windup pragmatically; a direction-aware saturation freeze in the FW rate controller remains a possible follow-up slice. Host tests 20/20 (T17: one reset per edge, none while held); SITL scenarios/sequence/figures regression green.
New 3D LOCK box (permanentId 78): while the sticks are centered the current attitude (captured through the singularity-free reduced attitude controller, so any attitude incl. knife edge or vertical) is held; stick input flies pure rates with the lock target following the aircraft, and the NEW attitude locks when the sticks center again. Closes the gap to ArduPlane's ACRO attitude lock. Presets/figures/ floor take priority over the lock box. Host tests 21/21 (T18: capture/hold/follow/re-lock edges); SITL: windowed mean attitude drift 0.0 deg over 6 s hold, stick moves the lock by 15 deg, new lock drift 0.8 deg.
Doc section 7, Ebene 1: evaluate the orientation hold error function and level gain on injected quaternions (8x float32 in: q_est, q_target wxyz; 6x float32 out: err_deg xyz, rate_target_dps xyz). Pure computation on the target MCU's float32 - no controller, estimator or arming state is touched, deterministic single-step, safe in any build. Lets the singularity checklist run against the real F4/F7 numerics over MSP. SITL: 82 test vectors (signs, yaw invariance, pitch-90 sweep, antipode, exact 180, near-inverted degeneracy regression, denormalized input, random grid) pass with worst float32-vs-float64 deviation 0.0001 deg.
While PROP HANG is the active hold target and the nose is near the zenith, the thrust carries the weight and a dedicated throttle PID owns the altitude axis: - I-term seeded from the pilot's throttle at engage: learns the model's hover throttle online, no setting needed - Altitude target latches only once the vertical motion has settled (engaging mid pull-up must not freeze a fly-through altitude) - Elevation hysteresis 60/45 deg: the attitude wobble around the hang must not flap the controller (every re-engage would re-capture the target - a ratcheting drift) - Tilt compensated output (vertical thrust component), throttle stick out of the mid deadband hands control back to the pilot - Hooked into the mixer throttle path before scaling, so battery compensation still applies Settings ohold_hover_thr_p/i/d (PG_HOVER_THROTTLE_CONFIG). SITL: hands-free prop hang holds +-2.3 m over 12 s in a thrust-borne plant with motor lag, pilot throttle override climbs away cleanly.
FIGSEG_IMPULSE: open-loop full-rate kick (p1 pitch %, p2 yaw %, p3 ms) for snap/spin entries; the rate loop saturates the surfaces, the next segment (or the level hold) catches the resulting attitude shortest path. SITL: 193 deg/s peak, caught wings-level 1.1 deg after. FIGSEG_WAIT_POS: airspace containment - bank toward HOME (course loop, 0.8 deg bank per deg of course error, capped at p2) until GPS_distanceToHome < p1. The coordinated turn rates are fed forward via the existing pidTurnAssistant (fw_reference_airspeed), otherwise the heading-free hold regulates the physical turn yaw rate to zero and the aircraft never turns. Holds level while no home fix exists. SITL: turn-in and approach verified (course 331->188 deg coordinated, distance 315->137 m closing); the full closed-loop containment test needs a consistent turn/heading plant model in the bench first (the bench plant's turn kinematics and the AHRS/COG heading chain disagree) - tracked as a bench issue, not firmware.
Found by driving the Configurator against SITL: with the 10 new boxes
(and all NAV boxes active once FEATURE_GPS is on) the active box-name
list exceeds MSP_PORT_OUTBUF_SIZE, serializeBoxNamesReply() returns an
MSP error and the Configurator aborts the connect ('No configuration
received'). Real F4/F7 targets have the 4 KB FLASHFS buffer; only
no-FLASHFS targets (SITL) sit at 512.
- Shorten the new box names (INVERT, KNIFE L/R, P-HANG, FLOOR,
F ROLL/LOOP/4PT/SEQ, 3DLOCK)
- Guard MSP_PORT_OUTBUF_SIZE with #ifndef and override to 1024 on SITL
Branch Targeting SuggestionYou've targeted the
If This is an automated suggestion to help route contributions to the appropriate branch. |
The static function has a single call site inside the FAST_CODE pidController and was inlined into .tcm_code, overflowing the 16 KB ITCM_RAM on OMNIBUSF7/V2 by 424 bytes. Same convention as pidApplyFixedWingRateController.
Author
|
Thanks @Jetrell for the pointer to #11595 (Auto Speed Mode) and the acc-based throttle idea. On the relationship to #11595: the attitude hold here is the general layer — what drives the throttle on top of it is a separate, pluggable criterion. Today that criterion is altitude (in the prop-hang regime, nose above ~60°), but it could equally be speed in level flight — which is exactly what #11595 does — or an acc-based load target during a maneuver, as @Jetrell suggested (acc x/y/z as the target once the maneuver commences: more thrust below target, less above). So rather than being mutually exclusive, #11595's airspeed throttle is a natural criterion to plug into the same layer. The practical things to coordinate: throttle ownership (only one controller drives the throttle at a time) and, ideally, a shared criterion-selection so we don't end up with two separate throttle PIDs doing the same job. Mechanically there's overlap in Good direction overall — noted for the airspeed-aware / knife-edge thrust follow-up. |
Author
|
Update: full closed-loop validation against JSBSim We now validate the orientation-hold work in a closed loop against JSBSim (LGPL 2.1, via Results over a 22 s hold, altitude span:
Nothing climbs away, nothing falls out — every hold keeps altitude or sinks gently. Replay videos (3D flight path, pilot sticks vs controller outputs incl. throttle, switch positions, FC-estimated vs true attitude/altitude) are in the bench repo: https://github.com/swissembedded/inav-sitl-bench (docs/videos/). Notable findings from the loop:
Roadmap:
If somebody likes the thrill: go ahead — we did not find any reason not to. Set |
The plain attitude holds (INVERT, KNIFE L/R) only had static pitch trims; altitude was an unregulated aerodynamic equilibrium that happened to look stable on a well-trimmed symmetric airframe and drifted 15-40 m otherwise. Export the figure sequencer's altitude assist and apply it in the hold preset path, referenced to the altitude captured when the hold engages. The internal cos-blend fades it out toward nose-vertical, so the prop hang stays owned by the hover throttle controller. JSBSim closed loop, 22 s holds: inverted returns to entry altitude (-0.1 m end drift, was +15..40 m); knife edge holds within 0.7 m (was 5.8 m equilibrium).
…is captured During the entry the transient altitude error deflected the hold target: a knife-edge entry could stall at half the bank (target pushed by up to assist_max while rolling in) and only creep to the preset over tens of seconds. Gate the assist on a small attitude error (<25 deg) and keep the altitude reference tracking while still capturing, so the hold locks the altitude where the attitude settles, not where the switch flipped. JSBSim closed loop: knife L now captures -90 within 3 s and holds 2.0 m altitude span; inverted through a 3 m/s gust: 2.3 m span, ~0 end drift.
Engaging inverted from level is a near-antipodal tilt error: the shortest-rotation cross product barely rises above numerical noise, so the rotation axis -- and with it the whole entry path -- was an arbitrary mix of roll and yaw. Seen as a reproducible ~65 deg heading swing while rolling in. For tilt errors beyond ~150 deg prefer the body X axis (projected orthogonal to the target up, sign kept continuous with the cross product), falling back to the shortest rotation when body X is parallel to the target up (prop-hang entry from a dive). JSBSim closed loop: inverted entry heading swing -1 deg (was +65), knife entries unchanged.
…reshold Replace the dot < -0.87 if/else with a linear ramp (tilt error 120..150 deg) blending from the shortest-rotation cross product to the body-X preference. Removes the chattering risk when the tilt error dwells at the former threshold; behaviour at the endpoints is unchanged. JSBSim: inverted entry swing +0 deg, spans inverted 1.7 m / knife 1.9 m / hang 4.3 m.
Author
|
Update: a correction and four controller fixes The correction first: the altitude regulation itself worked where it was wired in - the roll figure held 1.2 m and the prop hang ran on its hover throttle PID. But it was only wired into figures; the plain holds (inverted / knife edge) had no altitude regulation at all, only static trims: the elevator stayed flat at zero while altitude "held". What looked like altitude hold there was a trim equilibrium of the symmetric test airframe, drifting 15 to 40 m under slightly different entry conditions. The bench now plots the FC outputs over time and flies every hold through a 3 m/s gust; an equilibrium cannot fake a disturbance response. Fixes on the branch:
New: closed-loop thrust vectoring. A pusher delta (elevons only) prop-hangs purely on the vectored nozzle, exercising the TVC path incl. inverse throttle compensation. All 8 replay videos are in the bench repo. |
The hold target becomes a persistent attitude quaternion, seeded on the actual attitude when a source engages and slewed toward the requested attitude at fig_roll_rate instead of stepping there. The regulator error therefore stays small at all times and the entry path is an explicit target trajectory (rolling like a pilot into near-antipodal targets moves from the error computation into the slew axis preference). The error function returns to the pure shortest-tilt rotation; the target's twist follows the actual attitude every cycle (free yaw axis, compliance groundwork for held-twist sources). Figure IMPULSE segments re-seed the target so the catch slews from where the spin ends. Floor recovery keeps tracking directly, safety before entry aesthetics. Bench: level1 numerics 82/82 against the float64 mirror; JSBSim suite green (roll 1.5 m / knife L+R 2.1 m / inverted 2.6 m spans, hang holds 6.2 m after the now energy-conserving pull, floor + spin unchanged); inverted entry heading swing 1.7 deg.
An updraft unloads the thrust, the hover altitude PID cuts the throttle and with it the prop wash over the control surfaces - the attitude authority starves and the hang nods up to 75 deg around vertical until the gust ends (found by the automated gust battery; whether it breaks varies run to run, the authority is marginal). The floor keeps the throttle at a configurable minimum while hovering; excess lift is accepted as a climb instead. Found by experiment near the model's hover throttle. Default 1000 = no floor beyond motor idle, behaviour unchanged.
One propeller, one floor: the authority that scales with thrust is the prop wash over the surfaces or the vectored nozzle - the same minimum applies to both steering paths.
Hovering has almost no natural aerodynamic damping and the prop-wash moment responds with a lag, so angle-loop gains that are well damped in forward flight can limit cycle around the vertical: the gust battery showed a growing 1-2 Hz pitch/yaw oscillation with the surfaces at a quarter deflection and the throttle healthy - a phase margin problem, not an authority problem. Same philosophy as the hover throttle learning its hover point: a detector watches the tilt-error zero crossings (0.4..3 Hz band, amplitude above noise) and each detected half wave backs the angle gain off fast; quiet time recovers it slowly toward 1.0. The scale settles just below the stability boundary for the actual airframe, CG and battery state, re-learned on every hang. Active only while PROP HANG holds near vertical; no setting, no persistence.
Small airframes oscillate fast: a 0.7 m model with its small inertia limit cycles at 4-8 Hz where a 1.5 m aerobat sits at 1-2 Hz. Noise rejection is the job of the amplitude gates, not of the band.
Starting every hang at full gain is not conservative: the controller has to oscillate its way down for 1-3 seconds first (amplitude gate plus a few half waves at 0.85 each). The learned scale now freezes at hang exit, is written back to the config and saved to EEPROM on disarm - the next hang and the next flight start at the learned value. A value learned under worse conditions self-corrects upward through the release while hovering quietly. Never writes EEPROM while armed.
An autogyro's rotor is its wing: lift AND lateral-tilt roll authority scale with rotor rpm squared, and the rpm lives on the inflow through the disk. In slow flight or a botched launch the rpm decays, the tilt goes soft, and the aircraft rolls away with the stick at the stop - attitude control alone cannot recover it, and these airframes carry no rpm feedback. The ONE lever that restores authority is thrust (thrust -> speed -> inflow -> rpm). The guard is a box (ROTOR GUARD, permanent id 80) following the altitude-floor pattern. Detector: bank beyond rotor_guard_bank (default 60 deg - an autogyro never flies that on purpose) WHILE sinking past rotor_guard_sink, sustained 300 ms; the sink condition keeps deliberate maneuvers (climbing half-loop turns) from tripping it. Recovery: orientation-hold target wings-level plus rotor_guard_pitch nose DOWN (feed the disk), throttle floor cruise + rotor_guard_throttle_add (deliberately NOT pitch-scaled - the nose is down, pitch-to-throttle would starve the one lever that matters), motor forced running through a low stick, roll/pitch sticks suppressed like the floor (yaw stays steering). Release: bank under 20 deg and sink arrested, or stick takeover (centered once, then a fresh deflection). The altitude floor outranks the guard: height beats rotor rpm. Status: compiles and links (SITL 319/319); the bench proof against the JSBSim Auto-G2 rotor-rpm plant model (tip-over reproduced without the guard, caught with it) is the next step.
All three measured against the JSBSim Auto-G2 rotor-rpm plant: (1) nose-down only while the excursion persists - once the wings answer, the recovery levels off; a T/W<1 autogyro can never climb nose-down and the old release condition became unreachable. (2) release redesigned: wings held level for 1.5 s, NOT sink arrested - height is the altitude floor's job, and the gyro settles in a slow descent at any sane recovery attitude (the old condition pinned the recovery active into the ground). The box stays armed and simply re-trips on the next excursion. (3) minimum 5 s on the recovery before any release: the wings answering is the catch dynamics, not a healthy rotor - the rpm rebuilds from inflow over seconds, and an early release re-tipped DEEPER (-48 caught, released, -138 into the terrain, measured). Time on the throttle floor is the honest rpm proxy without rpm feedback. Proof: starved from 80 m, two catches, minimum altitude 17 m, ends flying level at ias 16 with the throttle returned. The unprotected contrast flight tips to 154 deg and impacts.
The bench replay and gates could not SEE when a recovery owns the aircraft - the box readback only shows ARMED switches, so a figure silently flown under floor override would fake that figure's proof. fc_core publishes floor-armed / floor-recovery / rotor-guard-recovery as a bitmask in debug slot 7 (cycles through the simulator reply at 125 Hz); altitude_floor exports the armed state. The rotor guard setting descriptions now carry the SITL-proven Auto-G2 tuning (bank 45, throttle_add 380) and the reason the defaults differ.
Daniel: a piloted trajectory is not predictable - the 3 s linear lookahead read every fast loop downline (30 m/s vertical, pulls out in 15 m at the 4 g limit) as a 90 m crash and silently co-flew the loops (measured via the safety-word instrumentation: 6-7 percent of figure frames under floor override across the fleet). The engage is now the original contract: sinking through the floor line triggers the recovery, nothing else. Above the line the sky belongs to the pilot; the height below the line is the recovery budget the user chooses with alt_floor_altitude (a dive catch consumes 15-25 m, documented).
Yaw-from-course and the centrifugal compensation both assume coordinated forward flight - heading follows course and the lateral acceleration is v x omega. At aerobatic attitudes the assumption is broken and the aiding actively bends the estimate: measured in SITL with truth GPS injected, a prop hang carries +4.6 deg median pitch bias and 19.2 deg peak tilt divergence (clean without GPS: 0.0/2.4), inverted +7.1/16.5. ahrs_gps_aiding_max_tilt (default 60 deg, 0 = off): beyond this tilt from level both aiding paths fade out instantly - the raw accelerometer is the lesser error there - and fade back over 2 s after returning below the limit. Normal flight, including steep turns under 60 deg, keeps full GPS support. Proof (same truth-GPS A/B): hang +1.1 deg bias / 3.7 deg divergence, inverted -0.4 / 3.9 - back at the no-GPS baseline, with GPS live. imuConfig PG version bumped for the new field.
The pilot overrides the autopilot - held sticks keep steering, so a full held rudder drives a spin straight through the floor (measured: the recovery cannot arrest an autorotation whose driver stays live). The floor catches once the sticks are released; no yaw neutralization in the recovery. This is the design, now stated in the setting.
The guard's only lever is thrust, and the pilot owns the thrust: with the throttle stick at idle the guard neither trips nor keeps a running recovery (instant release). A tip in the flare must not force a go-around, and a rollout tip must never get power on the ground. SITL-proven on the Auto-G2 (gated): guard armed, throttle pulled to idle, rotor starves and the gyro tips - recovery bit stays 0 and the FC throttle output stays at idle throughout; the tip pair (starve at 1120 us, above min_check) behaves unchanged on the same binary.
Companion to the slot-7 safety word: exposes the estimator's EFFECTIVE accelerometer weight (1000 = full trust) so the bench can verify the rate-ignore gate actually silences the centrifugally poisoned acc during sustained spins instead of trusting the label.
Daniel's call after maintainer-psychology review: the whole suite must look like a small change you can switch off. Feature bit 5 (recycled unused slot, GEOZONE precedent), CLI 'feature FW_AEROBATICS': - off (default): none of the 12 boxes exist, the Modes tab looks exactly like upstream; the AHRS GPS-aiding tilt gate is bypassed (weight pinned 1.0); crash detection inert - behavior bit-identical to upstream with the feature off - on: the full aerobatics suite (holds, figures, floor, rotor guard, hover throttle, TVC, crash detection, tilt gate) - config.c boot scrub no longer clears bit 5 (it is a real feature now)
- figure sequencer: aborting mid-IMPULSE/SPIN (box off, disarm) left the transient command flags latched; pidOrientationHold then kept applying stale open-loop full-rate commands AHEAD of the floor/rotor recovery target, defeating the catch. The flags now die with the figure (verified at the pid.c impulse branch). - MSP2_INAV_SET_FIGURE_SEQUENCE: validate the segment type BEFORE writing the PG - a rejected frame no longer leaves half-written parameters behind. - MSP outbuf: >512KB targets without FLASHFS kept a 512 B reply buffer while the full box-name list is ~736 B - serializeBoxNamesReply() failed and broke the Modes tab. Non-FLASHFS + USE_ORIENTATION_HOLD now gets 1024 B (the SITL-only bump moves to the generic path).
- debug[6] (acc weight) and debug[7] (safety word) are SITL-only now: raw debug[] writes on a real target clobber the user's selected debug channel - common.h: USE_ORIENTATION_HOLD/USE_THRUST_VECTORING/USE_CRASH_ DETECTION fold into the EXISTING >512KB block (no second gate, and flash-tight F411/F722 no longer pay for crash detection); SITL enables all three in its target.h - docs told the truth already except: altitude_floor.h still said 'predicted' (the law is breakthrough), fc_core's priority comment omitted the rotor-guard recovery, orientation_hold.h named the wrong slew setting for preset entries, the held-twist comment described a path that does not exist, one German word - regimeGainFreeze resets the limit-cycle detector with the regime (a frozen half-wave no longer backs the learned gain off on re-entry); the hover elevation 45 exists once, named
…orbit Daniel's contracts, each bench-proven on the floor_spin scenario: LATCH - the pilot sets the floor high enough; if it has to catch while ANY aerobatic mode is active (every hold, every figure, the sequencer, 3D lock - switch forgotten), that mode is latched OUT: it must not restart when the recovery releases and dive straight back in a loop. The latch clears only when the pilot switches the mode away. ORBIT - back at the floor the aircraft does NOT hand back: it circles the breach point and waits, the pilot gets time to collect themselves. Healthy position estimate -> the REAL fixed-wing loiter flies it (forced poshold anchored on the breach via the geozone flag path; the anchor is injected in the POSHOLD initialize because the forced event re-fires per RX cycle and would re-anchor 'here'). No healthy estimate -> constant-bank circle (needs no heading). nav_fw_loiter_radius must match the airframe speed (documented: R >= v^2/(g tan bank) - hunting at full bank otherwise); the recovery climb-throttle floor stands down while the nav owns pitch and throttle (measured: it ballooned the orbit 70 -> 212 m). TAKEOVER reads RAW receiver sticks, not rcCommand: the fixed-wing nav writes rcCommand to fly the loiter and the mix released the orbit by itself (measured). TELEMETRY - the radio speaks it (EdgeTX voice via the CRSF flight-mode text): FLOR/FLOF on floor arm/disarm (HRST transient idiom), CTCH during the catch, ORBT while orbiting.
The mag correction is a cross product - its torque scales with sin(error) and vanishes at 180 deg even though the error is maximal. After a flat spin the estimate parked there (measured: 184 deg off, stable 60+ s; GPS-COG is equally blind, same idiom). Two stages: past 90 deg the error vector is rescaled to full pull (walks off the saddle), and if the residual stays past 90 deg for a full second the estimate is re-seeded about earth Z so mag north snaps into place - the mag-driven sibling of the existing multirotor GPS yaw reset. Post-spin heading recovers to a few degrees within the catch phase (bench-measured); a clean sustained 27 deg circle holds 0.1 deg yaw error with GPS+mag and 6 deg on mag alone.
The nav poshold honors pilot pitch as a climb-rate override, so a panic-held down-elevator rode the orbit back through the floor (measured: 24 m under a 55 m line, ground impact on the chopped dive). A held stick is not a takeover: sinking back through the line now drops the orbit and re-engages the aggressive climb (attitude force + throttle floor) until the height is recaptured. The SITL safety word gains bit 32 (orbitViaNav) so the bench can discriminate the nav loiter from the degraded constant-bank orbit, and the orbit debug slots 4/5 (heading error to / distance from the breach anchor) replace the leftover 4242/1111 init-path markers.
Pure code motion, no behavior change - proven by the full SITL gate suite (19 shows, floor trio, fig_abort, gyro pair/land): - pid.c: the orientation-hold rate controller body moves to flight/orientation_hold.c (orientationHoldApplyRateTargets); the pid hook is a thin adapter mapping per-axis state through oholdAxisRate_t (118 -> 21 lines, three module includes dropped). - servos.c: the TVC input-row computation moves to flight/thrust_vectoring.c (thrustVectoringApplyInputs); the mixer keeps a one-line call. - hover_throttle.c: the recovery throttle floors move to their owning modules (altitudeFloorClimbThrottleUs with its nav-orbit suppression, rotorGuardThrottleFloorUs); the throttle path takes the highest claim. - fc_core.c: the SITL safety word is composed by the module (orientationHoldDebugSafetyWord), the core loop only writes the slot. - crash detection defaults OFF (explicit opt-in upstream; the bench provisions it ON for the crash proofs). Knife left/right and the spin family were already one mechanism (one preset table, one earth-vertical distribution for 4 attitudes x 2 directions); MSP2_INAV_ORIENTATION_HOLD_TEST stays for the level-1 bench mirror and is excluded at PR-slice assembly instead.
The reseed sat in the core Mahony mag update ungated - it ran for every user with a compass, not just 3D flight. The failure mode it fixes (heading estimate parked on the antipode) was only ever measured after a sustained sub-cruise flat spin; a normal fixed wing never enters that regime. Gate both halves (the sub-90 error escape and the hard re-seed) behind feature(FEATURE_FW_AEROBATICS), matching the tilt gate in the same file, so the shared estimator is byte-identical with the feature off.
…batics Crash detection was gated behind FEATURE_FW_AEROBATICS plus a crash_detection bool setting - wrong for a standalone, platform-general feature. Give it its own runtime feature bit (recycled 1<<9), the GUI-visible master enable, default off. The redundant config group and bool setting are removed (the feature bit IS the enable), so it no longer depends on the aerobatics suite - a crash detector is useful on any airframe. SITL builds clean.
Standalone thermal soaring, compile-gated by USE_SOARING (> 512 KB targets + SITL), runtime by the pilot's SOARING mode. Independent of the aerobatics suite - own PR. flight/soaring.c: - net (total-energy) variometer from the pitot airspeed and the vertical estimate, own polar sink compensated with the EXACT cos of the bank (ArduSoar uses a small-angle approximation that drifts at the 35-45 deg thermal bank) - thermal centering: sin/cos gradient of the vario deviation over each turn (points at the strongest climb) plus a wind-drift shift - the thermal is locked to the air mass so the circle slides at wind * dt, unscaled (ArduSoar scales wind by climb/strength without physical basis and blows up in weak lift) - cruise <-> thermal state machine, altitude band, drift clamp Seven SOAR_* parameters (PG_SOARING_CONFIG). SITL debug slots 0-3 carry the net vario and centre estimate for the bench. Builds clean; the nav loiter integration and the bench centering gate follow.
The floor-orbit anchor (navActivateFloorOrbitAt / floorOrbitTarget) was named and gated for the aerobatics floor only. Rename it neutral (navForcedPosholdActivateAt / navForcedPosholdAnchor) and widen the gate to USE_ORIENTATION_HOLD || USE_SOARING so the altitude floor and thermal soaring share the same loiter machinery - without renaming USE_ORIENTATION_HOLD itself (still the aerobatics gate). Soaring now activates the anchor on the thermal-centre estimate at thermal entry, re-asserts it each cycle (the wind-shifted centre), and clears it on exit. Floor behaviour is unchanged: floor_dive and floor_spin gates both PASS on the renamed path.
Bench glide-polar sweep (phugoid averaged) gives the 1.8 m motor glider a min sink of 0.61 m/s at 8 m/s (best L/D 13.2) - a healthy polar, confirming the plant model is fine. Set the net-vario sink compensation default to 60 cm/s (was a guessed 80). Measured, not guessed.
Catch up the autogenerated CLI reference with settings.yaml (soar_*, rotor_guard_*, ahrs_gps_aiding_max_tilt, crash_detection now a feature bit). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…TICS The flag grew from gating just the orientation-hold mode to gating the whole fixed-wing aerobatics core (orientation holds, figure sequencer, altitude floor, rotor guard, the MSP handlers). Its name no longer matched its scope, and the runtime side is already FEATURE_FW_AEROBATICS / cli "FW_AEROBATICS". Pure rename of the compile flag; the ORIENTATION_HOLD_MODE flight mode and the BOXINVERTED/BOXKNIFE* boxes keep their names. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
sqrt(r^2+p^2+y^2) < 15 is exactly r^2+p^2+y^2 < 15^2 - drop the per-loop sqrt on these flash- and cycle-tight boards. Behaviour is unchanged. The comment records why the crash stillness keeps the vector magnitude (not the landing detector's mean-abs averageAbsGyroRates, which blinds to a single-axis wreck) and its own baro-rate + GPS ground speed (the impact corrupts the fused velocity for ~4.5 s). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
MSP2_INAV_ORIENTATION_HOLD_TEST is a bench/HIL level-1 verification hook (inject two quaternions, read back the attitude error and rate target) - no place on a real FC. Gate its handler on SITL_BUILD as well as USE_FW_AEROBATICS (which it needs for orientationHoldComputeAttitudeError). Also add the missing EOF newline to msp_protocol_v2_inav.h. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
The module found and centred a thermal but never touched the throttle - a measure-only soarer. Add the ArduSoar motor handling: while BOXSOARING is active and the aircraft is circling a thermal (thermalling already implies armed + airplane + pitot and altitude within [soar_alt_min, soar_alt_max]), force the throttle to idle so the glider soars on the lift. It is a throttle-to-idle override, not a motor stop/disarm: the control surfaces keep flying the loiter, and normal throttle returns the instant the thermal is left or the aircraft sinks below soar_alt_min (both drop thermalling). Reuse the fixed-wing throttle-override hook the neighbours use - soaringThrottleApply() bends mixerThrottleCommand in mixTable()'s FW branch, the same apply-chain as hoverThrottleApply / orientationHoldLoadGovernorThrottle, not the getMotorStatus() -> MOTOR_STOPPED path (that hard-stops a multirotor's PID-mixed motors; soaring wants a soft idle). Applied before the aerobatics paths so an altitude-floor / rotor-guard recovery climb can still override the idle. No new setting: thermalling + soar_alt_min already encode the condition. The motor state stays observable via soaringThermalling() and the throttle log; SITL adds debug[4] = the applied idle throttle to the existing SITL_BUILD block. FW only, gated on USE_SOARING; a non-soaring build is byte-unaffected. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Reduces the aerobatics CLI surface (maintainer feedback: ~30 settings is
too many) without losing runtime tunability. The 11 removed settings are
values the controller either regulates away or that are fixed airframe
properties, not things a pilot should pick:
ohold_inverted/knife_left/knife_right_pitch_trim, ohold_knife_speed_ff
feed-forwards the integrating throttle assist regulates to vz->0;
seed at neutral 0 (field kept, no CLI setting).
ohold_hover/inverted/knife/figure_gain
seeds for the limit-cycle gain learner, which converges to the
highest non-oscillating gain from any start; fixed 100 %, read back
on the new DEBUG_FW_AEROBATICS blackbox channel instead of a setting.
ohold_hover_thr_min
redundant with INAV's motor idle; the hover floor is now just
getThrottleIdleValue(). Field removed (PG_HOVER_THROTTLE_CONFIG 3->4).
ohold_hover_baro_weight
fixed init 100 (field kept, runtime-tunable).
ahrs_gps_aiding_max_tilt
a fixed property of the coordinated-flight assumption, not a knob;
now #define GPS_AIDING_MAX_TILT_DEG 60, on/off stays on the
FEATURE_FW_AEROBATICS bit. Field removed (PG_IMU_CONFIG 3->4).
The two PG version bumps reset hoverThrottleConfig and imuConfig to
defaults on flashing. Kept fields keep the reversibility Daniel asked for:
a future CLI setting can be re-added without touching the control code.
Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…ahead The floor is the panic safety net: diving low, the pilot forgets to flip the aerobatic mode switch back, and whatever mode is selected (any hold, figure, or spin) the floor switches it OUT and flies a normal stable upright attitude that pulls up and away - a stable attitude is what prevents the crash. Also documents the mode LATCH (the interrupted mode stays locked until the flight-mode switch is cycled, so a held/forgotten switch cannot drop straight back into the ground). Fixes a stale claim: the doc said a "predicted breach (sink rate looked ahead a few seconds)" triggers the recovery. The firmware deliberately does NOT predict (altitude_floor.c: "A piloted trajectory is not predictable ... the old 3 s lookahead"); sinking THROUGH the floor line is the trigger, matching the alt_floor_altitude setting description. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
The floor catch latches the interrupted hold/figure box out so it cannot restart on recovery release and dive back into the floor (the fly-up / fall-back loop). But the latch was cleared by orientationHoldResetSource Tracking(), which fc_core calls whenever ORIENTATION_HOLD_MODE is inactive - and the latch is exactly WHY the mode is inactive (it blocks the interrupted box). So the moment the recovery ended, the reset cleared the latch, the box re-engaged, and the aircraft flew straight back into its aerobatic attitude on the next stick touch (measured: brief stick touch after an inverted catch drove the roll back to 176 deg). Clear the latch only on a real exit - the pilot deselecting the latched mode (orientationHoldFloorLatchTick) or disarm - never just because the latch itself made the hold un-requested. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…switch cycle Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
swissembedded
force-pushed
the
feature/quaternion-attitude-hold
branch
from
July 20, 2026 08:26
b8b0891 to
ea480cc
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
What this is
This PR adds an attitude-anywhere flight mode family for fixed wing: flip a switch and the plane holds any orientation - sustained inverted flight, knife edge (either side), or a prop hang with hands-free hover throttle. On top of that sits a figure engine that flies rolls, loops, point rolls and whole scripted aerobatic sequences with altitude and airspace gates.
Everything is built around one idea: stop thinking in Euler angles. The controller computes the shortest 3D rotation between the estimated and the target attitude directly on the quaternions (reduced-attitude / tilt control, heading-free by construction). There is no gimbal lock and no special-casing at pitch 90; the near-antipodal engage (level to inverted) resolves deterministically by rolling about body X, like a pilot. A loop is just "pitch rotation, 360 degrees, cumulative".
Left/top: engaging the four holds from level flight and bailing out back to ANGLE - reduced-attitude tilt error, untuned default gains on a generic SITL plant (damping comes from airframe aerodynamics and per-model tuning). Right/bottom: a gated Immelmann sequence and the prop-hang hover throttle. Every plot is reproducible with the bench repo linked below (
python bench.py scenarios / sequence / hover).What it does
INVERT,KNIFE L,KNIFE R,P-HANG, plus3DLOCK(capture and hold the attitude you are flying right now). Sticks stay live and command rates on top of the hold - you fly relative to the held attitude.TVC ROLL/PITCH/YAW(61-63) with thrust-based gain compensation (tvc_gain,tvc_thrust_comp) - vane authority rises when thrust drops, instead of coupling vanes rigidly to the control surfaces.FLOORbox): a switchable training floor. Predictive engage (position + 3 s of sink) catches the plane above the configured minimum altitude, climbs it out, and hands back control. Switch off to land.F ROLL,F LOOP,F 4PTfly single figures;F SEQruns a 16-segment script (roll/pitch rotations, timed holds, open-loop impulse kicks for snap entries,WAIT_ALTclimb gates,WAIT_TIMEpauses, andWAIT_POS- fly back toward home between figures, so the sequence respects a confined airspace). An altitude assist holds height through rolls and in the plain holds (referenced to the entry altitude, engaging once the attitude is captured).What it deliberately does NOT touch
PID_LEVELgains - the same structure as ANGLE, just with a quaternion error instead of Euler errors.USE_ORIENTATION_HOLD,USE_THRUST_VECTORING) and lives in new files (orientation_hold.c,figure_sequencer.c,altitude_floor.c,hover_throttle.c,thrust_vectoring.c); the diff in shared files is small and mode-guarded.New CLI settings
ohold_inverted_pitch_trimohold_knife_left_pitch_trim/ohold_knife_right_pitch_trimohold_hover_thr_p/i/dalt_floor_altitude/alt_floor_margin/alt_floor_climb_pitchtvc_gain/tvc_thrust_compfig_roll_rate/fig_loop_rate/fig_point_dwellfig_assist_z_gain/fig_assist_vz_gain/fig_assist_maxHow it was verified (no test flights yet - that is what this RFC is for)
scipy.spatial.transform.Rotationand the published intrinsic-ZYX convention (16/16), including INAV'saxisAngleToQuaternionconjugate convention.MSP2_INAV_ORIENTATION_HOLD_TEST) evaluates the error function on injected quaternion pairs - 82 test vectors vs a float64 reference, worst float32 deviation 0.0001 deg. The same script runs over USB against real F4/F7 hardware, props off.MSP_SIMULATORHITL injection -> real AHRS -> real controller -> mixer -> back into the plant) plays through every mode: all holds from upright and antipodal starts, pitch-90 crossings, ANGLE bailouts, the altitude floor catch, TVC compensation, single figures, a gated Immelmann sequence, 3D lock, and the hover hang. A second, full-aerodynamics plant (JSBSim: stall table, wind gusts, thrust vectoring) flies every hold through a 3 m/s gust; 8 replay videos with FC-output traces. Bench repo: https://github.com/swissembedded/inav-sitl-bench (GPL-3.0).Looking for testers
This is SITL-proven but not yet flight-tested - and here is the honest part: my 3D-capable airframe is on order, but building and maidening it will take me a while. So anybody with the time, the skills and a suitable aerobatic model (ideally with TVC) can beat me to the first real flight:
swissembedded/inav:feature/quaternion-attitude-hold(based on current master)swissembedded/inav-configurator:release-9.1-ourssmall_angle = 180, start high, verify the level-1 numerics over USB first (props off). Expect the worst case when testing: assume the model can be in any attitude with any deflection when you take over - altitude is your friend, keep the bailout switch under your thumb.Taking back manual control is guaranteed by design (all paths verified in code and SITL):
switching the mode box off drops straight back to ACRO; ANGLE and HORIZON sit above orientation hold in the mode priority chain, so the bailout switch overrides a still-active hold box; MANUAL passthrough overrides everything except failsafe; the figure sequencer aborts instantly when its box goes off; the hover throttle hands the throttle back the moment the stick leaves the mid deadband; failsafe behavior is unchanged from stock.
Feedback wanted on: box naming, whether the figure sequencer belongs in this PR or a follow-up, and defaults for the new settings.