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 94 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.
Member
Am I understanding that correctly to say that with these servos, if I switch directly from Only to make them "move slower", you incrementally send intermediate positions. But the servos don't remember what you sent how fast - they are always trying to go to whatever position is currently being commanded.
Does the target loop need send a rate greater than that achievable? |
Author
|
Fair point - the servo always moves toward the command. The effect is subtler, concrete example from the airframe we are maidening (an RC-Factory Veloxity with TowerPro MG92B servos, 0.08 s/60 deg): if the controller commands a surface to x and reverses the command before the servo gets there, the surface only ever reaches x/2. The commanded amplitude never physically happened - fast command content is silently amplitude-clipped by the servo's slew limit. To your question - does the loop need rates greater than achievable: for stability, no, measured (the 50-case gust matrix scores identically against servo-limited actuators, 49/50). But we still think a low-pass on the FC output, matched to the declared servo speed, is the cleaner solution than letting the servo clip silently: commanded and physical trajectory then agree, the FC knows what the surface is actually doing, and transient oddities from writing unreachable steps disappear. On average it does not matter - in transients it does. And sometimes you deliberately want LESS than the servo's full rate - flaps are the classic example: they add area and camber for low-speed lift, and deployed too fast at speed the aerodynamic load rises steeply (v^2) and can overload the mounts, tracks and the actuator itself - so you deploy them slowly on purpose. Same idea for soft control feel or scale-like flying - exactly what smix speed was built for. The widening just lets the same mechanism also express a real servo: this very MG92B (0.08 s/60) needs a speed value of ~625, far beyond the uint8 maximum of 255 (~0.20 s/60) - today not even a mid-range digital servo can be declared. (Also spotted in the fresh CI run: crash_detection.c broke no-baro targets - unused dT/lastBaroAltCm under -Werror. Fixed locally, goes up with the next push; nothing of the current rework is pushed yet.) |
Member
|
Okay, so normal servo - if you try to move it faster than it can physically move, it moves just as fast as it can. What I'm wondering (and truly do not know) is if it would make sense to handle it at higher / earlier level. INAV will never command my Zohd Drift to roll at 1000° degrees per second, because I set the roll rate appropriate for the aircraft. We don't wait until it sends the servo mixer a command for 900° of aileron deflection to try to achieve that rate, it never tries to achieve an unachievable rate of roll. Never sends such a thing to the mixer to start with. So the mixer doesn't have to say "900 degrees of servo deflection is impossible". Might the same general concept apply here? Apply rate constraints BEFORE it gets sent to the servo mixer? Ps - I'm not being polite by phrasing this as a question. I haven't read the code and don't know. |
The fresh CI run on GEPRC_F722_AIO_UART3 (no USE_BARO) caught it: crashVerticalRateCms' dT parameter and the baro statics are unused when the baro path compiles out. Statics and their reset now live under USE_BARO, the no-baro path marks dT unused and falls through to the fused estimate as before. Verified: the failing CI target builds clean.
From the servo discussion on the PR (Jetrell, sensei-hacker): the smix speed field is the right low-pass for the FC output - commanded and physical surface trajectory then agree instead of the servo silently amplitude-clipping fast command content (command x, reverse before the servo arrives, and the surface only ever saw x/2). And sometimes less than the servo's full rate is wanted deliberately - flaps are the classic case, deployed slowly on purpose because the aerodynamic load rises with v^2 and can overload mounts, tracks and the actuator. But the field was uint8 in 10 us/s units: even 255 means a full sweep in 0.39 s (~0.20 s/60 deg). A mid-range digital servo (TowerPro MG92B, 0.08 s/60 deg - the servo on our maiden airframe) needs ~625; the fastest aerobatic HV class (0.05 s/60) needs ~833. Not declarable. speed becomes uint16 with MAX_SERVO_SPEED 1000 (full sweep in 0.1 s = the fastest servo class; beyond that no servo follows). Backward compatible by construction: same unit, so every stored value keeps its meaning; the CLI is text and takes the full range; the legacy MSP messages stay byte-identical and clamp the reported value to 255 - an old configurator shows 255 and can write at most 255, nothing is corrupted. A 16-bit MSP2 message plus the configurator field follow in their own focused PR. Mixer profile PG version bumped.
|
There are other settings Can I assume all these changes are only going to be related to this flight function for now. And not across the board ? We don't want to risk destroying peoples servo's prematurely. This is why I touched on platform type. I'm leaning towards this feature only being applicable to begin with, to a specific fixedwing platform type. i.e. Tractor(single or twin) plane with a tail. Because hovering a pusher, or any of these other 3D style maneuvers just isn't practical for that motor position, unless the pusher has vectored thrust. I have tried in the past to know this. |
…s ceiling" This reverts commit 59638db.
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.
A nose-prop glider can only carry the pitot on a wing, so it sits a lateral distance off the turn axis and reads high on the outer half of a thermal circle, low on the inner - which would bias the net vario and its centering gradient. soaringCgAirspeedCms() rebuilds the CG airspeed as v_measured + yaw_rate * lateral_offset (gyro body rate), and the vario uses it. Parameter soar_pitot_offset [cm, signed]. This is distinct from INAV's GPS-wind 'virtual pitot' fallback; it is a lever-arm correction, the first in our tree (we use the body rate for AHRS centrifugal comp and the load governor, never for the pitot). SITL builds clean.
A pitot mounted a lateral distance off the fuselage centreline - the only prop-free spot on a nose-prop model is a wing - reads v_cg - yaw_rate*r_y in a turn: high on the outer half of a circle, low on the inner. This biases anything that consumes the airspeed while turning (nav loiter, RTH circles, TECS, and thermal soaring's vario). getAirspeedEstimate() now adds it back so every consumer sees the airspeed at the CG. New pitot_lever_arm [cm, signed, + = right]; default 0 = a centreline tube = byte-identical to before. The GPS-wind fallback (already at the CG) is untouched. ArduPilot compensates IMU/GPS sensor position this way but not the pitot - this is the same lever-arm idea applied to the airspeed tube.
getAirspeedEstimate() is now the CG airspeed (turn-rate corrected in pitotmeter.c), so soaring no longer carries its own soaringCgAirspeedCms / soar_pitot_offset - the vario just reads getAirspeedEstimate() and is turn-clean. One correction, in the sensor layer, for every consumer.
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.