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 62 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.
The one-shot widened window deadlocked one layer deeper: any non-simulator request between frame bursts (an arming-flags poll, a setting read) consumed the widening, and since only a tick cleared the flag, the next simulator frame found the window already spent. Anchor semantics instead: every arrival restarts a fresh sub-tick creep window at the current simulated time, ticks re-anchor the 1 ms grid with a monotonicity high-water mark. Deadlock is now structurally impossible - simulated time can always creep just far enough to parse whatever arrived, and never runs more than about a millisecond past the last received data.
The stall detector caught creep values around 2^64: the anchor written by the TCP receive thread was read torn by the main thread, an anchor in the future freezes the clock until the next arrival. A mutex around tick, arrival and the lockstep micros path (cheap at SITL call rates) plus an underflow guard fixes it.
The stall reporter showed the last failure mode: the final in-window serial pass can land right at the creep cap, putting its next execution beyond the frozen ceiling while the arriving bytes were still being enqueued - the clock freezes with work queued. A pending-bytes counter (enqueue/read, maintained by the TCP driver) keeps the window open until the queued bytes are consumed; the arrival anchor is also set AFTER the enqueue so a woken pass cannot outrun the buffer fill.
Simulated time after N frames must be exactly N milliseconds. Creep chains from request bursts between frames could run micros() past the next grid point, and adopting the high-water mark into the tick base stretched the grid permanently - measured as 14 percent clock inflation against the injected sensor stream on a faster-than-realtime bench (the AHRS integrated 205 deg/s from a 180 deg/s gyro). The grid is now strict; the monotonicity clamp lives only on the micros() output, a short self-correcting flat spot instead of accumulating drift.
During the rotation roll and pitch are actively regulated FLAT - the controller keeps the plane level and damps the wobble - while only the yaw axis autorotates. FLAT SPIN mode (its own box, like INVERTED): the target is the level attitude with the usual free yaw; the pilot's rudder stick drives the autorotation (full stick saturates the yaw rate loop = full rudder, like a real spin), releasing the rudder stops the rotation with the attitude still held flat, releasing the box recovers normally. No altitude assist, a spin descends by design; the altitude floor preempts globally. FIGSEG_SPIN (p1 turns with sign, p2 rudder percent, p3 timeout): the programmed variant with a wrap-aware turn counter, rudder open loop while roll/pitch stay closed on the flat target. Enter stalled via a preceding IMPULSE segment.
The persistence change left the learned scale globally active after a hang exit: switching from the hover into another hold ran inverted or knife edge at the low learned factor. The stored value still persists for the next hang; outside the hover regime the angle gain is full. The freeze/write-back also runs on mode exit now - landing straight out of a hang and disarming must not lose the learned value.
Three sweep rounds under lockstep against the aerobat3d plant: the hold span shrinks monotonically from 28.7 m at the old 25/30 defaults to a 5.0 m plateau from P 55 upward with D at 100 (including a 3 m/s downdraft gust), with the QUIETEST throttle of all configs - higher damping ends the chasing. The plateau itself is the altitude estimator, not the PID: the estimate wanders 3.5 m and sits 2.5 m above the truth while hovering.
Releasing a hold far from level dropped the full attitude error onto the Euler level controller as one step. A prop hang exit whipped: ~90 deg of error at near zero airspeed commands full rates, the airframe has no authority to arrest the resulting pitch rate at the horizon and slices through to nose down (SITL lockstep: +90 to -90 deg pitch in 0.4 s with a 180 deg roll-over, settled only after 1.2 s, 9 m altitude loss). The hold now keeps the aircraft for one handover transition: on release toward ANGLE with more than 30 deg of tilt it slews its target to level at ohold_entry_rate and hands over once the attitude is within 10 deg. A deflected stick aborts to the pilot instantly; a 3 s timeout and any re-engaged hold source also end the handover. Exits to ACRO stay raw, floor recovery ends level by construction. SITL lockstep hang exit after: level in 0.4 s, 8 deg undershoot, no roll excursion, 4.8 m altitude loss. flat_spin/loop_fig exits near level are untouched (handover correctly does not engage).
In a knife edge or inverted hold the attitude controller owns the surfaces and the pitch assist owns the flight path, but the throttle is a frozen pilot stick: if the speed is too low for the attitude's lift (fuselage lift at knife edge, inverted wing lift) the hold can only sink. The throttle criterion of these holds is vz -> 0: a slow integrating trim (capped +-150 us) around the pilot's stick adds throttle while the hold sinks and takes it back while it climbs, plus a small vz damping term. The pilot stays the base: moving the stick moves the operating point, a deliberate throttle cut stays a cut, 0 disables. This is the second instance of the declared-per-hold throttle criterion (prop hang: altitude PID owns the throttle; knife/inverted: vz trim around the pilot). A NAV ALTHOLD combination was considered and rejected: its fixed wing controller assumes upright forward flight and owns pitch, which conflicts with every attitude this feature exists for. If the pilot engages a NAV mode it outranks the hold as before. SITL A/B (lockstep, JSBSim): spans improve modestly (knife 1.8 -> 1.6 m, inverted 1.6 -> 1.4 m; low-throttle knife 2.4 -> 2.1 m) because the bench airframe's pitch assist already carries most of it; the trim stays well inside its cap. The headroom matters on airframes with less fuselage lift.
A slow roll lost ~15 deg of course per roll (SITL truth; the FC estimate walked the other way): the figure target rotated about the body axis, so the roll axis followed wherever the nose drifted, and the reduced attitude error is heading-free by construction - nothing pulled the line back. Figures now anchor their trajectory to the heading captured at figure start (q_yaw(psi0) composed with the RP target) and regulate the FULL attitude error. Verified identity (bench math_verify section G): the full error differs from the reduced one exactly by the twist about body-up, so tilt regulation is unchanged and the line-hold is purely additive. Segments that change the heading on purpose (WAIT_POS banks toward home) or fly open loop (impulse, spin) release the anchor and it re-captures on completion; pilot holds, 3D LOCK and the flat spin stay heading-free as designed. Two coupled fixes this needed: - the target slew must OUTRUN the figure rate (rate + 90 dps): at exactly the figure rate it chases the rotating target saturated and the heading correction never closes - anchored figures slew the full rotation (rate-limited slerp): the reduced slew works on the up vectors only and simply inherits the drifted heading into the target SITL lockstep: slow roll course loss 17.4 -> 0.8 deg (FC frame 0.0), loop course 0.4 deg, altitude spans unchanged.
The throttle stick outside the mid deadband no longer hands the whole throttle back to the pilot: it commands a climb rate (full deflection = 2 m/s) by RAMPING the altitude reference, and the unchanged altitude loop tracks the moving target. Releasing the stick latches wherever the ramp stopped. Direct pilot throttle on top of the altitude PID would be two controllers fighting over one actuator - the classic oscillation; commanding the target keeps a single loop, so the pilot can correct without exciting one. The reference clamps to the reachable neighbourhood (windup guard at the throttle floor / saturation) and a stick slammed to the bottom stays a hard throttle cut (bailout). SITL lockstep: 55% stick = +1.12 m/s measured (+1.1 commanded), release holds the new altitude, sink command respects the throttle floor, zero throttle reversals in any phase.
Hovering on the prop pollutes the accelerometer Z (the specific force never matches the kinematics the way it does in forward flight) and the inertial altitude estimate wanders meters around the truth while carrying a bias - the estimator, not the throttle PID, limits how well the hover holds altitude. While the hover throttle owns the altitude the baro deserves more trust: apply ohold_hover_baro_weight (x100, default 1.0, the sweep winner) as a floor over inav_w_z_baro_p; 0 keeps the global weight. Forward flight is untouched. SITL lockstep A/B including the 3 m/s gust: hang truth span 5.2 -> 4.4 m, TVC hover 6.3 -> 5.0 m, estimate bias down ~0.8 m on both.
Member
|
You may already be aware of these, but I thought I'd mention them. I was working on integrating your jsbsim work when these two popped up in the quat hold:
|
…tude The spin axis was an implementation choice, not physics: the reduced attitude error leaves rotation about the EARTH VERTICAL free in every attitude, so the pilot's rudder command is now distributed onto the body axes via the earth-up direction in the body frame instead of being wired to body yaw. At flat/inverted that lands on the yaw axis (unchanged behavior), at knife edge on the pitch axis, at the hang on the roll axis. Body rates along this axis provably leave the tilt untouched (bench math mirror section H), so holding and spinning never fight. The attitude selector now picks the HELD attitude while the FLAT SPIN box picks the behavior: SEL off = flat spin, INVERTED = inverted flat spin, KNIFE L/R = knife edge spin, PROP HANG = torque roll. No assist and no trims in any spin variant; positive rudder = the same rotation seen from above regardless of attitude. The figure SPIN segment uses the same distribution. SITL lockstep: flat 12.5 turns/10 s (regression clean), inverted 12.3 turns/10 s ending still inverted on release, knife 6.4 turns/10 s holding the knife within 11 deg mean; all three stop the rotation within half a degree of residual turn when the rudder centers.
The body axis nearest the vertical receives the rudder stick with its own positive sign: right rudder yaws the airframe right at flat AND inverted, so the rotation seen from above reverses in the inverted flat spin - exactly like a real aircraft. At the knife edge the stick maps to positive body pitch. SITL: flat -12.5 turns/10 s, inverted +9.0 (earth sense reversed as expected), knife +3.8 holding the edge.
Daniel's direction: the normal-flight gains are the REFERENCE, every hold regime runs on a single learned scale of them instead of its own gain set - transitions overshoot exactly where a regime is untrained. The hover limit-cycle learner (zero crossings with amplitude gates, fast attack, slow release, freeze at exit, EEPROM save on disarm) becomes a per-regime table: hover (unchanged, body pitch/yaw axes), inverted, knife and figure (tilt axes roll/pitch). Flying the same figures repeatedly converges their scale - they get better with every flight. The target regime's scale applies from the moment the source switches, so the entry slew already flies with it. Spins and the special sources (lock, floor, exit handover) learn nothing; normal flight always runs the reference gains. New settings ohold_inverted_gain / ohold_knife_gain / ohold_figure_gain (firmware-maintained, editable), PG bump resets the stored group once. SITL lockstep: hover regression behaves identically to the pre-refactor build under equal starting values (the PG bump wipes the learned hover gain once - first battery relearns, the disarm save then persists it: measured 100 -> 64 across one battery). A deliberately hot LEVEL P (160) produces a single damped overshoot on the bench airframe, not a limit cycle - the learner correctly stays passive there (no false positive); real limit-cycle material exists only in the hover regime in this plant.
Above the nose-elevation gate (60/45 deg hysteresis) the thrust carries the weight and the hover altitude controller owns the throttle - now also when a knife edge or inverted hold is pulled up into a harrier, not only under the PROP HANG box. Below the gate the vz trim remains the indirect energy path: the alpha continuum (knife -> harrier -> hover) becomes one mechanism whose direct-thrust share is the existing tilt compensation. The climb-rate stick references the throttle position captured at engage, so entering the hover regime out of a pull-up at cruise throttle does not read as a climb command (the hang entered at mid stick behaves exactly as before; SITL span 4.4 m, regression clean).
The fuselage side force carries the weight at the knife edge and scales with v^2: flying slower needs MORE nose-above-horizon angle immediately, not only after an altitude error has built up for the reactive assist. Without an airspeed sensor the own throttle is the v^2 proxy (T ~ v^2 in steady flight); the prop wash over the tail linearizes the theoretical throttle-to-angle hyperbola, so a linear term around the mid-throttle trim point is the honest model (same conclusion as the classic throttle-to-rudder mixers and ArduPilot's airspeed-based knife-edge feedforward, which trade under the same physics). 0 disables (default). SITL lockstep A/B, knife edge through throttle steps 1650/1400/1900: altitude band 2.9 -> 1.7 m with ff=12.
Daniel's spec for the energy side: the speed the pilot entered with is KEPT as the forward component - the assist base scales the pilot's throttle by cosRef/cos(theta), so a rising nose (assist, speed feedforward, harrier transition) no longer bleeds speed through the shrinking horizontal thrust share (the forward complement of the hover PID's 1/sin compensation; thrust-based, no sensor). When the model sinks the vz trim raises the operating point as before - and when the HOLD OSCILLATES it now does too: an oscillating knife edge usually means the surfaces are starving, more airflow is the physical cure while the gain learner only treats the symptom (signal comes from the regime limit-cycle detector). SITL lockstep regression with ff=12: knife span 1.5 m, inverted 1.6 m, zero drift, hold errors unchanged.
Field observation: a good regulator masks the approach to the envelope edge - the attitude stays clean while the surfaces silently work toward saturation, then everything lets go at once (the ramp becomes a cliff; the pilot noticed the stall only when the controller could no longer compensate). The mean control effort is therefore the early escalation criterion, ahead of sinking and far ahead of oscillation: the low-passed maximum of |axisPID|/pidSumLimit above 70% raises the knife/inverted assist speed proportionally while reserve is still left. Escalation chain now: effort trend (early) -> sinking (vz trim) -> oscillation (regime detector). SITL regression: clean holds stay below the threshold (no false trigger), spans unchanged (knife 1.5 m, inverted 1.5 m). The positive path needs a stall-capable plant or the real airframe - the bench model has no honest stall hysteresis.
After a crash the motor otherwise keeps running on the pilot's throttle. An acceleration spike above crash_g_threshold (default 6 g) followed by the aircraft lying still within 2 s - rotation below 25 deg/s and the accelerometer resting near 1 g for half a second - disarms with the new reason CRASH. The stillness confirmation is the false-positive filter: a flying aircraft is never still, so snaps, spins and hard gusts cannot trigger it. Hand launch rule: the detector arms only once the aircraft is clearly in the air - nav launch reports flying, or the throttle was held above cruise level for a second - so a hand-launched (or carried) armed aircraft does not disarm from handling bumps. SITL lockstep scenarios: flight + impact + stillness disarms; flight + same impact + continued 200 deg/s rotation stays armed; armed on the ground + bump stays armed.
Aerobatic attitudes shade the GPS antenna and the reported epv lags the real degradation - a decaying fix kept pulling the altitude estimate while the baro knew better (the reason the bench flew BARO_ONLY as a stand-in). The vertical solution degrades first on a thin constellation, so GPS-Z now requires a margin of two satellites over the gps_min_sats fix threshold; below it the altitude stays baro-first while GPS XY keeps working as before. SITL: with a valid fix at 7 sats a +50 m GPS altitude lie leaves the estimate baro-anchored; at 12 sats the estimate follows GPS as intended.
Field experience: after a crash into high grass or corn a SHORT motor burst helps locating the aircraft - a hard disarm would require the arm switch and lose that. The detector now CUTS the motor (mixer forces idle) while staying armed; moving the throttle to zero and up again re-allows it deliberately. The original problem (motor keeps running on the pilot's throttle after an impact) stays solved: holding the stick up changes nothing until the acknowledge gesture. SITL: cruise 0.64 -> crash cuts to idle while armed -> stick held high stays cut -> zero-then-up restores 0.64. The snap-rotation and ground-handling negative scenarios are unchanged (detector untouched).
Bench measurements drove three changes to the stillness confirmation: - The fused vertical speed is unusable right after an impact: a 12 g / 0.3 s pulse drives the INS estimate tens of m/s off and the baro pulls it back only after ~4.5 s, far beyond any safe confirmation window. Stillness now checks the RAW baro rate (PT1, 0.5 s), which is honest half a second after the airframe stops. The window stays at 3 s and the cut fires ~1 s after a real crash instead of ~6 s. - IMU + baro cannot tell a crashed airframe from a coordinated level line or shallow turn (both are rate-still, 1 g, baro-flat - measured as a false cut 1.2 s after a hard pull). With a 3D GPS fix the ground speed (< 3 m/s) provides that discrimination; without GPS the setting description now tells the pilot to keep the threshold above the figure g load. - Stillness tightened to 15 dps / 0.9-1.1 g / 1.0 s confirm, default threshold raised to 8 g. SITL: crash + still (GPS 0 and GPS-less) cuts at still+1.0 s, gesture restores the motor; 11 s of post-spike maneuvering and level lines with a moving GPS fix never cut; the panic-dive floor test with hard pulls stays clean.
The floor recovery flew upright + climb pitch but left the throttle wherever the pilot froze it - a panic chop meant an idle-power climb command mushing at 16 kts into the floor plane, and the stick-low motor stop even turned the motor fully off. Three chokepoints, measured on the panic-dive bench case (throttle chopped, down-elevator held): - hover_throttle: while the recovery is active the mixer throttle gets a floor of cruise throttle + pitch-to-throttle compensation for the recovery climb angle; more pilot throttle always wins. - getMotorStatus: the recovery keeps the motor RUNNING through a held low stick - the same override navigation gets via nav_overrides_motor_stop; the pilot override is the floor switch. - pidOrientationHold: roll/pitch stick rates are suppressed during the recovery (the panic-held elevator fought the recovery target down to -13 deg and flew it under power through the floor to 34 m); yaw stays live for steering. Panic dive from 250 m with the stick held down: catch at the floor plane (min 132 m vs 34 m before), airspeed recovers to 65 kts on the raised throttle, release/re-catch cycles around floor + margin as designed.
The recovery climb already ends at floor + margin (the margin IS the configurable delta above the floor) - now the pilot can also take over early: once the sticks have returned to center after the catch, a fresh roll/pitch deflection releases the recovery immediately. The panic-held down-elevator from the dive does not count (it never centered), and yaw stays a steering input, not a release. Settings docs spell out where the climb stops.
The scheduler-backlog heuristic reads HOST load, not flight-controller load: a SITL loop is paced by the simulator frame stream (in lockstep exactly one 1 kHz tick per injected frame), so a busy host blocks arming with a false SYSTEM_OVERLOADED. Compile-gated on SITL_BUILD - hardware targets keep the check unchanged.
Follow-up to the SITL_BUILD compile gate: a SITL run without --lockstep now keeps the stock check (SITL should stay as close to original behaviour as possible). Only the lockstep mode - where the loop is paced by the simulator frame stream and host load reads as a false scheduler backlog - disables the gate, at runtime via the existing sitlLockstepEnabled option flag.
Author
|
Pushed a large update - the branch moved substantially since the draft
Everything is validated against a 50-case gust matrix (5 held attitudes For testers, so nobody needs a toolchain: prebuilt firmware files are @sensei-hacker thank you for the valuable feedback - your questions |
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
2 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.