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 16 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.
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
1 participant
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, no special-casing at pitch 90, no roll/pitch ambiguity when inverted — 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 optional altitude assist holds height through rolls via an earth-referenced elevator offset.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. 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.