Fixed wing: quaternion orientation hold — inverted flight, knife edge, prop hang, figure sequencer (RFC, testers wanted)Feature/quaternion attitude hold#11695
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.
|
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. |
|
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.
|
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 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.
|
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 User documentation for operating and tuning the whole mode family is on @sensei-hacker thank you for the valuable feedback - your questions |
User-facing documentation for the new mode family: the ten boxes and what they do, throttle semantics per hold, the FLAT SPIN combinations with aircraft-referenced rudder sense, the altitude floor's arming / catch / takeover rules, crash detection incl. the no-GPS caveat, learned gains, the physical-trim-first setup order, and the SITL lockstep note. Written so the page can be pasted into the INAV wiki unchanged after merge; settings details stay in Settings.md.
Impact-plus-stillness motor cut is useful on any airframe, not only with the orientation-hold modes it happened to ship inside. Give it its own feature define and decouple it: - common.h defines USE_CRASH_DETECTION unconditionally - crash_detection.c guards USE_CRASH_DETECTION (was USE_ORIENTATION_HOLD) - mixer.c: the crash motor cut now applies independently of the hover throttle - a plane without the aerobatics modes still gets it - fc_core.c update hook and the settings.yaml PG condition move to the new define SITL-verified: crash detection compiles and links with the orientation- hold modes disabled. First step toward extracting crash detection into its own PR (it fits with the landing detector and should serve multirotors too).
The aerobatics suite is ~12 KB and experimental; on a 512 KB board it leaves almost no headroom (measured: F722 at 98% flash, ~9 KB free). Restrict it to targets with room to spare - MCU_FLASH_SIZE > 512 - so F722, F411 and other 512 KB boards are exempt and keep their flash. F405, F765, H743 and the rest keep the feature. SITL has no MCU_FLASH_SIZE (native build), so it enables the suite explicitly in its own target.h, exactly as it does for USE_GEOZONE - the bench needs it. SITL-verified both ways (feature on; and off, which is the 512 KB path).
For a feature this size a reference is not enough - a user needs a walkthrough. Expand the guide with: - a 'learned gains - do not hand-tune these' warning (the four *_gain settings are firmware-maintained; hand-tuning fights the learner) - a Step 0-7 first-flights sequence, physical trim first, each step naming the symptom of a wrong value - a symptom-to-setting troubleshooting table - a note that crash detection is a standalone feature being split out Wiki-ready.
…er too Two changes so a crashed multirotor is covered, not just a fixed wing: - platform gate STATE(AIRPLANE) -> STATE(AIRPLANE) || STATE(MULTIROTOR) (rovers/boats excluded - an impact there is no reason to cut). The in-flight latch was already platform-general: the throttle-held rule covers both, the fixed-wing hand launch just arms it earlier; no GPS needed either way. - the cut now goes through getMotorStatus() -> MOTOR_STOPPED_USER, not by lowering mixerThrottleCommand. On a multirotor the throttle command is added to the per-motor PID mix, so lowering it alone would let the attitude loops keep spinning a crashed copter's motors; the stopped status forces every motor to idle directly. Fixed wing is unchanged in behaviour (SITL crash_test: cut at still+1.0 s, throttle gesture restores). The stillness detector (rates/1g/frozen baro/GPS ground speed) is already platform-neutral. This is the form crash detection should take in its own PR - independent of the orientation-hold modes and useful to every craft that flies and can crash.
|
@swissembedded There are a couple more things I thought I'd raise with you concerning this. And I was wondering how you are going to deal with these conditions ? Presently INAV uses Feedforward and Rates based on the amount of control surface throws you have applied via the servo Mixer travel or Outputs, and the linkage/horn leverage. However passing the angular rate target directly to the servo mixer can lead to considerable rate error if both of these settings are not tuned to suit the specific airflow over the control surfaces at given speeds.
This makes it near impossible to tune the feedforward correctly to suit this type of airplanes rate of rotation. The other thing I was wondering about. Is the application of this based on platform type. And the mode function it is asked to achieve. |
Review feedback round (Jetrell, sensei-hacker): too many settings that ask the pilot what the firmware can know, and modes offered on airframes that cannot fly them. Every item below removes a question: - Load governor (ohold_load_limit, g x 10 - a fact about the airframe: what it may pull; CLI only). Figures and spins fly AT the load budget: the commanded rotation and the target slew trim down with the measured overload (integral, no droop), and DURING a maneuver the throttle bleeds too - a governed rotation at full power just converts into speed and keeps the load (a = v * omega). Plain holds fighting a gust keep full authority (a governed recovery let the disturbance win). SITL: loop exit pull 13 g -> 7 g peak, zero time above 8 g. - Spin rate cap: full rudder commands at most half a turn per second about the earth vertical, independent of the ACRO yaw rate (an uncapped hot tune commanded 1260 deg/s; a real flat spin is ~330). - Crash detection asks nothing anymore: the impact threshold derives from the detected accelerometer (15% below full-scale - a spike that near saturation is an impact on any airframe, and the stillness that must follow is the discriminator, not the g number). crash_g_threshold (uint8, would not even hold a 32 g IMU's range) is replaced by a boolean crash_detection, ON by default. The SITL fake accel now reports a realistic 16 g scale so the derivation holds on the bench. - Hover / knife / inverted throttle gains derived, five settings gone (ohold_hover_thr_p/i/d, ohold_assist_thr_p/i): every one of them was throttle-us per unit of motion, and they all share one airframe fact, the throttle-to-thrust slope - (hover throttle - idle) per 1 g. The hover point is learned online; the gains anchor on the slow-filtered APPLIED throttle, not the engage seed (a 200 us low seed made the altitude loop 36% too soft in the gust matrix). - Thrust-first-guess authority scaling: above the airframe's cruise throttle the commanded hold authority backs off with thrust (surface moment ~ airflow^2 ~ thrust; no pitot carried, and GPS is gone in aerobatic attitudes). The hover regime always keeps the full throw - its airflow IS the wash. Resolves the 45-deg-3D-throw vs smooth cruise tune dilemma without a setting. - Platform gating: a knife edge is held on the yaw effector, so a mixer without one (flying wing: no rudder, no TVC yaw vane) does not offer the KNIFE boxes, and a stale configuration that still maps them is ignored in flight (servoMixerHasYawControl). - docs/OrientationHold.md verified claim-by-claim against the code (one stale statement fixed: after the last segment the sequencer holds LEVEL with assist until the box is released), Settings.md regenerated. Validated: 50-case gust matrix 49 PASS (the one fail is a pre-existing orientation-sensitive funjet TVC hover case, analysed and documented), full maneuver sweep, multirotor crash test (16 g / 3 ms pulse, cut and gesture-restore), all against a bench plant with servo-honest actuator slew (0.06 s/60 aileron-elevator, 0.10 s/60 rudder, +-15 deg nozzle).
|
Following up on the servo point with numbers first, then a concrete question.
@Jetrell the question: would widening smix speed to uint16 (same unit) be acceptable? Small firmware change, but it touches MSP_SERVO_MIX_RULES and the configurator field, so it would be its own focused PR. It would cover the thrust-vectoring vanes too (ordinary smix rules, inputs 61-63). Or is there an existing mechanism we missed that already declares a servo's real slew rate to the FC? Or is smix speed the wrong parameter for this altogether? |
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? |
|
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.) |
|
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.
3 participants
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.