diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md new file mode 100644 index 00000000000..6b210adaeca --- /dev/null +++ b/docs/OrientationHold.md @@ -0,0 +1,352 @@ +# Orientation Hold: 3D aerobatics for fixed wing + +*(wiki-ready: this page is written so it can be pasted into the INAV +wiki as-is once the feature merges)* + +Orientation Hold is a flight mode family that lets a fixed-wing model +hold ANY attitude: sustained inverted flight, knife edge (either side), +a prop hang with hands-free hover throttle, controlled flat spins, and +scripted aerobatic figures flown on a line. It is a quaternion +controller, so there is no gimbal lock and no special-casing at pitch +90 - a loop is just "pitch rotation, 360 degrees". + +Status: implemented and flown in a closed-loop JSBSim SITL simulation +(deterministic lockstep, replay videos); first hardware flights are +upcoming, so treat everything here as experimental. Be honest about what +the simulation proves, because it is uneven. The actively driven +behaviours - the flat-spin family, the scripted figures, and the floor +recovery - command real, measured control authority in the bench. The +static holds (inverted, knife edge, prop hang) do NOT yet: the SITL +aerobatic airframes are near-symmetric and self-trim at those attitudes, +and the bench disturbance is a uniform vertical gust that shifts angle of +attack without producing a rolling or pitching moment, so the controller +is never forced to work to hold them. The holds are implemented but +unproven until a hardware flight - or a deliberately destabilised bench +model - makes the airframe actually want to leave the attitude. + +## Requirements + +- Fixed wing (`platform_type = AIRPLANE`). Multirotors are untouched. +- A target with more than 512 KB flash (F405, F765, H743, ...): the + feature is excluded on F722/F411 builds to preserve their flash space. +- Barometer required (altitude assist, floor, hover throttle). +- GPS optional but recommended: it gates crash detection in flight and + hardens the altitude estimate during aerobatics. +- Thrust-vectoring models: assign the servo mixer inputs TVC ROLL / + PITCH / YAW (61-63) to the vane servos. `tvc_gain` scales them, + `tvc_thrust_comp` raises vane deflection as thrust drops so the loop + gain stays constant (vane torque follows thrust). + +## The modes + +| Box | What it does | +| --- | --- | +| INVERT | holds sustained inverted level flight | +| KNIFE L / KNIFE R | holds a left / right knife edge | +| P-HANG | prop hang: nose vertical, hover throttle owns the altitude | +| FLAT SPIN | controlled spin about the earth vertical; combine with the holds | +| 3DLOCK | sticks centered = hold the current attitude, deflected = rate flying | +| FLOOR | altitude safety floor with automatic upright + climb recovery | +| F ROLL / F LOOP / F 4PT | one-switch figures: axial roll, loop, 4-point roll | +| F SEQ | flies a scripted figure sequence (programmed via MSP) | + +INVERT, KNIFE L, KNIFE R and P-HANG are four separate boxes; the +natural mapping is one multi-position selector switch with one band per +hold, plus a separate switch for FLOOR and one for the figure bands. + +Platform capability is enforced: a knife edge is held on the yaw +effector (rudder or a TVC yaw vane), so on a mixer without one - a +flying wing - the KNIFE boxes are not offered, and a stale +configuration that still maps them is ignored in flight. The laws of +aerodynamics outrank the switch; a wing can still prop-hang, fly +inverted and use every figure. What thrust cannot deliver, no mode can +promise: expect roughly half the work from excess power and half from +the airframe. + +### Holds (INVERT, KNIFE L/R, P-HANG) + +Problem being solved: flying inverted or on the knife edge by hand +means holding constant corrective pressure, and any distraction ends +the maneuver. The hold takes over the attitude; you keep flying. + +- Engaging a hold slews the target from your current attitude to the + hold at `ohold_entry_rate` - no snap, no 180-degree surprise. +- Holds are heading-free: the rotation about the earth vertical stays + yours. Rudder steers in level/inverted flight; at the prop hang the + free axis is body roll (that is the torque-roll axis). +- Sticks carve held angle offsets from the hold (ANGLE semantics, + `ohold_stick_angle`); releasing returns the target gently. Yaw is + always a rate. +- Leaving a hold far from level hands over to ANGLE with a slew to the + horizon, so a hover exit does not whip through nose down. +- Per-attitude pitch trims: `ohold_inverted_pitch_trim`, + `ohold_knife_left_pitch_trim`, `ohold_knife_right_pitch_trim` (the + sides are separate on purpose - prop effects are not symmetric). +- Big 3D throws and a smooth fast tune coexist: above the airframe's + cruise throttle the commanded hold authority scales back with thrust + (the airflow proxy - surface moment goes with airflow squared), so + the same throws that hover the model do not over-deflect at speed. + The hover regime always keeps the full throw. No setting; the closed + rate loop refines the guess. + +### Throttle behavior per hold + +Holding the attitude is half the job; each hold also declares what the +throttle means: + +- **P-HANG**: a hover throttle PID owns the altitude. Your throttle + stick commands a CLIMB RATE around the point where you engaged; + slamming the stick low remains a hard cut (bailout). The hover base + throttle is learned from your own throttle at engage - there is no + per-model hover setting to find. Altitude ownership follows the + ATTITUDE: pull a knife edge up into a harrier and the hover + controller takes the altitude over seamlessly. +- **KNIFE / INVERT**: the base is your throttle scaled so the forward + thrust component keeps the speed you chose; a slow vz-to-zero trim + adds power while the hold sinks (its gains derive from your own + operating point - no settings), and a speed feedforward puts more + nose on the knife immediately when the throttle (the v-squared proxy + without an airspeed sensor) is low (`ohold_knife_speed_ff`). +- **Stall reserve**: sustained control effort toward saturation raises + power while the attitude still looks clean - the early warning. By + the time an attitude degrades, the escalation chain has already gone + through effort trend, then sinking, then oscillation. + +### FLAT SPIN family + +Problem being solved: a spin mode wired to body yaw is only correct in +a flat attitude - inverted or knife-edge spins would be impossible. + +The spin command is a rotation about the EARTH VERTICAL - exactly the +axis the holds leave free - distributed onto the body axes from the +current tilt. The identical mode therefore does: + +- FLAT SPIN alone: the classic upright flat spin, +- FLAT SPIN + INVERT: inverted flat spin, +- FLAT SPIN + KNIFE L/R: knife-edge spin, +- FLAT SPIN + P-HANG: torque roll. + +Your rudder commands the rotation rate, with aircraft-referenced sense: +right rudder spins the airframe right when upright AND when inverted - +seen from above, an inverted spin reverses, like a real aircraft. +Releasing the rudder stops the rotation with the attitude still held; +releasing the box recovers. + +Full rudder commands at most half a turn per second (a display spin, +not a tumble - independent of your ACRO yaw rate), and the load +governor backs the command off with the measured load (see below). A +stalled airframe can still autorotate faster than commanded; at idle +power the rudder has little authority to hold it back - that is +physics, not a tune. + +### 3DLOCK + +Sticks centered: the current attitude is captured and held. Sticks +deflected: pure rate flying, and the lock follows - it freezes on +whatever attitude you had when the sticks came back to center. Think of +it as "hold whatever I'm doing" for improvised 3D. + +### FLOOR (altitude safety floor) + +Problem being solved: this is the PANIC net. Practicing low 3D, a bad +moment ends at the ground - the aircraft dives and the pilot, in the +panic, forgets to flip the aerobatic mode switch back to normal. The +floor is what saves the airframe: whatever mode is selected - any hold, +any figure, a flat spin - sinking through the floor line switches the +aerobatic mode OUT and flies a normal, stable, upright attitude that +pulls up and away from the ground. A stable flight attitude is the thing +that prevents the crash; the floor just makes the aircraft take one. + +- Set the floor with `alt_floor_altitude` (meters above home). The + floor ARMS only after you have climbed above floor + margin once, so + switching it on before takeoff never grabs the aircraft. +- Sinking THROUGH the floor line is the trigger - no prediction, the + crossing fires it - and engages an automatic upright + climb recovery + that OVERRIDES the selected aerobatic mode. It catches out of a dive + with the elevator still held, and out of a spin. +- The recovery brings its own energy: a throttle floor of cruise + + pitch compensation, the motor keeps running through a panic-chopped + stick, and held roll/pitch sticks are ignored (they used to drag the + recovery target down). Yaw stays live for steering. +- The catch LATCHES OUT whatever aerobatic mode it interrupted - any + hold, any figure, a flat spin, all the same. The pilot flies again the + instant they touch the sticks, but the interrupted figure does NOT + restart on its own: it stays suppressed until the pilot switches its + mode OFF and back ON. This is the fix for the fly-up / fall-back loop - + where the figure re-engaged the moment the recovery released and dived + straight back into the floor, over and over. A forgotten or held-on + switch cannot drop the aircraft back into the ground. +- The climb ends at floor + `alt_floor_margin` and the aircraft loiters + there, waiting. Touching the sticks hands manual control back (in your + base ANGLE/ACRO mode); the latched figure still will not restart until + you cycle its switch. To land, switch the FLOOR box OFF - only then + does the aircraft descend through the line instead of being caught. + +### Figures (F ROLL, F LOOP, F 4PT, F SEQ) + +One-switch figures fly an axial roll, a loop (`fig_loop_rate` - radius +is rate and speed: R = v / omega) or a 4-point roll. F SEQ flies a +scripted sequence of segments (roll / pitch / hold / wait-altitude / +wait-time / impulse / wait-position / spin), programmed over MSP; +community tooling can turn a written routine into such a script. + +Figures fly ON A LINE: the heading captured at figure start anchors the +trajectory, and the full attitude error is regulated - a slow roll +stays on its string instead of walking off course. An altitude assist +holds the entry altitude through the figure. After the last segment the +sequencer holds LEVEL at the entry altitude (assist active) until you +release the box; switching the box off at any time aborts instantly. + +### Load governor (`ohold_load_limit`) + +Problem being solved: a figure flown "fast AND tight" is bounded by one +number - the load. Centripetal load is speed times rotation rate +(radius r = v^2 / a), so an aggressive loop rate at full power reads +double-digit g at the exit pull. + +`ohold_load_limit` [g x 10, default 40 = 4 g] is a fact about your +airframe: what it may pull. While the measured load sits above it, the +governor slows the figure's rotation, the target slew (the catch-up +pull toward a distant target is the hardest load of a maneuver, not the +rotation) and - only while a figure or spin flies - bleeds throttle, +because a governed rotation at full power just converts into speed and +keeps the load. Plain holds at 1 g and your normal flying are never +touched. Set it to your airframe's structural rating; 0 disables the +governor entirely. + +### Crash detection (`crash_detection`) + +*Note: crash detection is a standalone feature (`USE_CRASH_DETECTION`), +independent of the orientation-hold modes and intended for any platform +including multirotors. It is described here for completeness but is being +moved to its own pull request.* + +Problem being solved: after an unscheduled arrival the prop keeps +churning until you walk over and disarm. + +A crash has one signature: a sharp acceleration spike, then NOTHING. A +spike near the accelerometer's full-scale, followed by the airframe +lying still (no rotation, resting 1 g, frozen raw baro, and - with a +GPS fix - zero ground speed) CUTS the motor while staying armed. Moving +the throttle to zero and up again re-allows it deliberately: short +motor bursts are the most reliable way to find a plane in high grass or +corn. Hand-launch safe (it arms only once clearly flying). + +There is no threshold to tune: the impact level is derived from the +detected accelerometer (15% below its full-scale - ~13.6 g on a 16 g +IMU), which even the hardest 3D figure stays clear of, and the +stillness that must follow is what tells a crash from a hard maneuver. +`crash_detection` is ON by default; set it to OFF to disable. + +## Learned gains - do not hand-tune these + +Four settings look like gains but are **written by the firmware, not by +you**: `ohold_hover_gain`, `ohold_inverted_gain`, `ohold_knife_gain`, +`ohold_figure_gain`. Each is a per-regime damping scale (in %) relative +to your normal-flight PIDs. A limit-cycle detector watches for a buzz in +that regime and backs the scale off, recovering slowly when the buzz is +gone; the value is saved on disarm. Fly a hold or figure a few times and +it settles itself. You only touch these to RESET them (set to 100) if +you changed props/airframe and want the learner to start over. Leave +them alone otherwise. + +Everything else below is yours to set. + +## First flights and tuning + +Do this in order. Each step depends on the one before it being right; +skipping ahead just moves the symptom. + +**Step 0 - bench, props off.** Run the level-1 MSP check (bench repo) +and confirm on the ground: each hold box drives the surfaces the right +way (roll the model by hand in INVERT, the ailerons should fight back to +inverted), and the FLOOR box, when you fake a low altitude, commands +nose-up. Wrong sign here is a reversed servo or a wrong mode range, not +a gain. + +**Step 1 - trim the airframe physically. This is not optional.** In the +order of the trimming checklist (see the bench repo quick guide): +level trim; CG via the 45-degree inverted test (only a breath of down +elevator should hold the line - move the battery, never the software); +per-side knife-edge coupling; thrust line; aileron differential. Every +later step assumes a trimmed airframe. A hold buzzing or a figure +drifting almost always traces back to a trim you skipped here. + +**Step 2 - per-attitude pitch trims.** From those trim flights, set +`ohold_inverted_pitch_trim`, and `ohold_knife_left_pitch_trim` / +`ohold_knife_right_pitch_trim` separately (the sides are not symmetric). +Symptom of too little: the hold sinks or the nose drops in that +attitude. Too much: it balloons/climbs. Aim for a hold that neither +climbs nor sinks with the sticks centered. + +**Step 3 - entry feel.** `ohold_entry_rate` (deg/s) is how fast the +target rolls into a hold when you flip the box. Too slow feels mushy and +lags your intent; too fast snaps and can overshoot on a heavy model. +Start at the default and adjust to taste. + +**Step 4 - hover.** Hold a prop hang. `ohold_hover_thr_min` is the +throttle floor that keeps prop-wash authority in updrafts - raise it if +the model feels rudderless/limp at the top of the hover, lower it if it +climbs when you back off. There is no hover PID to tune: the hover base +throttle is learned online (from your own stick at engage), and the +altitude-loop gains derive from that learned point at runtime - the +throttle-to-thrust slope is the one airframe fact they all share. +`ohold_hover_baro_weight` raises the baro share of the altitude estimate +in the hover regime and normally stays put. The throttle stick is a +climb-rate command while hovering; a slammed-low stick is still a hard +cut. + +**Step 5 - knife edge energy.** `ohold_knife_speed_ff` adds nose-up +angle as throttle (the speed proxy) drops, so the edge holds height at +low speed. Symptom of too little: the knife sinks as you slow down. +Too much: the nose climbs and it balloons off the line. `ohold_stick_angle` +is how far a full roll/pitch stick carves the held attitude off the +preset - taste, larger = more authority to reshape the line by hand; +`ohold_stick_return_rate` is how fast the target eases back to the +preset after you let go. + +**Step 6 - figures.** `fig_roll_rate`, `fig_loop_rate`, +`fig_point_dwell` set the one-switch figure speeds. Loop radius follows +from rate and speed (R = v / omega): halve `fig_loop_rate` for double +the radius. The rate settings are the CEILING - the load governor +(`ohold_load_limit`, see above) slows the figure and bleeds throttle +whenever the measured load exceeds the budget, so an aggressive rate is +safe to program: at the budget the figure flies as fast and as tight as +the load allows. The altitude assist (`fig_assist_z_gain`, +`fig_assist_vz_gain`, `fig_assist_max`) holds the entry altitude through +a figure - raise the gains if figures drift down, lower them if the +model pumps altitude during a slow roll. + +**Step 7 - the safety floor.** Only once the above is trusted, set +`alt_floor_altitude` (m above home) and `alt_floor_margin`. Test it high: +climb above floor + margin, then push over and HOLD the down elevator - +the floor must catch and level against the held stick. +`alt_floor_climb_pitch` is the recovery climb angle. + +## Troubleshooting - symptom to setting + +| Symptom | Look at | +| --- | --- | +| Hold buzzes / oscillates in one attitude | first check trim (step 1); to reset a learned gain set the matching `ohold_*_gain` to 100 | +| Inverted / knife sinks with sticks centered | that attitude's pitch trim too low; knife also `ohold_knife_speed_ff` | +| Hold balloons / climbs | pitch trim too high | +| Hover feels limp / rudderless up high | raise `ohold_hover_thr_min` | +| Knife edge drops as it slows | raise `ohold_knife_speed_ff` | +| Entry into a hold snaps / overshoots | lower `ohold_entry_rate` | +| Loop too tight / too wide | `fig_loop_rate` (radius = speed / rate) | +| Figure slower / wider than the rate says, throttle dips in it | the load governor at work - raise `ohold_load_limit` if the airframe is rated for more, or accept the wider line | +| Figure drifts down | raise `fig_assist_z_gain` / `fig_assist_max` | +| Wrong surface direction in a hold | reversed servo or wrong mode range, not a gain (step 0) | +| Floor does not catch | `alt_floor_altitude`/`margin`, and confirm it armed (climb above floor+margin once) | + +All settings with their exact ranges are in [Settings.md](Settings.md). + +## Simulation + +Everything above can be flown against a JSBSim plant through +`MSP_SIMULATOR`. SITL gained a deterministic lockstep mode +(`--lockstep`): the sim clock advances exactly 1 ms per injected frame, +so the same input produces the same flight bit for bit, host load does +not matter, and many SITL instances can run in parallel. Replay videos +of every mode and several scripted routines live in the companion bench +repository, all flown on one configuration. diff --git a/docs/Settings.md b/docs/Settings.md index 3d7351dc371..29eb6805521 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -392,6 +392,36 @@ Optical flow module alignment (default CW0_DEG_FLIP) --- +### alt_floor_altitude + +Altitude floor [m above home]. With the ALT FLOOR mode active and armed (climbed above floor + margin once), SINKING THROUGH the floor engages an automatic upright + climb recovery - no prediction, the crossing is the trigger. Set the floor high enough that the recovery fits below it (a dive recovery consumes roughly 15-25 m). Back at the floor the aircraft ORBITS the breach point on the fixed-wing loiter (nav_fw_loiter_radius) and waits - GPS-anchored while the position estimate is healthy (level flight restores the antenna's sky view), a constant-bank circle otherwise; the pilot gets time to collect themselves, there is no automatic hand-back. SET nav_fw_loiter_radius TO MATCH YOUR SPEED: the circle must be physically flyable, radius >= v^2 / (9.81 * tan(bank)) - an aerobatic airframe at 25 m/s needs roughly 150 m; too small a radius makes the loiter hunt at full bank. THE PILOT OVERRIDES THE AUTOPILOT: held sticks keep steering (a full held rudder drives a spin straight through the floor) - release the sticks and the floor catches; centering the sticks once and then deflecting roll/pitch takes over and releases the orbit. A catch that interrupts ANY active aerobatic mode (every hold, every figure, the sequencer) LATCHES that mode out until the pilot switches it away. Switch the ALT FLOOR mode off to land. + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 5 | 500 | + +--- + +### alt_floor_climb_pitch + +Nose up pitch target [deg] flown during altitude floor recovery + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 5 | 45 | + +--- + +### alt_floor_margin + +Margin above the altitude floor [m]: the floor arms after climbing above floor + margin once, and the recovery climb ends there - back above floor + margin and climbing, control returns to the pilot. A roll/pitch input after the catch (sticks centered once first) releases the recovery immediately. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 2 | 100 | + +--- + ### alt_hold_deadband Defines the deadband of throttle during alt_hold [r/c points] @@ -1162,6 +1192,66 @@ If failsafe activated when throttle is low for this much time - bypass failsafe --- +### fig_assist_max + +Cap [deg] on the altitude assist nose-up offset + +| Default | Min | Max | +| --- | --- | --- | +| 12 | 0 | 30 | + +--- + +### fig_assist_vz_gain + +Altitude assist: nose-up offset [deg per m/s] of sink rate during figures. Keep low: the climb rate estimate lags and a strong damping term fights fast figures + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 20 | + +--- + +### fig_assist_z_gain + +Altitude assist: nose-up offset [deg per 10 m] of altitude error during figures. The controller distributes the offset to elevator and rudder as the roll phase demands + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 100 | + +--- + +### fig_loop_rate + +Pitch rate [deg/s] flown by the FIGURE LOOP mode + +| Default | Min | Max | +| --- | --- | --- | +| 90 | 30 | 360 | + +--- + +### fig_point_dwell + +Dwell time [ms] on each point of the FIGURE 4PT ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 100 | 2000 | + +--- + +### fig_roll_rate + +Roll rate [deg/s] flown by the FIGURE ROLL and FIGURE 4PT ROLL modes + +| Default | Min | Max | +| --- | --- | --- | +| 90 | 30 | 360 | + +--- + ### fixed_wing_auto_arm Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. @@ -4462,6 +4552,66 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within --- +### ohold_entry_rate + +Target slew rate [deg/s] for entering an orientation hold preset (INVERTED, KNIFE EDGE, PROP HANG). The entry rolls the hold target from the current attitude to the preset at this rate; figures keep their own fig_roll_rate / fig_loop_rate + +| Default | Min | Max | +| --- | --- | --- | +| 180 | 30 | 720 | + +--- + +### ohold_load_limit + +Load budget [g x 10] the governor holds figures and spins to - a fact about the airframe (what it may pull), not a tuning knob. Load is speed times rotation rate, so at a given speed the budget is simultaneously the fastest rotation and the tightest radius (r = v^2/a): the governor slows the commanded rotation and the target slew with the measured overload, and bleeds throttle while a figure or spin flies (a governed rotation at full power just converts into speed, the load would stay). Plain holds at 1 g are untouched. 0 disables the governor. + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 160 | + +--- + +### ohold_stick_angle + +Body-frame target offset [deg] at full roll/pitch stick while an orientation hold preset is active: the deflection is a held angle offset from the rotated reference (carving), centered sticks return the target at ohold_stick_return_rate. Yaw stays a rate command. 0 = sticks act as raw rate commands like before + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 90 | + +--- + +### ohold_stick_return_rate + +Rate [deg/s] the hold target returns to the preset after the roll/pitch sticks center + +| Default | Min | Max | +| --- | --- | --- | +| 45 | 5 | 180 | + +--- + +### ohold_turn_roll_limit + +Automatic roll lean [deg] allowed while a commanded turn (the stick of the pose's vertical axis: rudder at level/inverted, elevator at the knife) flies a curve in a hold. The lean IS the curve physics - tan(bank) = turn rate x speed / g - and is commanded into the target instead of being fought; this caps it. The lean exists only WHILE yaw is commanded. 0 = no lean, flat turns only. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 60 | + +--- + +### ohold_turn_roll_return + +Time [ms] the automatic curve lean eases back out after the yaw stick returns to centre (gentle, no snap). + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 100 | 5000 | + +--- + ### opflow_hardware Selection of OPFLOW hardware. @@ -5862,6 +6012,76 @@ Defines rotation rate on ROLL axis that UAV will try to archive on max. stick de --- +### rotor_guard_bank + +Autogyro tip-over guard (ROTOR GUARD mode): bank angle [deg] beyond which, while sinking, the roll excursion counts as a tip-over (rotor rpm decayed, lateral tilt authority gone). Recovery: wings level, nose slightly down, throttle floor - thrust is the only lever that restores rotor rpm. TUNE PER AIRFRAME to just above the steepest bank it flies on purpose; the default is deliberately conservative, the SITL-proven Durafly Auto-G2 value is 45. + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 30 | 90 | + +--- + +### rotor_guard_min_height + +Below this height [m, baro above the arming/start altitude] the guard flies NO aggressive recovery power - near the ground the power burst does more harm than good; wings level + cushion only. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 100 | + +--- + +### rotor_guard_pitch + +Pitch target [deg] during rotor guard recovery. Keep >= 0: the rotor must stay LOADED - a nose-down push unloads the disk and decays the rotor rpm FASTER (real-gyro doctrine, power push-over). Small negative values only for airframes proven to need them. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -20 | 10 | + +--- + +### rotor_guard_pitch_limit + +Attitude limiter, pitch [deg]: max commanded pitch while the ROTOR GUARD mode is on - a steep nose-up bleeds the airspeed that drives the rotor, a steep nose-down unloads the disk; both starve the rpm. + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 10 | 45 | + +--- + +### rotor_guard_roll_limit + +Attitude limiter, bank [deg]: with the ROTOR GUARD mode on, the COMMANDED curve flight is limited to this bank - past ~35 deg an autogyro's vertical lift collapses and no catch has anything left to work with, so a commanded attitude is never allowed there. The tip-AWAY (uncommanded excursion when the rotor starves) is what the guard's recovery catches. + +| Default | Min | Max | +| --- | --- | --- | +| 35 | 10 | 60 | + +--- + +### rotor_guard_sink + +Minimum sink rate [cm/s] for the tip-over detection - a banked climb or a flown figure does not trip the guard + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 10 | 1000 | + +--- + +### rotor_guard_throttle_boost + +Recovery throttle boost [%], RELATIVE: the floor is the throttle the aircraft was operating on when the guard tripped (at least cruise) raised by this percentage of its thrust - a headwind day flies on a higher trim throttle and the recovery scales with it, instead of guessing an absolute value. With the rotor loaded, the brief power burst is the fastest way back to authority (thrust -> speed -> inflow -> rpm). More pilot throttle always wins, and an IDLE stick disables the guard entirely (landing intent). Must be enough that the airframe LEVELS OFF - a T/W below 1 needs more. + +| Default | Min | Max | +| --- | --- | --- | +| 25 | 0 | 100 | + +--- + ### rpm_gyro_filter_enabled Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV @@ -6242,6 +6462,76 @@ The strength factor of a Smith Predictor of PID measurement. In percents --- +### soar_alt_max + +Leave the thermal once this altitude [m] is reached. + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 0 | 5000 | + +--- + +### soar_alt_min + +Do not enter a thermal below this altitude [m] - a safety floor for autonomous soaring. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 3000 | + +--- + +### soar_bank + +Bank angle [deg] flown while circling a thermal. + +| Default | Min | Max | +| --- | --- | --- | +| 35 | 15 | 50 | + +--- + +### soar_centre_gain + +Gain [%] on the thermal-centering gradient shift. Higher centres faster but chases turbulence; the wind-drift shift is always applied at full wind speed regardless. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 300 | + +--- + +### soar_sink_level + +Level-flight sink rate [cm/s] at the tuning airspeed, used to compensate the aircraft's own sink out of the net vario. Raise it if the vario reads high in still air, lower it if it reads low. Default ~60 is a 1.8 m motor glider (measured min sink 0.6 m/s); a clean sailplane is lower. + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 500 | + +--- + +### soar_vario_exit + +Net climb rate [cm/s] below which circling stops and the aircraft returns to cruise (the thermal was flown through or died). + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1000 | + +--- + +### soar_vario_trigger + +Net (total-energy) climb rate [cm/s] above which the SOARING mode stops cruising and starts circling a thermal. Requires a pitot - the net vario is meaningless without airspeed. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 1000 | + +--- + ### spektrum_sat_bind 0 = disabled. Used to bind the spektrum satellite to RX @@ -6522,6 +6812,26 @@ Turtle mode power factor --- +### tvc_gain + +Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources. Values above 100 use more of the mechanical vectoring travel per stabilized unit (the output stays clamped at full deflection); hover-heavy setups typically need 200 or more. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 400 | + +--- + +### tvc_thrust_comp + +Inverse thrust compensation [%] for the TVC inputs: vane/tilt authority scales with thrust, 100 compensates fully (deflection ~ 1/thrust, capped at low thrust), 0 disables + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + ### tz_automatic_dst Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c243d9215f3..8baf2ad35ac 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -323,6 +323,8 @@ main_sources(COMMON_SRC flight/failsafe.c flight/failsafe.h + flight/figure_sequencer.c + flight/figure_sequencer.h flight/imu.c flight/imu.h flight/kalman.c @@ -331,8 +333,20 @@ main_sources(COMMON_SRC flight/smith_predictor.h flight/rate_dynamics.c flight/rate_dynamics.h + flight/altitude_floor.c + flight/altitude_floor.h + flight/rotor_guard.c + flight/rotor_guard.h + flight/soaring.c + flight/soaring.h + flight/crash_detection.c + flight/crash_detection.h + flight/hover_throttle.c + flight/hover_throttle.h flight/mixer.c flight/mixer.h + flight/orientation_hold.c + flight/orientation_hold.h flight/pid.c flight/pid.h flight/pid_autotune.c @@ -340,6 +354,8 @@ main_sources(COMMON_SRC flight/power_limits.h flight/rth_estimator.c flight/rth_estimator.h + flight/thrust_vectoring.c + flight/thrust_vectoring.h flight/servos.c flight/servos.h flight/mixer_profile.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 0bb74bac1ac..843bfdb7f65 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -79,6 +79,7 @@ typedef enum { DEBUG_GPS, DEBUG_LULU, DEBUG_SBUS2, + DEBUG_FW_AEROBATICS, // 0-3: learned regime gain scale [%] hover/inverted/knife/figure DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 2acb9c8172e..632c48f9265 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -132,7 +132,16 @@ #define PG_GEOZONE_CONFIG 1042 #define PG_GEOZONES 1043 #define PG_GEOZONE_VERTICES 1044 -#define PG_INAV_END PG_GEOZONE_VERTICES +#define PG_ALTITUDE_FLOOR_CONFIG 1045 +#define PG_THRUST_VECTORING_CONFIG 1046 +#define PG_ORIENTATION_HOLD_CONFIG 1047 +#define PG_FIGURE_SEQUENCER_CONFIG 1048 +#define PG_FIGURE_SEQUENCE 1049 +#define PG_HOVER_THROTTLE_CONFIG 1050 +#define PG_CRASH_DETECTION_CONFIG 1051 +#define PG_ROTOR_GUARD_CONFIG 1052 +#define PG_SOARING_CONFIG 1053 +#define PG_INAV_END PG_SOARING_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c index 3891b366927..22102a55da4 100644 --- a/src/main/drivers/accgyro/accgyro_fake.c +++ b/src/main/drivers/accgyro/accgyro_fake.c @@ -82,7 +82,9 @@ static int16_t fakeAccData[XYZ_AXIS_COUNT]; static void fakeAccInit(accDev_t *acc) { - acc->acc_1G = 9806; + acc->acc_1G = 2048; // 16 G scale, matching a real IMU (HITL injects + // acc.accADCf directly in g, so this only sets the + // full-scale the crash detector reads back) } void fakeAccSet(int16_t x, int16_t y, int16_t z) diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 7b1c462572e..b0761f162ba 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -129,8 +129,14 @@ void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i]; port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize; pthread_mutex_unlock(&port->receiveMutex); + __atomic_add_fetch(&sitlRxBytesPending, 1, __ATOMIC_RELAXED); } } + // lockstep: newly arrived bytes restart the frozen-clock creep window so + // the serial task gets scheduled to parse what just arrived - AFTER the + // enqueue, so the woken pass cannot outrun the buffer fill (see + // target/SITL/target.c) + sitlLockstepRxArrival(); } void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize ) { @@ -234,6 +240,9 @@ uint8_t tcpRead(serialPort_t *instance) port->serialPort.rxBufferTail = (port->serialPort.rxBufferTail + 1) % port->serialPort.rxBufferSize; pthread_mutex_unlock(&port->receiveMutex); + if (sitlRxBytesPending > 0) { + __atomic_sub_fetch(&sitlRxBytesPending, 1, __ATOMIC_RELAXED); + } return ch; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9bb2c776883..58a58d593d1 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -160,8 +160,8 @@ static uint8_t commandBatchErrorCount = 0; // sync this with features_e static const char * const featureNames[] = { "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE", - "", "SOFTSERIAL", "GPS", "RPM_FILTERS", - "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", + "FW_AEROBATICS", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "CRASH_DETECTION", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE", @@ -222,7 +222,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "HEADTRACKER", "GPS", "LULU", - "SBUS2" + "SBUS2", + "FW_AEROBATICS" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d3021317ae5..5077d7d6977 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -206,8 +206,9 @@ void validateAndFixConfig(void) accelerometerConfigMutable()->acc_notch_hz = 0; } - // Disable unused features - featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); + // Disable unused features (bit 5 FEATURE_FW_AEROBATICS and bit 9 + // FEATURE_CRASH_DETECTION are real now and must survive the boot scrub) + featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); #if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) { diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e3bde5f3eb7..0af6d687d29 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -37,11 +37,11 @@ typedef enum { FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, FEATURE_GEOZONE = 1 << 4, //was FEATURE_MOTOR_STOP - FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS + FEATURE_FW_AEROBATICS = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE - FEATURE_UNUSED_4 = 1 << 9, // was FEATURE_SONAR + FEATURE_CRASH_DETECTION = 1 << 9, // was FEATURE_SONAR FEATURE_TELEMETRY = 1 << 10, FEATURE_CURRENT_METER = 1 << 11, FEATURE_REVERSIBLE_MOTORS = 1 << 12, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..70cba7069d1 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -87,6 +87,12 @@ #include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" +#include "flight/altitude_floor.h" +#include "flight/rotor_guard.h" +#include "flight/figure_sequencer.h" +#include "flight/crash_detection.h" +#include "flight/soaring.h" +#include "flight/orientation_hold.h" #include "flight/rate_dynamics.h" #include "flight/failsafe.h" @@ -254,7 +260,16 @@ static void updateArmingStatus(void) } /* CHECK: CPU load */ - if (isSystemOverloaded()) { + if (isSystemOverloaded() +#if defined(SITL_BUILD) + /* Under --lockstep the loop is paced by the simulator frame + * stream (exactly one 1 kHz tick per injected frame), not by + * the wall clock - host load reads as a scheduler backlog and + * would block arming with a false SYSTEM_OVERLOADED. Default + * SITL keeps the stock check. */ + && !sitlLockstepEnabled +#endif + ) { ENABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); } else { @@ -683,22 +698,75 @@ void processRx(timeUs_t currentTimeUs) bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs); bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce; - /* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD + /* Disable stabilised modes initially, will be enabled as required with priority + * auto ANGLE (failsafe/nav) > ALT FLOOR / ROTOR GUARD recovery > ANGLE > HORIZON > ORIENTATION HOLD > ANGLEHOLD * MANUAL mode has priority over these modes except when ANGLE auto enabled */ DISABLE_FLIGHT_MODE(ANGLE_MODE); DISABLE_FLIGHT_MODE(HORIZON_MODE); DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE); +#ifdef USE_FW_AEROBATICS + DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); + altitudeFloorUpdate(); + rotorGuardUpdate(); + figureSequencerUpdate(); +#if defined(SITL_BUILD) + // bench safety word (SITL only); composed by the module + debug[7] = orientationHoldDebugSafetyWord(); + // bench estimator XY (SITL only): the false-valid GPS audit compares the + // FC's local position estimate against the injected GPS and the plant + // truth; while thermalling the soaring module owns slots 0..4 instead + if (!soaringActive()) { + // slot 0 is free outside thermalling (past probes lived there) + debug[1] = lrintf(getEstimatedActualPosition(Z)); // [cm] + debug[2] = lrintf(getEstimatedActualPosition(X)); // [cm] + debug[3] = lrintf(getEstimatedActualPosition(Y)); // [cm] + } +#endif +#endif if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { - if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) { + if (autoEnableAngle) { ENABLE_FLIGHT_MODE(ANGLE_MODE); +#ifdef USE_FW_AEROBATICS + } else if (STATE(AIRPLANE) + && ((altitudeFloorRecoveryActive() && !altitudeFloorOrbitViaNav()) + || rotorGuardRecoveryActive())) { + // Automatic safety recovery (altitude floor, or the autogyro + // tip-over guard): overrides the pilot's stabilised mode + // selection until the aircraft is caught. The floor's ORBIT + // phase flies on the real nav loiter instead - the nav mode + // owns the aircraft there, not the orientation hold. + ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); +#endif + } else if (IS_RC_MODE_ACTIVE(BOXANGLE)) { +#ifdef USE_FW_AEROBATICS + // leaving a hold far from level: the hold slews its target to + // the horizon first, ANGLE takes over once the attitude is + // there (a hover exit otherwise whips through nose down) + if (STATE(AIRPLANE) && orientationHoldExitSlewPending()) { + ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); + } else +#endif + { + ENABLE_FLIGHT_MODE(ANGLE_MODE); + } } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); +#ifdef USE_FW_AEROBATICS + } else if (STATE(AIRPLANE) && orientationHoldIsRequested()) { + ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); +#endif } else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE); } } +#ifdef USE_FW_AEROBATICS + if (!FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { + orientationHoldResetSourceTracking(); + } +#endif + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { @@ -963,6 +1031,16 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processPilotAndFailSafeActions(dT); +#ifdef USE_CRASH_DETECTION + // impact followed by stillness stops the motor (hand-launch aware) + crashDetectionUpdate(dT); +#endif + +#ifdef USE_SOARING + // thermal soaring: net vario + wind-shifted thermal centering + soaringUpdate(dT); +#endif + // Check battery, GPS signal, arming status etc @ 200 Hz static uint8_t armingStatusDivider = 0; if (++armingStatusDivider >= 10) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3ec3f89f3f0..e0b5bd277d2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -75,7 +75,9 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/figure_sequencer.h" #include "flight/imu.h" +#include "flight/orientation_hold.h" #include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -565,6 +567,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 0); } break; +#ifdef USE_FW_AEROBATICS + case MSP2_INAV_FIGURE_SEQUENCE: + for (int i = 0; i < MAX_FIGURE_SEQUENCE_SEGMENTS; i++) { + sbufWriteU8(dst, figureSequence(i)->type); + sbufWriteU16(dst, figureSequence(i)->p1); + sbufWriteU16(dst, figureSequence(i)->p2); + sbufWriteU16(dst, figureSequence(i)->p3); + sbufWriteU8(dst, figureSequence(i)->flags); + } + break; +#endif + case MSP2_INAV_SERVO_MIXER: for (int i = 0; i < MAX_SERVO_RULES; i++) { sbufWriteU8(dst, customServoMixers(i)->targetChannel); @@ -2371,6 +2385,28 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } else return MSP_RESULT_ERROR; break; + +#ifdef USE_FW_AEROBATICS + case MSP2_INAV_SET_FIGURE_SEQUENCE: + sbufReadU8Safe(&tmp_u8, src); + if ((dataSize == 9) && (tmp_u8 < MAX_FIGURE_SEQUENCE_SEGMENTS)) { + // validate BEFORE touching the PG: a rejected frame must not + // leave half-written parameters behind (review finding) + const uint8_t segType = sbufReadU8(src); + if (segType >= FIGSEG_TYPE_COUNT) { + return MSP_RESULT_ERROR; + } + figureSegment_t *seg = figureSequenceMutable(tmp_u8); + seg->type = segType; + seg->p1 = sbufReadU16(src); + seg->p2 = sbufReadU16(src); + seg->p3 = sbufReadU16(src); + seg->flags = sbufReadU8(src); + } else + return MSP_RESULT_ERROR; + break; +#endif + #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_SET_LOGIC_CONDITIONS: sbufReadU8Safe(&tmp_u8, src); @@ -4247,6 +4283,13 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ } if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { + // injected signal strength (HITL_GPS_CNO): the byte itself sits at + // the message tail and was stored LAST frame - the GPS block is + // processed mid-parse, one sim frame of lag is physical anyway + if (SIMULATOR_HAS_OPTION(HITL_GPS_CNO)) { + gpsSolDRV.cnoMean = simulatorData.gpsCno; + gpsSolDRV.flags.validCno = true; + } gpsSolDRV.fixType = sbufReadU8(src); gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; gpsSolDRV.numSat = sbufReadU8(src); @@ -4359,6 +4402,13 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ } rxSimSetFailsafe(SIMULATOR_HAS_OPTION(HITL_FAILSAFE_TRIGGERED)); + + // optional trailing byte, only present when the sender sets the + // option (official HITL plugins do not): mean C/N0 [dBHz] of the + // strongest signals - consumed by the NEXT GPS block above + if (SIMULATOR_HAS_OPTION(HITL_GPS_CNO)) { + simulatorData.gpsCno = sbufReadU8(src); + } } // Backward compatibility for HITL Plugin 1.X @@ -4373,6 +4423,12 @@ static mspResult_e mspProcessSimulatorCommand(sbuf_t *dst, sbuf_t *src, const in return MSP_RESULT_ERROR; } +#ifdef SITL_BUILD + // lockstep (--lockstep): every simulator frame advances the simulated + // clock by exactly one millisecond, see target/SITL/target.c + sitlLockstepTick(); +#endif + const uint8_t simMspVersion = sbufReadU8(src); // Get the Simulator MSP version if (simMspVersion != SIMULATOR_MSP_VERSION_2 && simMspVersion != SIMULATOR_MSP_VERSION_3) { return MSP_RESULT_ERROR; @@ -4456,6 +4512,43 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif +// Bench/HIL level-1 test injection - SITL only, kept off flight hardware +#if defined(SITL_BUILD) && defined(USE_FW_AEROBATICS) + case MSP2_INAV_ORIENTATION_HOLD_TEST: { + // Level-1 test injection (bench/HIL): evaluate the orientation hold + // error function and the level gain on the given quaternions. + // Pure computation on this MCU's float32 - no controller or + // estimator state is touched, safe in any build/flight state. + if (dataSize != 8 * sizeof(uint32_t)) { + *ret = MSP_RESULT_ERROR; + break; + } + union { uint32_t u; float f; } pun; + fpQuaternion_t qEst, qTarget; + float * const in[8] = { &qEst.q0, &qEst.q1, &qEst.q2, &qEst.q3, + &qTarget.q0, &qTarget.q1, &qTarget.q2, &qTarget.q3 }; + for (int i = 0; i < 8; i++) { + pun.u = sbufReadU32(src); + *in[i] = pun.f; + } + + fpVector3_t errDeg; + orientationHoldComputeAttitudeError(&errDeg, &qEst, &qTarget); + for (int i = 0; i < 3; i++) { + pun.f = errDeg.v[i]; + sbufWriteU32(dst, pun.u); + } + for (int axis = 0; axis < 3; axis++) { + pun.f = constrainf(errDeg.v[axis] * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + -currentControlProfile->stabilized.rates[axis] * 10.0f, + currentControlProfile->stabilized.rates[axis] * 10.0f); + sbufWriteU32(dst, pun.u); + } + *ret = MSP_RESULT_ACK; + break; + } +#endif + case MSP2_COMMON_SETTING: *ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; break; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 65654ccd97b..17be6baa53c 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -31,6 +31,7 @@ #include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" +#include "flight/servos.h" #include "io/osd.h" @@ -109,6 +110,18 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXGIMBALRLOCK, .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 }, { .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 }, { .boxId = BOXGIMBALHTRK, .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 }, + { .boxId = BOXINVERTED, .boxName = "INVERT", .permanentId = 69 }, + { .boxId = BOXKNIFELEFT, .boxName = "KNIFE L", .permanentId = 70 }, + { .boxId = BOXKNIFERIGHT, .boxName = "KNIFE R", .permanentId = 71 }, + { .boxId = BOXPROPHANG, .boxName = "P-HANG", .permanentId = 72 }, + { .boxId = BOXALTFLOOR, .boxName = "FLOOR", .permanentId = 73 }, + { .boxId = BOXFIGROLL, .boxName = "F ROLL", .permanentId = 74 }, + { .boxId = BOXFIGLOOP, .boxName = "F LOOP", .permanentId = 75 }, + { .boxId = BOXFIGPOINTROLL, .boxName = "F 4PT", .permanentId = 76 }, + { .boxId = BOXFIGSEQ, .boxName = "F SEQ", .permanentId = 77 }, + { .boxId = BOXATTLOCK, .boxName = "3DLOCK", .permanentId = 78 }, + { .boxId = BOXFSPIN, .boxName = "FLAT SPIN", .permanentId = 79 }, + { .boxId = BOXROTORGUARD, .boxName = "ROTOR GUARD", .permanentId = 80 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -283,6 +296,31 @@ void initActiveBoxIds(void) } if (sensors(SENSOR_ACC)) { ADD_ACTIVE_BOX(BOXANGLEHOLD); +#ifdef USE_FW_AEROBATICS + // the whole aerobatics suite sits behind one runtime feature + // (FW_LAUNCH pattern, Daniel's call): feature off = none of + // these boxes exist, the Modes tab looks exactly like upstream + if (feature(FEATURE_FW_AEROBATICS)) { + ADD_ACTIVE_BOX(BOXINVERTED); + // a knife edge is held on the rudder (or a TVC yaw vane): + // a model without any yaw effector (flying wing) cannot + // fly one, so the knife modes are not offered on such a + // mixer + if (servoMixerHasYawControl()) { + ADD_ACTIVE_BOX(BOXKNIFELEFT); + ADD_ACTIVE_BOX(BOXKNIFERIGHT); + } + ADD_ACTIVE_BOX(BOXPROPHANG); + ADD_ACTIVE_BOX(BOXALTFLOOR); + ADD_ACTIVE_BOX(BOXROTORGUARD); + ADD_ACTIVE_BOX(BOXFIGROLL); + ADD_ACTIVE_BOX(BOXFIGLOOP); + ADD_ACTIVE_BOX(BOXFIGPOINTROLL); + ADD_ACTIVE_BOX(BOXFIGSEQ); + ADD_ACTIVE_BOX(BOXATTLOCK); + ADD_ACTIVE_BOX(BOXFSPIN); + } +#endif } } @@ -449,6 +487,20 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); +#ifdef USE_FW_AEROBATICS + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXINVERTED)), BOXINVERTED); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKNIFELEFT)), BOXKNIFELEFT); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKNIFERIGHT)), BOXKNIFERIGHT); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPROPHANG)), BOXPROPHANG); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXALTFLOOR)), BOXALTFLOOR); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXROTORGUARD)), BOXROTORGUARD); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGROLL)), BOXFIGROLL); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGLOOP)), BOXFIGLOOP); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGPOINTROLL)), BOXFIGPOINTROLL); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGSEQ)), BOXFIGSEQ); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXATTLOCK)), BOXATTLOCK); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFSPIN)), BOXFSPIN); +#endif #ifdef USE_SERIAL_GIMBAL if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2e972d1b304..56562f384cf 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -85,6 +85,18 @@ typedef enum { BOXGIMBALRLOCK = 57, BOXGIMBALCENTER = 58, BOXGIMBALHTRK = 59, + BOXINVERTED = 60, + BOXKNIFELEFT = 61, + BOXKNIFERIGHT = 62, + BOXPROPHANG = 63, + BOXALTFLOOR = 64, + BOXFIGROLL = 65, + BOXFIGLOOP = 66, + BOXFIGPOINTROLL = 67, + BOXFIGSEQ = 68, + BOXATTLOCK = 69, + BOXFSPIN = 70, + BOXROTORGUARD = 71, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index faec9fdef15..fc34b64647a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -108,6 +108,7 @@ typedef enum { ANGLEHOLD_MODE = (1 << 17), NAV_FW_AUTOLAND = (1 << 18), NAV_SEND_TO = (1 << 19), + ORIENTATION_HOLD_MODE = (1 << 20), } flightModeFlags_e; extern uint32_t flightModeFlags; @@ -202,6 +203,7 @@ typedef enum { HITL_RANGEFINDER = (1 << 12), // Simulate Rangefinder data HITL_FAILSAFE_TRIGGERED = (1 << 13), // Simulate Failsafe triggered condition HITL_SITL_MODE = (1 << 14), // For INAV XITL in Sitl mode (sends no emulated sensor data) + HITL_GPS_CNO = (1 << 15), // Optional trailing byte: mean C/N0 of the strongest signals [dBHz] (antenna-shading model) } simulatorFlags_t; typedef struct { @@ -214,6 +216,7 @@ typedef struct { uint16_t rssi; uint16_t current; // dA (deciamperes; * 10 = cA) uint16_t rangefinder; // cm + uint8_t gpsCno; // HITL_GPS_CNO: injected mean C/N0 [dBHz], applied to the next GPS block } simulatorData_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4e8affb0221..e7a86491304 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4502,3 +4502,225 @@ groups: field: noWayHomeAction table: geozone_rth_no_way_home type: uint8_t + + - name: PG_ALTITUDE_FLOOR_CONFIG + type: altitudeFloorConfig_t + headers: ["flight/altitude_floor.h"] + condition: USE_FW_AEROBATICS + members: + - name: alt_floor_altitude + description: "Altitude floor [m above home]. With the ALT FLOOR mode active and armed (climbed above floor + margin once), SINKING THROUGH the floor engages an automatic upright + climb recovery - no prediction, the crossing is the trigger. Set the floor high enough that the recovery fits below it (a dive recovery consumes roughly 15-25 m). Back at the floor the aircraft ORBITS the breach point on the fixed-wing loiter (nav_fw_loiter_radius) and waits - GPS-anchored while the position estimate is healthy (level flight restores the antenna's sky view), a constant-bank circle otherwise; the pilot gets time to collect themselves, there is no automatic hand-back. SET nav_fw_loiter_radius TO MATCH YOUR SPEED: the circle must be physically flyable, radius >= v^2 / (9.81 * tan(bank)) - an aerobatic airframe at 25 m/s needs roughly 150 m; too small a radius makes the loiter hunt at full bank. THE PILOT OVERRIDES THE AUTOPILOT: held sticks keep steering (a full held rudder drives a spin straight through the floor) - release the sticks and the floor catches; centering the sticks once and then deflecting roll/pitch takes over and releases the orbit. A catch that interrupts ANY active aerobatic mode (every hold, every figure, the sequencer) LATCHES that mode out until the pilot switches it away. Switch the ALT FLOOR mode off to land." + default_value: 30 + field: floorAltitude + min: 5 + max: 500 + - name: alt_floor_margin + description: "Margin above the altitude floor [m]: the floor arms after climbing above floor + margin once, and the recovery climb ends there - back above floor + margin and climbing, control returns to the pilot. A roll/pitch input after the catch (sticks centered once first) releases the recovery immediately." + default_value: 10 + field: floorMargin + min: 2 + max: 100 + - name: alt_floor_climb_pitch + description: "Nose up pitch target [deg] flown during altitude floor recovery" + default_value: 15 + field: floorClimbPitch + min: 5 + max: 45 + + - name: PG_ROTOR_GUARD_CONFIG + type: rotorGuardConfig_t + headers: ["flight/rotor_guard.h"] + condition: USE_FW_AEROBATICS + members: + - name: rotor_guard_bank + description: "Autogyro tip-over guard (ROTOR GUARD mode): bank angle [deg] beyond which, while sinking, the roll excursion counts as a tip-over (rotor rpm decayed, lateral tilt authority gone). Recovery: wings level, nose slightly down, throttle floor - thrust is the only lever that restores rotor rpm. TUNE PER AIRFRAME to just above the steepest bank it flies on purpose; the default is deliberately conservative, the SITL-proven Durafly Auto-G2 value is 45." + default_value: 60 + field: bankDeg + min: 30 + max: 90 + - name: rotor_guard_sink + description: "Minimum sink rate [cm/s] for the tip-over detection - a banked climb or a flown figure does not trip the guard" + default_value: 100 + field: sinkCms + min: 10 + max: 1000 + - name: rotor_guard_pitch + description: "Pitch target [deg] during rotor guard recovery. Keep >= 0: the rotor must stay LOADED - a nose-down push unloads the disk and decays the rotor rpm FASTER (real-gyro doctrine, power push-over). Small negative values only for airframes proven to need them." + default_value: 0 + field: recoveryPitchDeg + min: -20 + max: 10 + - name: rotor_guard_throttle_boost + description: "Recovery throttle boost [%], RELATIVE: the floor is the throttle the aircraft was operating on when the guard tripped (at least cruise) raised by this percentage of its thrust - a headwind day flies on a higher trim throttle and the recovery scales with it, instead of guessing an absolute value. With the rotor loaded, the brief power burst is the fastest way back to authority (thrust -> speed -> inflow -> rpm). More pilot throttle always wins, and an IDLE stick disables the guard entirely (landing intent). Must be enough that the airframe LEVELS OFF - a T/W below 1 needs more." + default_value: 25 + field: throttleBoostPct + min: 0 + max: 100 + - name: rotor_guard_min_height + description: "Below this height [m, baro above the arming/start altitude] the guard flies NO aggressive recovery power - near the ground the power burst does more harm than good; wings level + cushion only." + default_value: 15 + field: minHeightM + min: 0 + max: 100 + - name: rotor_guard_roll_limit + description: "Attitude limiter, bank [deg]: with the ROTOR GUARD mode on, the COMMANDED curve flight is limited to this bank - past ~35 deg an autogyro's vertical lift collapses and no catch has anything left to work with, so a commanded attitude is never allowed there. The tip-AWAY (uncommanded excursion when the rotor starves) is what the guard's recovery catches." + default_value: 35 + field: rollLimitDeg + min: 10 + max: 60 + - name: rotor_guard_pitch_limit + description: "Attitude limiter, pitch [deg]: max commanded pitch while the ROTOR GUARD mode is on - a steep nose-up bleeds the airspeed that drives the rotor, a steep nose-down unloads the disk; both starve the rpm." + default_value: 30 + field: pitchLimitDeg + min: 10 + max: 45 + + - name: PG_THRUST_VECTORING_CONFIG + type: thrustVectoringConfig_t + headers: ["flight/thrust_vectoring.h"] + condition: USE_THRUST_VECTORING + members: + - name: tvc_gain + description: "Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources. Values above 100 use more of the mechanical vectoring travel per stabilized unit (the output stays clamped at full deflection); hover-heavy setups typically need 200 or more." + default_value: 100 + field: gain + min: 0 + max: 400 + - name: tvc_thrust_comp + description: "Inverse thrust compensation [%] for the TVC inputs: vane/tilt authority scales with thrust, 100 compensates fully (deflection ~ 1/thrust, capped at low thrust), 0 disables" + default_value: 100 + field: thrustComp + min: 0 + max: 100 + + - name: PG_ORIENTATION_HOLD_CONFIG + type: orientationHoldConfig_t + headers: ["flight/orientation_hold.h"] + condition: USE_FW_AEROBATICS + members: + - name: ohold_load_limit + description: "Load budget [g x 10] the governor holds figures and spins to - a fact about the airframe (what it may pull), not a tuning knob. Load is speed times rotation rate, so at a given speed the budget is simultaneously the fastest rotation and the tightest radius (r = v^2/a): the governor slows the commanded rotation and the target slew with the measured overload, and bleeds throttle while a figure or spin flies (a governed rotation at full power just converts into speed, the load would stay). Plain holds at 1 g are untouched. 0 disables the governor." + default_value: 40 + field: loadLimitG + min: 0 + max: 160 + - name: ohold_entry_rate + description: "Target slew rate [deg/s] for entering an orientation hold preset (INVERTED, KNIFE EDGE, PROP HANG). The entry rolls the hold target from the current attitude to the preset at this rate; figures keep their own fig_roll_rate / fig_loop_rate" + default_value: 180 + field: entryRateDps + min: 30 + max: 720 + - name: ohold_stick_angle + description: "Body-frame target offset [deg] at full roll/pitch stick while an orientation hold preset is active: the deflection is a held angle offset from the rotated reference (carving), centered sticks return the target at ohold_stick_return_rate. Yaw stays a rate command. 0 = sticks act as raw rate commands like before" + default_value: 30 + field: stickAngleMaxDeg + min: 0 + max: 90 + - name: ohold_stick_return_rate + description: "Rate [deg/s] the hold target returns to the preset after the roll/pitch sticks center" + default_value: 45 + field: stickReturnRateDps + min: 5 + max: 180 + - name: ohold_turn_roll_limit + description: "Automatic roll lean [deg] allowed while a commanded turn (the stick of the pose's vertical axis: rudder at level/inverted, elevator at the knife) flies a curve in a hold. The lean IS the curve physics - tan(bank) = turn rate x speed / g - and is commanded into the target instead of being fought; this caps it. The lean exists only WHILE yaw is commanded. 0 = no lean, flat turns only." + default_value: 15 + field: turnRollLimitDeg + min: 0 + max: 60 + - name: ohold_turn_roll_return + description: "Time [ms] the automatic curve lean eases back out after the yaw stick returns to centre (gentle, no snap)." + default_value: 1000 + field: turnRollReturnMs + min: 100 + max: 5000 + + - name: PG_FIGURE_SEQUENCER_CONFIG + type: figureSequencerConfig_t + headers: ["flight/figure_sequencer.h"] + condition: USE_FW_AEROBATICS + members: + - name: fig_roll_rate + description: "Roll rate [deg/s] flown by the FIGURE ROLL and FIGURE 4PT ROLL modes" + default_value: 90 + field: rollRate + min: 30 + max: 360 + - name: fig_loop_rate + description: "Pitch rate [deg/s] flown by the FIGURE LOOP mode" + default_value: 90 + field: loopRate + min: 30 + max: 360 + - name: fig_point_dwell + description: "Dwell time [ms] on each point of the FIGURE 4PT ROLL" + default_value: 500 + field: pointDwellMs + min: 100 + max: 2000 + - name: fig_assist_z_gain + description: "Altitude assist: nose-up offset [deg per 10 m] of altitude error during figures. The controller distributes the offset to elevator and rudder as the roll phase demands" + default_value: 20 + field: assistZGain + min: 0 + max: 100 + - name: fig_assist_vz_gain + description: "Altitude assist: nose-up offset [deg per m/s] of sink rate during figures. Keep low: the climb rate estimate lags and a strong damping term fights fast figures" + default_value: 1 + field: assistVzGain + min: 0 + max: 20 + - name: fig_assist_max + description: "Cap [deg] on the altitude assist nose-up offset" + default_value: 12 + field: assistMax + min: 0 + max: 30 + + - name: PG_SOARING_CONFIG + type: soaringConfig_t + headers: ["flight/soaring.h"] + condition: USE_SOARING + members: + - name: soar_vario_trigger + description: "Net (total-energy) climb rate [cm/s] above which the SOARING mode stops cruising and starts circling a thermal. Requires a pitot - the net vario is meaningless without airspeed." + default_value: 50 + field: varioTriggerCms + min: 0 + max: 1000 + - name: soar_vario_exit + description: "Net climb rate [cm/s] below which circling stops and the aircraft returns to cruise (the thermal was flown through or died)." + default_value: 0 + field: varioExitCms + min: 0 + max: 1000 + - name: soar_alt_min + description: "Do not enter a thermal below this altitude [m] - a safety floor for autonomous soaring." + default_value: 50 + field: altMinM + min: 0 + max: 3000 + - name: soar_alt_max + description: "Leave the thermal once this altitude [m] is reached." + default_value: 500 + field: altMaxM + min: 0 + max: 5000 + - name: soar_bank + description: "Bank angle [deg] flown while circling a thermal." + default_value: 35 + field: bankDeg + min: 15 + max: 50 + - name: soar_sink_level + description: "Level-flight sink rate [cm/s] at the tuning airspeed, used to compensate the aircraft's own sink out of the net vario. Raise it if the vario reads high in still air, lower it if it reads low. Default ~60 is a 1.8 m motor glider (measured min sink 0.6 m/s); a clean sailplane is lower." + default_value: 60 + field: sinkLevelCms + min: 0 + max: 500 + - name: soar_centre_gain + description: "Gain [%] on the thermal-centering gradient shift. Higher centres faster but chases turbulence; the wind-drift shift is always applied at full wind speed regardless." + default_value: 100 + field: centreGainPct + min: 0 + max: 300 diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c new file mode 100644 index 00000000000..7c5008390d3 --- /dev/null +++ b/src/main/flight/altitude_floor.c @@ -0,0 +1,286 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#ifdef USE_FW_AEROBATICS + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/vector.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/altitude_floor.h" +#include "flight/imu.h" + +#include "navigation/navigation.h" + +#include "sensors/battery.h" + +#include "rx/rx.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, PG_ALTITUDE_FLOOR_CONFIG, 0); + +PG_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, + .floorAltitude = SETTING_ALT_FLOOR_ALTITUDE_DEFAULT, + .floorMargin = SETTING_ALT_FLOOR_MARGIN_DEFAULT, + .floorClimbPitch = SETTING_ALT_FLOOR_CLIMB_PITCH_DEFAULT, +); + +static bool floorArmed = false; // climbed above floor + margin once +static bool floorRecovery = false; +static bool floorOrbit = false; // recovery phase 2: circle at the floor +static bool orbitViaNav = false; // orbit flown by the nav loiter +static bool sticksSeenCentered = false; +static fpVector3_t breachPos; // orbit anchor: where the floor broke + +// Orbit once the climb is done: circle the BREACH POINT and wait. Level +// flight puts the antenna back at the sky, so GPS is healthy again +// (Daniel: no-GPS is only the degradation, not the design case) - with a +// healthy position estimate the aircraft is handed to the REAL fixed-wing +// loiter (forced position hold on the breach point, nav_fw_loiter_radius, +// wind-corrected; Daniel: use the loitering machinery). Without one it +// degrades to a constant-bank circle flown by the hold. Only a stick +// takeover or switching the box away releases either. +#define FLOOR_ORBIT_BANK_DEG 22.0f +// P-gain of the orbit altitude hold: 1 deg pitch per metre of error, +// bounded well below the climb pitch - the orbit holds, it does not zoom +#define FLOOR_ORBIT_PITCH_P_DEG_PER_M 1.0f +#define FLOOR_ORBIT_PITCH_MIN_DEG (-5.0f) +#define FLOOR_ORBIT_PITCH_MAX_DEG 10.0f + +void altitudeFloorUpdate(void) +{ + if (!IS_RC_MODE_ACTIVE(BOXALTFLOOR) || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE) + || !navIsAltitudeEstimateTrusted()) { + floorArmed = false; + floorRecovery = false; + floorOrbit = false; + if (orbitViaNav) { + navForcedPosholdClear(); + orbitViaNav = false; + } + return; + } + + const float z = getEstimatedActualPosition(Z); // cm above home + const float vz = getEstimatedActualVelocity(Z); // cm/s + const float floorCm = altitudeFloorConfig()->floorAltitude * 100.0f; + const float marginCm = altitudeFloorConfig()->floorMargin * 100.0f; + + if (!floorArmed) { + // Arm only after climbing above floor + margin once, so switching + // the box on while on the ground (or arming below the floor) never + // grabs the aircraft during takeoff + floorArmed = z > (floorCm + marginCm); + return; + } + + if (!floorRecovery) { + // Engage when the aircraft BREAKS THROUGH the floor, sinking - no + // prediction. A piloted trajectory is not predictable (a loop + // downline at 30 m/s "predicts" a 90 m crash and pulls out in 15; + // measured as the floor silently co-flying every fast loop under + // the old 3 s lookahead). The line is the contract: above it the + // sky belongs to the pilot, crossing it downward triggers the + // recovery, and the height below the line is the recovery budget + // the user chooses with alt_floor_altitude. + if (vz < 0.0f && z < floorCm) { + floorRecovery = true; + floorOrbit = false; + sticksSeenCentered = false; + // the breach point becomes the orbit anchor; the loiter + // altitude is the floor + margin the climb ends at + breachPos.x = getEstimatedActualPosition(X); + breachPos.y = getEstimatedActualPosition(Y); + breachPos.z = floorCm + marginCm; + } + } else { + // RE-BREACH GUARD: the nav poshold honors the pilot's pitch stick + // as a climb-rate override, so a panic-HELD down-elevator rides + // the orbit back through the line (measured: orbit descending to + // 24 m under a held stick, floor at 55). A held stick is NOT a + // takeover - sinking back through the floor drops the orbit and + // re-engages the aggressive climb (attitude force + throttle + // floor own the aircraft again) until the height is recaptured. + if (floorOrbit && vz < 0.0f && z < floorCm) { + floorOrbit = false; + if (orbitViaNav) { + navForcedPosholdClear(); + orbitViaNav = false; + } + } + // Climb done (back above floor + margin, climbing): do NOT hand + // back - transition to the ORBIT. The aircraft circles at the + // floor around the breach point and WAITS; after the shock the + // pilot gets as many seconds as they need to collect themselves. + // With a healthy position estimate the REAL fixed-wing loiter + // flies it (forced poshold on the breach point - immune to the + // post-figure heading estimate, it flies GPS vectors); otherwise + // the hold flies a constant-bank circle as the degraded form. + if (!floorOrbit && z > (floorCm + marginCm) && vz > 0.0f) { + floorOrbit = true; + if (navigationPositionEstimateIsHealthy()) { + navForcedPosholdActivateAt(&breachPos); + orbitViaNav = true; + } + } + if (orbitViaNav) { + // the poshold FSM re-anchors on current position at init - + // keep the breach point asserted every cycle + navForcedPosholdAssert(&breachPos); + } +#if defined(SITL_BUILD) + // bench telemetry (demuxed into the flight CSV): how the recovery + // relates to the breach anchor - heading error to it and distance + // from it. SITL only, same rationale as the safety word. + { + const float dx = breachPos.x - getEstimatedActualPosition(X); + const float dy = breachPos.y - getEstimatedActualPosition(Y); + const int32_t brg = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(dy, dx))); + debug[4] = wrap_18000(brg - attitude.values.yaw * 10) / 10; // deg x10 + debug[5] = (int32_t)(calc_length_pythagorean_2D(dx, dy) / 100.0f); // m + } +#endif + // The ONLY releases: the pilot takes over (sticks must return to + // center ONCE first - the panic-held down-elevator from the dive + // is not a takeover - then a fresh roll/pitch deflection hands + // control back immediately; yaw stays steering), or the box goes + // off (guard clause above). RAW receiver sticks, not rcCommand: + // the fixed-wing nav loiter WRITES rcCommand to fly the orbit, + // and reading the mix made the loiter release itself (measured). + const bool deflected = + ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) > rcControlsConfig()->deadband + || ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) > rcControlsConfig()->deadband; + if (!sticksSeenCentered) { + sticksSeenCentered = !deflected; + } else if (deflected) { + floorRecovery = false; + floorOrbit = false; + if (orbitViaNav) { + navForcedPosholdClear(); + orbitViaNav = false; + } + } + } +} + +bool altitudeFloorRecoveryActive(void) +{ + return floorRecovery; +} + +bool altitudeFloorArmed(void) +{ + return floorArmed; +} + +float altitudeFloorRecoveryPitchDeg(void) +{ + if (floorOrbit) { + // orbit altitude hold: small proportional pitch about the orbit + // altitude (floor + margin), never the full climb pitch + const float targetCm = (altitudeFloorConfig()->floorAltitude + + altitudeFloorConfig()->floorMargin) * 100.0f; + const float errM = (targetCm - getEstimatedActualPosition(Z)) / 100.0f; + return constrainf(errM * FLOOR_ORBIT_PITCH_P_DEG_PER_M, + FLOOR_ORBIT_PITCH_MIN_DEG, FLOOR_ORBIT_PITCH_MAX_DEG); + } + return (float)altitudeFloorConfig()->floorClimbPitch; +} + +float altitudeFloorRecoveryRollDeg(void) +{ + if (!floorOrbit || orbitViaNav) { + return 0.0f; // climb flies wings level; the nav loiter flies itself + } + // Degraded orbit (no healthy position estimate): a constant-bank + // circle that drifts with the wind but needs NO heading estimate - + // deliberately, because the post-figure heading can sit on the + // antipode (measured: 184 deg yaw error after a flat spin, stable + // for 60 s - mag and COG corrections vanish at sin(180)). Any law + // that steers by heading dies there; the blind circle does not. + return FLOOR_ORBIT_BANK_DEG; +} + +bool altitudeFloorOrbitActive(void) +{ + return floorOrbit; +} + +bool altitudeFloorRecoveryNoseUp(void) +{ + // ATTITUDE GATE for the recovery power (flight contract): while the + // nose points DOWN the pilot's chopped throttle is followed - the + // recovery must NEVER accelerate toward the ground (a powered + // nose-down bank is the death spiral). Power is allowed only once the + // attitude points UP: upright-ish and the nose above the horizon. + return floorRecovery + && attitude.values.pitch >= 0 + && calculateCosTiltAngle() >= 0.5f; // within ~60 deg of upright +} + +int16_t altitudeFloorClimbThrottleUs(void) +{ + // The recovery climb tops the pilot's throttle up to what the climb + // needs (cruise + the standard pitch-to-throttle compensation) - but + // ONLY once the attitude points up (altitudeFloorRecoveryNoseUp): + // nose down, the chopped throttle is followed and the motor stays + // off. NOT while the orbit runs on the nav loiter - the nav owns + // pitch AND throttle there, and a parallel climb floor pumps energy + // against its altitude hold (measured: ballooned the 70 m orbit to + // 212 m). 0 = no claim on the throttle. + if (!floorRecovery || orbitViaNav || !altitudeFloorRecoveryNoseUp()) { + return 0; + } + return currentBatteryProfile->nav.fw.cruise_throttle + + lrintf(altitudeFloorRecoveryPitchDeg() * currentBatteryProfile->nav.fw.pitch_to_throttle); +} + +bool altitudeFloorOrbitViaNav(void) +{ + return orbitViaNav; +} + +// metres above (positive) or below (negative) the floor line - the +// telemetry/OSD readout of how much sky is left before the net +float altitudeFloorDistanceM(void) +{ + const float floorCm = altitudeFloorConfig()->floorAltitude * 100.0f; + return (getEstimatedActualPosition(Z) - floorCm) / 100.0f; +} + +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h new file mode 100644 index 00000000000..f69854e72cb --- /dev/null +++ b/src/main/flight/altitude_floor.h @@ -0,0 +1,84 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include + +#include "config/parameter_group.h" + +// Altitude floor ("training floor"): while the ALT FLOOR box is active and +// the aircraft has climbed above floor + margin once, BREAKING THROUGH the +// floor while sinking engages an automatic recovery (shortest-path roll to +// upright plus climb pitch via the orientation hold controller) until the +// aircraft is back above the floor and climbing. No prediction - a piloted +// trajectory is not predictable; the line itself is the law. Switch the +// box off to land. + +typedef struct altitudeFloorConfig_s { + uint16_t floorAltitude; // m above home + uint16_t floorMargin; // m above the floor to arm / release + uint8_t floorClimbPitch; // deg nose-up target during recovery +} altitudeFloorConfig_t; + +PG_DECLARE(altitudeFloorConfig_t, altitudeFloorConfig); + +// Run once per RC processing cycle (before flight mode selection) +void altitudeFloorUpdate(void); + +// True while the automatic recovery is flying the aircraft +bool altitudeFloorRecoveryActive(void); + +// True once the floor is armed (climbed above floor + margin) +bool altitudeFloorArmed(void); + +// Recovery pitch target (deg, nose up): full climb pitch while below the +// floor, a gentle altitude-hold pitch once orbiting +float altitudeFloorRecoveryPitchDeg(void); + +// Recovery roll target (deg): wings level during the climb, the orbit +// bank once the aircraft is back at the floor waiting for the pilot +float altitudeFloorRecoveryRollDeg(void); + +// True in recovery phase 2: circling at the floor around the breach +// point until the pilot takes over (stick input) or switches the box off +bool altitudeFloorOrbitActive(void); + +// True while the orbit is flown by the forced nav loiter (healthy +// position estimate); false in the degraded constant-bank circle +bool altitudeFloorOrbitViaNav(void); + +// Throttle floor for the recovery climb [us]: cruise throttle plus +// pitch-to-throttle for the climb angle; 0 while inactive or while the +// nav loiter owns the throttle. The throttle path takes the MAX of all +// module claims - more pilot throttle always wins there. +// attitude gate for the recovery power: true only when the nose points up +// (upright-ish, nose above the horizon) - the contract's death-spiral guard +bool altitudeFloorRecoveryNoseUp(void); +int16_t altitudeFloorClimbThrottleUs(void); + +// Metres above (positive) / below (negative) the floor line - the +// telemetry/OSD readout of how much sky is left before the net +float altitudeFloorDistanceM(void); diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c new file mode 100644 index 00000000000..0f6ba13d6cc --- /dev/null +++ b/src/main/flight/crash_detection.c @@ -0,0 +1,245 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#ifdef USE_CRASH_DETECTION + +#include "common/maths.h" +#include "common/utils.h" +#include "common/vector.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/crash_detection.h" +#include "flight/mixer.h" + +#include "io/gps.h" + +#include "navigation/navigation.h" + +#include "rx/rx.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/gyro.h" + +// The master enable is the FEATURE_CRASH_DETECTION bit (a GUI feature +// toggle, default off) - no config group of its own; the impact +// threshold is derived from the accelerometer full-scale, not a setting. + +// In-flight latch: the detector must never fire while the armed aircraft is +// carried to the strip or waits for a hand launch (it IS still then). It +// arms once the aircraft is clearly flying: nav launch completed, or the +// throttle held above cruise level for a moment. +#define CRASH_INFLIGHT_THROTTLE_US 1350 +#define CRASH_INFLIGHT_HOLD_S 1.0f + +// Impact -> stillness confirmation window. A flying aircraft is never +// still, so aggressive maneuvers (snap, spin, gust) cannot confirm; the +// window must close before the post-figure flight smooths out (a level +// line a few seconds after a hard snap IS rate-still and 1 g). +#define CRASH_WINDOW_S 3.0f +// Stillness must exclude QUASI-STEADY FLIGHT, not only maneuvering: a +// smooth mushing climb also has low rates and ~1 g (found as a false +// positive in SITL - a hard pull spiked the impact latch and the steady +// climb after it read as "lying still"). The vertical-motion condition is +// the discriminator: a crashed aircraft has a FROZEN baro, flight does +// not. The RAW baro rate is used, not the fused vertical speed: the +// impact spike corrupts the INS for ~4.5 s (bench-measured, 12 g pulse), +// far beyond the window, while the baro is honest half a second after +// the airframe stops. +#define CRASH_STILL_RATE_DPS 15.0f +#define CRASH_STILL_ACC_G_LO 0.9f +#define CRASH_STILL_ACC_G_HI 1.1f +#define CRASH_STILL_VZ_CMS 100.0f +#define CRASH_STILL_CONFIRM_S 1.0f +#define CRASH_BARO_RATE_TAU_S 0.5f +#define CRASH_STILL_GS_CMS 300 + +static bool inFlight = false; +static float inFlightTimerS; +static float impactWindowS; +static float stillTimerS; +static bool motorCut = false; +static bool cutAckLow = false; +#ifdef USE_BARO +static float baroRateCms; +static float lastBaroAltCm; +#endif + +static float crashVerticalRateCms(float dT) +{ +#ifdef USE_BARO + if (sensors(SENSOR_BARO)) { + const float baroAltCm = baro.BaroAlt; + const float rawRate = (baroAltCm - lastBaroAltCm) / dT; + lastBaroAltCm = baroAltCm; + baroRateCms += (rawRate - baroRateCms) * MIN(dT / CRASH_BARO_RATE_TAU_S, 1.0f); + return baroRateCms; + } +#else + UNUSED(dT); // no-baro builds fall through to the fused estimate +#endif + return getEstimatedActualVelocity(Z); +} + +void crashDetectionUpdate(float dT) +{ + // Runs on anything that flies and can crash - fixed wing and + // multirotor alike (a crashed copter with its props chewing the ground + // or a bystander is exactly what the motor cut is for). Rovers and + // boats are excluded: an impact there is not a reason to cut the motor. + // Its own feature bit (default off): without FEATURE_CRASH_DETECTION + // the FC behaves exactly like upstream. Independent of the aerobatics + // suite - a crash detector is useful on any airframe. + if (!feature(FEATURE_CRASH_DETECTION) + || !(STATE(AIRPLANE) || STATE(MULTIROTOR)) + || !ARMING_FLAG(ARMED)) { + inFlight = false; + inFlightTimerS = 0.0f; + impactWindowS = 0.0f; + stillTimerS = 0.0f; + motorCut = false; + cutAckLow = false; +#ifdef USE_BARO + baroRateCms = 0.0f; + lastBaroAltCm = baro.BaroAlt; +#endif + return; + } + + // after a crash the motor stays CUT (not disarmed) until the pilot + // acknowledges: throttle to zero, then up again re-allows the motor - + // short bursts help locating the aircraft in high grass or corn + if (motorCut) { + const bool thrLow = rcCommand[THROTTLE] < getThrottleIdleValue() + 50; + if (!cutAckLow) { + cutAckLow = thrLow; + } else if (!thrLow) { + motorCut = false; + cutAckLow = false; + impactWindowS = 0.0f; + stillTimerS = 0.0f; + } + return; + } + + // In-flight latch: the detector must not fire while the armed aircraft + // sits on the ground or is carried (it IS still then). It arms once the + // aircraft is clearly flying. The throttle-held rule is platform-general + // (a copter above hover, a plane under power); a fixed-wing hand launch + // arms it earlier via the launch state. GPS-less models are covered - + // this never depends on a position fix. + if (!inFlight) { + if (isNavLaunchEnabled()) { + inFlight = fixedWingLaunchStatus() >= FW_LAUNCH_FLYING; + } else if (rcCommand[THROTTLE] > CRASH_INFLIGHT_THROTTLE_US) { + inFlightTimerS += dT; + inFlight = inFlightTimerS > CRASH_INFLIGHT_HOLD_S; + } else { + inFlightTimerS = 0.0f; + } + if (!inFlight) { + return; + } + } + + const float vertRateCms = crashVerticalRateCms(dT); + + fpVector3_t accG; + accGetMeasuredAcceleration(&accG); // cm/s^2 + const float accMagG = fast_fsqrtf(sq(accG.x) + sq(accG.y) + sq(accG.z)) / GRAVITY_CMSS; + + // Impact threshold = 15% below the DETECTED accelerometer's full-scale + // (13.6 g on a 16 g IMU, 27 g on a 32 g one). This is NOT a user setting: + // a spike that near saturation is an impact on any airframe, and the exact + // g need not be tuned per aircraft - a hard 3D figure can briefly reach a + // similar peak, but a crash is "a spike and then NOTHING": the stillness + // that must follow (below) is the real discriminator, not the g value. + const float accFullScaleG = (acc.dev.acc_1G > 0) ? (32767.0f / acc.dev.acc_1G) : 16.0f; + const float thresholdG = 0.85f * accFullScaleG; + // impact latches the confirmation window + if (accMagG > thresholdG) { + impactWindowS = CRASH_WINDOW_S; + stillTimerS = 0.0f; + } + if (impactWindowS <= 0.0f) { + return; + } + impactWindowS -= dT; + + // Rotation compared as the squared magnitude, to skip the per-loop sqrt + // (these boards run flash- and cycle-tight). It is the vector magnitude, + // deliberately NOT the mean of the axes like the landing detector's + // averageAbsGyroRates(): the mean blinds to a single-axis rate, and a wing + // dropping the wreck onto its back is exactly a single-axis rate. This is + // also why the crash stillness is its own test and not the landing + // detector's - that one fuses the vertical/horizontal velocity the impact + // corrupts for ~4.5 s (above), which our baro rate + GPS ground speed do not. + const float sqRateDps = sq((float)gyroRateDps(FD_ROLL)) + + sq((float)gyroRateDps(FD_PITCH)) + + sq((float)gyroRateDps(FD_YAW)); + bool still = sqRateDps < sq(CRASH_STILL_RATE_DPS) + && accMagG > CRASH_STILL_ACC_G_LO + && accMagG < CRASH_STILL_ACC_G_HI + && fabsf(vertRateCms) < CRASH_STILL_VZ_CMS; +#ifdef USE_GPS + // a valid fix adds the discriminator IMU + baro cannot provide: a + // coordinated line or shallow turn right after a hard pull is + // rate-still, 1 g and baro-flat - but it MOVES, a crashed airframe + // does not. Without GPS (or without a fix) the g threshold has to + // separate figures from impacts on its own. + if (still && sensors(SENSOR_GPS) && gpsSol.fixType >= GPS_FIX_3D) { + still = gpsSol.groundSpeed < CRASH_STILL_GS_CMS; + } +#endif + + if (still) { + stillTimerS += dT; + if (stillTimerS > CRASH_STILL_CONFIRM_S) { + // impact followed by stillness: the flight is over, cut the motor + motorCut = true; + cutAckLow = false; + } + } else { + stillTimerS = 0.0f; + } +} + +bool crashDetectionMotorCut(void) +{ + return motorCut; +} + +#endif // USE_CRASH_DETECTION diff --git a/src/main/flight/crash_detection.h b/src/main/flight/crash_detection.h new file mode 100644 index 00000000000..f96246fe2c8 --- /dev/null +++ b/src/main/flight/crash_detection.h @@ -0,0 +1,49 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include + +#include "config/parameter_group.h" + +// Crash detection for any flying platform (fixed wing and multirotor): +// after an impact the motor otherwise keeps running on the pilot's +// throttle. An impact (acceleration spike) followed by stillness (no +// rotation, resting 1 g) within a short window CUTS the motor while staying +// armed; the pilot re-allows it by moving the throttle to zero and up again +// (short bursts help locating the aircraft in high grass or corn). A flying +// aircraft is never still, so aggressive maneuvers (snaps, spins, hard +// gusts, freestyle) cannot trigger it - the stillness confirmation is the +// filter. Only armed AFTER the aircraft is clearly in the air: a fixed-wing +// hand launch, or throttle held above cruise for a moment (both platforms). + +// Master enable is FEATURE_CRASH_DETECTION (a GUI feature toggle, +// default off) - no config group of its own. + +// Call once per main PID loop iteration (after the IMU update) +void crashDetectionUpdate(float dT); + +// True while the post-crash motor cut is active (mixer forces idle) +bool crashDetectionMotorCut(void); diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c new file mode 100644 index 00000000000..f5771d31a41 --- /dev/null +++ b/src/main/flight/figure_sequencer.c @@ -0,0 +1,484 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include + +#ifdef USE_FW_AEROBATICS + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/figure_sequencer.h" +#include "flight/imu.h" +#include "flight/orientation_hold.h" + +#include "navigation/navigation.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(figureSequencerConfig_t, figureSequencerConfig, PG_FIGURE_SEQUENCER_CONFIG, 0); + +PG_RESET_TEMPLATE(figureSequencerConfig_t, figureSequencerConfig, + .rollRate = SETTING_FIG_ROLL_RATE_DEFAULT, + .loopRate = SETTING_FIG_LOOP_RATE_DEFAULT, + .pointDwellMs = SETTING_FIG_POINT_DWELL_DEFAULT, + .assistZGain = SETTING_FIG_ASSIST_Z_GAIN_DEFAULT, + .assistVzGain = SETTING_FIG_ASSIST_VZ_GAIN_DEFAULT, + .assistMax = SETTING_FIG_ASSIST_MAX_DEFAULT, +); + +PG_REGISTER_ARRAY(figureSegment_t, MAX_FIGURE_SEQUENCE_SEGMENTS, figureSequence, PG_FIGURE_SEQUENCE, 0); + +typedef enum { + FIGURE_NONE = 0, + FIGURE_ROLL, + FIGURE_LOOP, + FIGURE_POINT_ROLL, + FIGURE_SEQUENCE, +} figureType_e; + +typedef enum { + FIG_STATE_IDLE = 0, + FIG_STATE_RUNNING, + FIG_STATE_DONE, // figure complete, holding level until box released +} figureState_e; + +static figureType_e activeFigure = FIGURE_NONE; +static figureState_e state = FIG_STATE_IDLE; +static timeMs_t startTimeMs; +static float startAltitudeCm; +static float targetRollDeg; +static float targetPitchDeg; +// governed rotation phase of the active figure / rotating segment (deg); +// advances by rate * load-governor * dT, resets at figure and segment start +static float figPhaseDeg; +static timeMs_t figPhaseLastMs; + +// sequence (FIGURE SEQ) state +static int seqIndex; +static timeMs_t seqSegStartMs; +static float seqBaseRoll; +static float seqBasePitch; +static float seqSegAltCm; // assist reference, captured at segment entry +static bool seqImpulseActive; +static float seqImpulseRates[3]; +static bool seqTurnCoordination; +static float seqTurnBankDeg; +static bool seqSpinActive; +static float seqSpinYawNorm; +static bool seqSpinTracking; // turn counter armed +static float seqSpinTurnDeg; // accumulated (wrap-aware) yaw rotation +static float seqSpinPrevYawDeg; + +static figureType_e requestedFigure(void) +{ + if (IS_RC_MODE_ACTIVE(BOXFIGSEQ)) { + return FIGURE_SEQUENCE; + } + if (IS_RC_MODE_ACTIVE(BOXFIGROLL)) { + return FIGURE_ROLL; + } + if (IS_RC_MODE_ACTIVE(BOXFIGLOOP)) { + return FIGURE_LOOP; + } + if (IS_RC_MODE_ACTIVE(BOXFIGPOINTROLL)) { + return FIGURE_POINT_ROLL; + } + return FIGURE_NONE; +} + +// Altitude assist: earth referenced nose-above-horizon offset from an +// altitude/climb-rate PID, blended out as the nose approaches vertical +// (there altitude is a thrust problem, not an attitude problem) +float figureAltitudeAssistDeg(float nosePitchDeg, float refAltCm) +{ + if (!navIsAltitudeEstimateTrusted()) { + return 0.0f; + } + + const float zErrM = (refAltCm - getEstimatedActualPosition(Z)) / 100.0f; + const float sinkMs = -getEstimatedActualVelocity(Z) / 100.0f; + + float offset = (figureSequencerConfig()->assistZGain / 10.0f) * zErrM + + figureSequencerConfig()->assistVzGain * sinkMs; + offset = constrainf(offset, -figureSequencerConfig()->assistMax, figureSequencerConfig()->assistMax); + + // The offset must raise the NOSE ELEVATION. With an accumulated pitch + // parameter past +/-90 (e.g. base pitch 180 after a half loop) the raw + // pitch parameter acts inverted on the elevation: elevation = sin(pitch), + // d(elevation)/d(pitch) flips sign with cos(pitch). Blend out toward + // nose-vertical with |cos| (= cos of the true elevation). + const float cosPitch = cos_approx(DEGREES_TO_RADIANS(nosePitchDeg)); + return offset * cosPitch; // magnitude blends with |cos|, sign corrects the direction +} + +void figureSequencerUpdate(void) +{ + // the load governor also serves the FLAT SPIN preset (no figure active), + // so it updates before the early return below + orientationHoldLoadGovernorUpdate(); + + const figureType_e req = requestedFigure(); + + if (req == FIGURE_NONE || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE)) { + activeFigure = FIGURE_NONE; + state = FIG_STATE_IDLE; + // the transient command flags MUST die with the figure: aborting + // mid-IMPULSE/SPIN (box off, disarm) otherwise leaves them + // latched, and pidOrientationHold keeps applying stale open-loop + // full-rate commands - AHEAD of the floor/rotor recovery target, + // defeating the catch (review finding, verified at pid.c impulse + // branch: it returns before the recovery error is computed) + seqImpulseActive = false; + seqSpinActive = false; + seqTurnCoordination = false; + return; + } + + if (state == FIG_STATE_IDLE || req != activeFigure) { + activeFigure = req; + state = FIG_STATE_RUNNING; + startTimeMs = millis(); + startAltitudeCm = getEstimatedActualPosition(Z); + seqIndex = 0; + seqSegStartMs = startTimeMs; + seqSpinTracking = false; + seqBaseRoll = 0.0f; + seqBasePitch = 0.0f; + seqSegAltCm = startAltitudeCm; + figPhaseDeg = 0.0f; + figPhaseLastMs = startTimeMs; + } + + // Rotating trajectories advance a GOVERNED phase instead of wall time: + // theta += rate * governor * dT, so the figure slows down exactly while + // the measured load exceeds the display budget ("fast AND tight" - the + // budget is the tightest radius the current speed allows, r = v^2/a). + // WAIT/dwell segments stay on wall time - a pause is a pause. + const float govScale = orientationHoldLoadGovernorScale(); + const timeMs_t phaseNowMs = millis(); + const float phaseDtS = constrainf((phaseNowMs - figPhaseLastMs) * 0.001f, 0.0f, 0.1f); + figPhaseLastMs = phaseNowMs; + + const float tS = (millis() - startTimeMs) * 0.001f; + float roll = 0.0f; + float pitch = 0.0f; + bool assist = false; + seqImpulseActive = false; // recomputed below while an IMPULSE runs + seqTurnCoordination = false; + seqSpinActive = false; // recomputed below while a SPIN runs + + switch (activeFigure) { + case FIGURE_ROLL: { + figPhaseDeg += figureSequencerConfig()->rollRate * govScale * phaseDtS; + roll = MIN(figPhaseDeg, 360.0f); + assist = true; + if (figPhaseDeg >= 360.0f) { + state = FIG_STATE_DONE; + roll = 0.0f; // 360 == 0, hold level + } + break; + } + + case FIGURE_LOOP: { + figPhaseDeg += figureSequencerConfig()->loopRate * govScale * phaseDtS; + pitch = MIN(figPhaseDeg, 360.0f); + if (figPhaseDeg >= 360.0f) { + state = FIG_STATE_DONE; + pitch = 0.0f; + assist = true; // level again: hold the entry altitude + } + break; + } + + case FIGURE_SEQUENCE: { + // advance through the programmed segment chain + while (state == FIG_STATE_RUNNING) { + if (seqIndex >= MAX_FIGURE_SEQUENCE_SEGMENTS + || figureSequence(seqIndex)->type == FIGSEG_END + || figureSequence(seqIndex)->type >= FIGSEG_TYPE_COUNT) { + state = FIG_STATE_DONE; + break; + } + + const figureSegment_t *seg = figureSequence(seqIndex); + const float tSeg = (millis() - seqSegStartMs) * 0.001f; + bool segDone = false; + + switch (seg->type) { + case FIGSEG_ROLL: { + const float span = ABS((float)seg->p1); + figPhaseDeg += figureSequencerConfig()->rollRate * govScale * phaseDtS; + const float theta = MIN(figPhaseDeg, span); + roll = seqBaseRoll + (seg->p1 < 0 ? -theta : theta); + pitch = seqBasePitch; + assist = seg->flags & FIGSEG_FLAG_ASSIST; + if (theta >= span) { + seqBaseRoll += seg->p1; + segDone = true; + } + break; + } + + case FIGSEG_PITCH: { + const float span = ABS((float)seg->p1); + figPhaseDeg += figureSequencerConfig()->loopRate * govScale * phaseDtS; + const float theta = MIN(figPhaseDeg, span); + roll = seqBaseRoll; + pitch = seqBasePitch + (seg->p1 < 0 ? -theta : theta); + assist = false; + if (theta >= span) { + seqBasePitch += seg->p1; + segDone = true; + } + break; + } + + case FIGSEG_HOLD: + seqBaseRoll = seg->p1; + seqBasePitch = seg->p2; + roll = seqBaseRoll; + pitch = seqBasePitch; + assist = seg->flags & FIGSEG_FLAG_ASSIST; + segDone = tSeg * 1000.0f >= seg->p3; + break; + + case FIGSEG_WAIT_ALT: { + // wings level, climb/descend to the target altitude + // via the assist mechanism, gate until reached + seqBaseRoll = 0.0f; + seqBasePitch = 0.0f; + roll = 0.0f; + pitch = 0.0f; + seqSegAltCm = seg->p1 * 100.0f; + assist = true; + const float tolCm = MAX(seg->p2, 1) * 100.0f; + segDone = navIsAltitudeEstimateTrusted() + && ABS(getEstimatedActualPosition(Z) - seqSegAltCm) < tolCm + && ABS(getEstimatedActualVelocity(Z)) < 150.0f; + break; + } + + case FIGSEG_WAIT_TIME: + roll = seqBaseRoll; + pitch = seqBasePitch; + assist = seg->flags & FIGSEG_FLAG_ASSIST; + segDone = tSeg * 1000.0f >= seg->p3; + break; + + case FIGSEG_IMPULSE: + // open-loop rate impulse (snap/spin entry): full-rate + // commands saturate the surfaces; the following + // segment (or the DONE level hold) catches whatever + // attitude results, shortest path + seqImpulseActive = true; + seqImpulseRates[FD_ROLL] = 0.0f; + seqImpulseRates[FD_PITCH] = constrainf(seg->p1, -100, 100) * 0.01f; + seqImpulseRates[FD_YAW] = constrainf(seg->p2, -100, 100) * 0.01f; + roll = seqBaseRoll; + pitch = seqBasePitch; + segDone = tSeg * 1000.0f >= seg->p3; + break; + + case FIGSEG_SPIN: { + // controlled flat spin: roll and pitch stay CLOSED + // LOOP on the flat attitude (the controller actively + // keeps the plane flat and damps the wobble) while + // the rudder is held open loop for the autorotation. + // The segment ends after p1 full turns or the p3 + // timeout; the altitude floor preempts globally. + seqSpinActive = true; + seqSpinYawNorm = ((seg->p2 != 0) ? constrainf(seg->p2, -100, 100) : 100.0f) + * 0.01f * (seg->p1 < 0 ? -1.0f : 1.0f); + seqBaseRoll = 0.0f; + seqBasePitch = 0.0f; + roll = 0.0f; + pitch = 0.0f; + assist = false; // the spin descends by design + const float yawDeg = DECIDEGREES_TO_DEGREES((float)attitude.values.yaw); + if (!seqSpinTracking) { + seqSpinTracking = true; + seqSpinTurnDeg = 0.0f; + } else { + float d = yawDeg - seqSpinPrevYawDeg; + while (d > 180.0f) { d -= 360.0f; } + while (d < -180.0f) { d += 360.0f; } + seqSpinTurnDeg += d; + } + seqSpinPrevYawDeg = yawDeg; + const float timeoutMs = (seg->p3 > 0) ? seg->p3 : 15000.0f; + segDone = fabsf(seqSpinTurnDeg) >= ABS(seg->p1) * 360.0f + || tSeg * 1000.0f >= timeoutMs; + if (segDone) { + seqSpinTracking = false; + } + break; + } + + case FIGSEG_WAIT_POS: { + // airspace containment: bank toward HOME until the + // distance drops below the radius. The course loop + // lives only in this segment; the attitude modes + // stay heading-free + seqBaseRoll = 0.0f; + seqBasePitch = 0.0f; + pitch = 0.0f; + assist = true; + if (STATE(GPS_FIX_HOME)) { + const float maxBank = (seg->p2 > 0) ? seg->p2 : 30.0f; + float courseErr = GPS_directionToHome - DECIDEGREES_TO_DEGREES((float)attitude.values.yaw); + while (courseErr > 180.0f) { courseErr -= 360.0f; } + while (courseErr < -180.0f) { courseErr += 360.0f; } + roll = constrainf(0.8f * courseErr, -maxBank, maxBank); + // banked turn: feed the coordinated turn rates + // forward, otherwise the heading-free controller + // regulates the (physical) turn yaw rate to zero + seqTurnCoordination = true; + seqTurnBankDeg = roll; + segDone = GPS_distanceToHome < (uint32_t)MAX(seg->p1, 10); + } else { + roll = 0.0f; // no home fix: hold level, gate stays + segDone = false; + } + break; + } + + default: + segDone = true; + break; + } + + if (!segDone) { + break; + } + seqIndex++; + seqSegStartMs = millis(); + figPhaseDeg = 0.0f; // fresh governed phase per segment + if (figureSequence(MIN(seqIndex, MAX_FIGURE_SEQUENCE_SEGMENTS - 1))->type != FIGSEG_WAIT_ALT) { + seqSegAltCm = getEstimatedActualPosition(Z); // assist reference for the next segment + } + } + if (state == FIG_STATE_DONE) { + roll = 0.0f; + pitch = 0.0f; + assist = true; + } + break; + } + + case FIGURE_POINT_ROLL: { + // 4 points: rotate 90 deg at roll rate, dwell, repeat. + // Stays on wall time (ungoverned): an axial roll pulls no + // meaningful load, and the dwell timing must remain exact. + const float rotS = 90.0f / figureSequencerConfig()->rollRate; + const float dwellS = figureSequencerConfig()->pointDwellMs / 1000.0f; + const float segS = rotS + dwellS; + const int seg = (int)(tS / segS); + if (seg >= 4) { + state = FIG_STATE_DONE; + roll = 0.0f; + } else { + const float tInSeg = tS - seg * segS; + roll = seg * 90.0f + MIN(tInSeg / rotS, 1.0f) * 90.0f; + } + assist = true; + break; + } + + default: + break; + } + + if (state == FIG_STATE_DONE) { + roll = (activeFigure == FIGURE_LOOP) ? 0.0f : roll; + assist = true; + } + + targetRollDeg = roll; + targetPitchDeg = pitch + (assist + ? figureAltitudeAssistDeg(pitch, activeFigure == FIGURE_SEQUENCE ? seqSegAltCm : startAltitudeCm) + : 0.0f); +} + +bool figureSequencerRequested(void) +{ + return activeFigure != FIGURE_NONE && state != FIG_STATE_IDLE; +} + +bool figureSequencerHeadingAnchored(void) +{ + // line-hold: figure trajectories fly on the heading captured at figure + // start. Segments that change the heading on purpose (WAIT_POS banks + // toward home) or fly open loop (impulse, spin autorotation) release + // the anchor; it re-captures on the current heading when they end. + return figureSequencerRequested() + && !seqTurnCoordination && !seqImpulseActive && !seqSpinActive; +} + +bool figureSequencerGetTurnBank(float *bankDeg) +{ + if (!seqTurnCoordination) { + return false; + } + *bankDeg = seqTurnBankDeg; + return true; +} + +bool figureSequencerGetRateCommand(float ratesNorm[3]) +{ + if (!seqImpulseActive) { + return false; + } + for (int i = 0; i < 3; i++) { + ratesNorm[i] = seqImpulseRates[i]; + } + return true; +} + +bool figureSequencerGetSpinCommand(float *yawNorm) +{ + if (!seqSpinActive) { + return false; + } + *yawNorm = seqSpinYawNorm; + return true; +} + +void figureSequencerGetTarget(float *rollDeg, float *pitchDeg) +{ + *rollDeg = targetRollDeg; + *pitchDeg = targetPitchDeg; +} + +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h new file mode 100644 index 00000000000..7bc995948e6 --- /dev/null +++ b/src/main/flight/figure_sequencer.h @@ -0,0 +1,127 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include + +#include "config/parameter_group.h" + +// Figure sequencer: flies aerobatic figures as time parameterized +// orientation-hold targets (roll/pitch trajectories). Because the +// orientation hold controller is heading free, a figure needs no attitude +// capture: a roll always rotates about the current heading, a loop flies in +// the current heading plane. Figures start when their box goes active +// (from roughly level flight), hold level when complete, and re-arm when +// the box is released. +// +// 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), blended out as the nose approaches +// vertical where altitude belongs to the thrust axis. + +typedef struct figureSequencerConfig_s { + uint16_t rollRate; // deg/s target roll rate for roll figures + uint16_t loopRate; // deg/s target pitch rate for the loop + uint16_t pointDwellMs; // ms dwell on each point of the 4 point roll + uint8_t assistZGain; // deg of nose-up per 10 m of altitude error + uint8_t assistVzGain; // deg of nose-up per m/s of sink + uint8_t assistMax; // deg cap on the altitude assist offset +} figureSequencerConfig_t; + +PG_DECLARE(figureSequencerConfig_t, figureSequencerConfig); + +// Programmable figure sequence (FIGURE SEQ box): a chain of segments flown +// in order. Rotations are cumulative on the running attitude baseline, so +// e.g. Immelmann = PITCH +180 then ROLL +180. Wait segments gate the chain +// on preconditions (altitude now; position is reserved, it needs heading +// control / nav coupling). +#define MAX_FIGURE_SEQUENCE_SEGMENTS 16 + +typedef enum { + FIGSEG_END = 0, // terminator / unused slot + FIGSEG_ROLL = 1, // p1: signed deg, cumulative, at fig_roll_rate + FIGSEG_PITCH = 2, // p1: signed deg, cumulative, at fig_loop_rate + FIGSEG_HOLD = 3, // p1: roll deg, p2: pitch deg (absolute), p3: ms + FIGSEG_WAIT_ALT = 4, // p1: target altitude m above home, p2: tolerance m + FIGSEG_WAIT_TIME = 5, // p3: ms, holds the current baseline attitude + FIGSEG_IMPULSE = 6, // open-loop rate impulse (snap/spin entry): + // p1: pitch %, p2: yaw %, p3: ms; the next + // segment catches the attitude afterwards + FIGSEG_WAIT_POS = 7, // airspace containment: bank toward HOME until + // distance < p1 m; p2: max bank deg (0 = 30) + FIGSEG_SPIN = 8, // controlled flat spin: roll/pitch CLOSED LOOP + // on the flat attitude, rudder open loop for + // the autorotation. p1: full turns (sign = + // direction), p2: rudder % (0 = 100), + // p3: timeout ms (0 = 15000). Enter stalled + // via a preceding IMPULSE segment. + FIGSEG_TYPE_COUNT +} figureSegmentType_e; + +#define FIGSEG_FLAG_ASSIST (1 << 0) // altitude assist during this segment + +typedef struct figureSegment_s { + uint8_t type; + int16_t p1; + int16_t p2; + int16_t p3; + uint8_t flags; +} figureSegment_t; + +PG_DECLARE_ARRAY(figureSegment_t, MAX_FIGURE_SEQUENCE_SEGMENTS, figureSequence); + +// Run once per RC processing cycle (before flight mode selection) +void figureSequencerUpdate(void); + +// True while a figure box is active (sequencer wants ORIENTATION_HOLD_MODE) +bool figureSequencerRequested(void); + +// True while the current segment flies on the heading captured at figure +// start (the line-hold). WAIT_POS turns and open-loop impulse/spin segments +// release the anchor; it re-captures when they complete. +bool figureSequencerHeadingAnchored(void); + +// Current figure target, valid while requested +void figureSequencerGetTarget(float *rollDeg, float *pitchDeg); + +// Earth-referenced nose-above-horizon offset from an altitude/climb-rate PID, +// blended out as the nose approaches vertical. Shared with the plain +// orientation holds so INVERT/KNIFE actively hold their entry altitude too. +float figureAltitudeAssistDeg(float nosePitchDeg, float refAltCm); + +// True while an open-loop IMPULSE segment runs; returns the commanded body +// rates normalized to -1..1 of the profile's max rates (full deflection) +bool figureSequencerGetRateCommand(float ratesNorm[3]); + +// True while a WAIT_POS segment banks toward home; returns the commanded +// bank (deg) for coordinated-turn rate feedforward +bool figureSequencerGetTurnBank(float *bankDeg); + +// True while a SPIN segment runs; returns the open-loop rudder command +// normalized to -1..1. Roll and pitch stay CLOSED LOOP on the flat +// attitude while this is active - only the yaw axis goes open loop. +bool figureSequencerGetSpinCommand(float *yawNorm); diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c new file mode 100644 index 00000000000..40ec10e0d23 --- /dev/null +++ b/src/main/flight/hover_throttle.c @@ -0,0 +1,397 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#ifdef USE_FW_AEROBATICS + +#include "common/axis.h" +#include "common/maths.h" +#include "common/quaternion.h" +#include "common/vector.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/altitude_floor.h" +#include "flight/rotor_guard.h" +#include "flight/hover_throttle.h" +#include "flight/imu.h" +#include "flight/mixer.h" + +#include "sensors/acceleration.h" +#include "flight/orientation_hold.h" +#include "flight/pid.h" + +#include "navigation/navigation.h" + +#include "rx/rx.h" + +#include "sensors/battery.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 4); + +PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, + // Baro trust while the hover throttle owns the altitude. Kept as a field + // (runtime-tunable) but no longer a CLI setting; 100 = the experimentally + // found floor over inav_w_z_baro_p for the hover regime. + .hoverBaroWeight = 100, +); + +// Derived throttle gains. The one airframe fact they all share is the +// throttle-to-thrust slope: at the hover point thrust equals weight, so +// (hover throttle - idle) is the throttle span per 1 g of specific force. +// The hover point is LEARNED at engage (I-term seeded from the pilot's +// stick), so every "throttle us per unit of motion" gain derives from it at +// runtime; the loop-shaping constants below are dimensionless rates, fixed +// at the values the rig sweep found, and scale to any airframe through the +// learned span. Replaces the former ohold_hover_thr_p/i/d and +// ohold_assist_thr_p/i settings - nobody can set "throttle us per m/s" +// better than this identity derives it. +#define HOVER_THR_STIFFNESS_S2 1.5f // [1/s^2] altitude error -> accel +#define HOVER_THR_DAMPING_S 1.8f // [1/s] climb rate -> decel +#define HOVER_THR_TRIM_S2 0.18f // [1/s^2] slow altitude trim (I) +#define ASSIST_DAMPING_S 0.8f // [1/s] knife/inverted vz damping +#define ASSIST_TRIM_S2 0.4f // [1/s^2] knife/inverted vz trim +#define GRAVITY_MSS (GRAVITY_CMSS / 100.0f) + +// throttle us that produce 1 g of specific-force change, from the learned +// base throttle (floored: a wrong low base must not collapse the gains) +static float usPerG(float baseUs) +{ + return MAX(baseUs - getThrottleIdleValue(), 100.0f); +} + +// Engage only when the nose is this close to the zenith; once engaged, +// stay active down to the release threshold. Without the hysteresis the +// attitude wobble around the hang flaps the controller and every +// re-engage captures a NEW altitude target at the current height -- a +// ratcheting drift. +#define HOVER_ENGAGE_NOSE_ELEVATION_DEG 60.0f +#define HOVER_RELEASE_NOSE_ELEVATION_DEG 45.0f + +// The target latches only once the vertical motion has settled: engaging +// mid pull-up (the normal way to enter a hang) must not freeze the target +// at some fly-through altitude +#define HOVER_LATCH_CLIMB_CMS 200.0f + +// Pilot correction: the throttle stick outside the mid deadband commands a +// CLIMB RATE (full deflection = this many m/s) while the controller keeps +// owning the throttle. Direct pilot throttle on top of the altitude PID +// would be two controllers fighting over one actuator - a classic +// oscillation; commanding the target instead leaves a single loop. +// Centered stick = hold; releasing latches the new altitude. +#define HOVER_STICK_CLIMB_MS 2.0f + +static bool hoverActive = false; +static bool hoverLatched = false; +static float targetAltCm; +static float iTermUs; +static int16_t stickRefUs; +static timeUs_t lastUpdateUs; +// slow filter of the APPLIED hover throttle: the thrust that actually +// carries the weight is the true 1 g point, independent of where the +// pilot's stick happened to sit at engage - the derived gains anchor here +// (an engage seed 200 us low made the altitude loop 36% too soft in SITL) +#define HOVER_THR_ANCHOR_TAU_S 2.0f +static float hoverThrAnchorUs; + +// ---- Knife/inverted throttle assist ---------------------------------------- +// +// In a knife edge or inverted hold the attitude controller owns the surfaces +// and the pitch-based altitude 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 +// and the pitch assist saturates against the missing energy. The throttle +// criterion of these holds is vz -> 0: a slow, integrating TRIM around the +// pilot's stick adds throttle while the hold sinks and takes it back while +// it climbs. The pilot stays the base - moving the stick moves the whole +// operating point, the learned trim rides on top. + +#define ASSIST_TRIM_MAX_US 150.0f +#define ASSIST_VZ_CLAMP_MS 4.0f // |vz| beyond this is an entry/zoom + // transient: freeze the trim, cap the + // damping term +#define ASSIST_OSC_RAISE_US_S 30.0f // trim raise rate while the hold + // oscillates (starved surfaces) +#define ASSIST_COS_FLOOR 0.5f // forward-component compensation cap + // (above ~60 deg the hover PID owns + // the throttle anyway) + +// Stall reserve: a good regulator MASKS the approach to the envelope edge - +// the attitude stays clean while the surfaces silently work their way +// toward saturation, then everything lets go at once (a ramp becomes a +// cliff; field observation). The mean control effort is therefore the EARLY +// escalation criterion, ahead of sinking and far ahead of oscillation: +// above the effort threshold the assist raises the speed while reserve is +// still left. +#define ASSIST_EFFORT_TAU_S 1.5f // effort trend low-pass +#define ASSIST_EFFORT_THRESHOLD 0.7f // of the pidSum authority +#define ASSIST_EFFORT_RAISE_US_S 40.0f // full raise rate at 100% effort + +static bool assistActive = false; +static float assistTrimUs; +static float assistCosRef; +static float assistEffortFilt; +static timeUs_t assistLastUs; + +static float assistControlEffort(void) +{ + float effort = 0.0f; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + const uint16_t limit = getPidSumLimit(axis); + if (limit > 0) { + effort = MAX(effort, fabsf((float)axisPID[axis]) / limit); + } + } + return MIN(effort, 1.0f); +} + +static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) +{ + // a deliberate throttle cut stays a throttle cut + if (!navIsAltitudeEstimateTrusted() + || pilotThrottle < getThrottleIdleValue() + 50) { + assistActive = false; + return pilotThrottle; + } + + const timeUs_t nowUs = micros(); + const float cosNow = MAX(cos_approx(DEGREES_TO_RADIANS(elevDeg)), ASSIST_COS_FLOOR); + if (!assistActive) { + assistActive = true; + assistTrimUs = 0.0f; + assistCosRef = cosNow; + assistEffortFilt = 0.0f; + assistLastUs = nowUs; + } + const float dT = constrainf((nowUs - assistLastUs) * 1e-6f, 0.0f, 0.1f); + assistLastUs = nowUs; + + // the CHOSEN speed is kept as the FORWARD component: when the nose + // rises (assist, speed feedforward, harrier transition) the horizontal + // thrust share shrinks with cos(theta) - scale the pilot's base so + // T*cos(theta) stays at its engage value instead of bleeding speed + const float baseUs = getThrottleIdleValue() + + (pilotThrottle - getThrottleIdleValue()) * (assistCosRef / cosNow); + + // gains derived from the pilot's own operating point: the throttle span + // above idle is the thrust the pilot flies with, and the us-per-motion + // gains scale with it (same identity as the hover PID above) + const float assistUsPerG = usPerG(baseUs); + const float climbMs = getEstimatedActualVelocity(Z) / 100.0f; + if (fabsf(climbMs) < ASSIST_VZ_CLAMP_MS) { + assistTrimUs = constrainf(assistTrimUs - ASSIST_TRIM_S2 * assistUsPerG / GRAVITY_MSS * climbMs * dT, + -ASSIST_TRIM_MAX_US, ASSIST_TRIM_MAX_US); + } + // an oscillating hold means the surfaces are starving: raise the + // operating point (more airflow), the gain learner only treats the + // symptom + if (orientationHoldRegimeOscillating()) { + assistTrimUs = constrainf(assistTrimUs + ASSIST_OSC_RAISE_US_S * dT, + -ASSIST_TRIM_MAX_US, ASSIST_TRIM_MAX_US); + } + // stall reserve (the EARLY criterion): sustained control effort toward + // saturation raises the speed while the attitude still looks clean + assistEffortFilt += (assistControlEffort() - assistEffortFilt) + * MIN(dT / ASSIST_EFFORT_TAU_S, 1.0f); + if (assistEffortFilt > ASSIST_EFFORT_THRESHOLD) { + const float urgency = (assistEffortFilt - ASSIST_EFFORT_THRESHOLD) + / (1.0f - ASSIST_EFFORT_THRESHOLD); + assistTrimUs = constrainf(assistTrimUs + ASSIST_EFFORT_RAISE_US_S * urgency * dT, + -ASSIST_TRIM_MAX_US, ASSIST_TRIM_MAX_US); + } + const float damping = -ASSIST_DAMPING_S * assistUsPerG / GRAVITY_MSS + * constrainf(climbMs, -ASSIST_VZ_CLAMP_MS, ASSIST_VZ_CLAMP_MS); + + // throttle_rule (flight contract, cap-only): the pilot's stick is the + // MAXIMUM - trim, damping, cos scale and stall reserve shape the power + // BELOW it, never above. Too little stick = controlled descent with the + // attitude held; an estimator faking "sinking" can never command + // unexpected power. The thumb is the motor. + return constrain(lrintf(baseUs + assistTrimUs + damping), + getThrottleIdleValue(), pilotThrottle); +} + +bool hoverThrottleIsEngaged(void) +{ + return hoverActive; +} + +static float noseElevationDeg(void) +{ + fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; + quaternionRotateVectorInv(&nose, &nose, &orientation); // body -> earth + return RADIANS_TO_DEGREES(asin_approx(constrainf(-nose.z, -1.0f, 1.0f))); +} + +int16_t hoverThrottleApply(int16_t pilotThrottle) +{ + const float elevDeg = noseElevationDeg(); + const float elevGate = hoverActive ? HOVER_RELEASE_NOSE_ELEVATION_DEG + : HOVER_ENGAGE_NOSE_ELEVATION_DEG; + + // The altitude ownership follows the ATTITUDE, not the selected box: + // above the elevation gate the thrust carries the weight (T*sin(alpha)) + // and the hover controller owns the altitude - also when the pilot + // pulled a knife edge or inverted hold up into a harrier with the stick + // offsets. Below the gate the vz trim is the (indirect) energy path. + // This is the alpha continuum: knife -> harrier -> hover is one + // mechanism whose direct-thrust share is the tilt compensation. + const bool thrustAxisHold = orientationHoldIsPropHang() + || orientationHoldIsKnifeOrInverted(); + + if (!ARMING_FLAG(ARMED) + || !thrustAxisHold + || !navIsAltitudeEstimateTrusted() + || elevDeg < elevGate) { + hoverActive = false; + if (ARMING_FLAG(ARMED) && orientationHoldIsKnifeOrInverted()) { + return knifeInvertedAssistApply(pilotThrottle, elevDeg); + } + assistActive = false; + // Recovery throttle claims are OWNED by their modules (the floor's + // climb math, the rotor guard's boost/burst) - and per the + // throttle_rule (cap-only, NO exceptions - Daniel) they too are + // CAPPED at the pilot's stick: the recovery flies its computed + // need bounded by the thumb. PILOT WARNING (manual, fat print): + // during a floor or rotor-guard catch KEEP THE THROTTLE UP - the + // stick is the catch's power budget; a chopped stick leaves the + // catch attitude authority but NO climb power. + const int16_t recoveryFloor = MAX(altitudeFloorClimbThrottleUs(), + rotorGuardThrottleFloorUs()); + if (ARMING_FLAG(ARMED) && recoveryFloor > 0) { + // MIN, not constrain-to-idle: a chopped stick stays a chopped + // stick (throttle-0 rule) - the mixer's normal armed handling + // applies, exactly as in the passthrough below + return MIN(recoveryFloor, pilotThrottle); + } + return pilotThrottle; + } + assistActive = false; + + const float z = getEstimatedActualPosition(Z); + + // a stick slammed to the bottom stays a hard throttle cut (bailout); + // everything above commands a sink rate instead + if (pilotThrottle < getThrottleIdleValue() + 50) { + hoverActive = false; + return pilotThrottle; + } + + if (!hoverActive) { + hoverActive = true; + hoverLatched = false; + targetAltCm = z; + // seed the I-term with the last pilot throttle: learns the model's + // hover throttle online instead of requiring a setting + iTermUs = pilotThrottle; + hoverThrAnchorUs = pilotThrottle; + // the climb-rate stick references the ENGAGE position: entering the + // hover regime out of a knife/harrier pull-up at cruise throttle + // must not read as a climb command + stickRefUs = pilotThrottle; + lastUpdateUs = micros(); + } + + // pilot throttle outside the deadband around the engage reference: a + // climb-rate command, the controller keeps the throttle (see + // HOVER_STICK_CLIMB_MS). Linear beyond the deadband, full = full rate. + float stickClimbMs = 0.0f; + const int16_t stickOff = pilotThrottle - stickRefUs; + if (ABS(stickOff) > rcControlsConfig()->mid_throttle_deadband) { + const float span = (PWM_RANGE_MAX - PWM_RANGE_MIDDLE) - rcControlsConfig()->mid_throttle_deadband; + const float beyond = (float)(ABS(stickOff) - rcControlsConfig()->mid_throttle_deadband); + stickClimbMs = constrainf(beyond / MAX(span, 1.0f), 0.0f, 1.0f) * HOVER_STICK_CLIMB_MS; + if (stickOff < 0) { + stickClimbMs = -stickClimbMs; + } + } + + const timeUs_t nowUs = micros(); + const float dT = constrainf((nowUs - lastUpdateUs) * 1e-6f, 0.0f, 0.1f); + lastUpdateUs = nowUs; + + const float climbCms = getEstimatedActualVelocity(Z); + if (!hoverLatched) { + targetAltCm = z; // follow until motion settles + if (fabsf(climbCms) < HOVER_LATCH_CLIMB_CMS) { + hoverLatched = true; + } + } + if (stickClimbMs != 0.0f) { + // the pilot's rate command RAMPS the altitude reference; the + // unchanged altitude loop tracks the moving target and releasing + // the stick latches wherever the ramp stopped. Clamping the + // reference to the reachable neighbourhood prevents windup when + // the aircraft cannot follow (throttle floor, saturation). + targetAltCm += stickClimbMs * 100.0f * dT; + targetAltCm = constrainf(targetAltCm, z - 500.0f, z + 500.0f); + } + + const float zErrM = (targetAltCm - z) / 100.0f; + const float climbMs = climbCms / 100.0f; + + // throttle floor: never cut the throttle below the motor idle, which keeps + // the prop wash (and with it the control authority) alive -- excess lift, + // e.g. in an updraft, is accepted as a climb instead + const int16_t floorThrottle = getThrottleIdleValue(); + + // gains derived from the learned hover point (see the constants above): + // us-per-motion = loop constant x (hover span per 1 g). The anchor is + // the slow-filtered APPLIED throttle - the thrust that actually holds + // the aircraft - not the engage seed + const float spanUsPerG = usPerG(hoverThrAnchorUs); + // throttle_rule (cap-only): the I-term may never wind above the pilot's + // stick - the stick is the power ceiling AND the anti-windup bound + iTermUs = constrainf(iTermUs + HOVER_THR_TRIM_S2 * spanUsPerG / GRAVITY_MSS * zErrM * dT, + floorThrottle, pilotThrottle); + + // thrust supports the weight with its vertical component only: + // compensate the tilt away from the zenith (capped, the elevation + // gate keeps this bounded anyway) + const float vertical = constrainf(sin_approx(DEGREES_TO_RADIANS(elevDeg)), 0.5f, 1.0f); + const float correction = (HOVER_THR_STIFFNESS_S2 * spanUsPerG / GRAVITY_MSS * zErrM + - HOVER_THR_DAMPING_S * spanUsPerG / GRAVITY_MSS * climbMs) / vertical; + + // throttle_rule (cap-only): the hover PID owns the altitude BELOW the + // pilot's stick - the stick must sit above the hover point, the loop + // trims down from it (stick low = commanded sink, stick up = climb + // command AND the headroom for it). Recovery floors keep their own + // raise path above (the two contract exceptions). + const int16_t outUs = constrain(lrintf(iTermUs + correction), floorThrottle, pilotThrottle); + hoverThrAnchorUs += (outUs - hoverThrAnchorUs) * MIN(dT / HOVER_THR_ANCHOR_TAU_S, 1.0f); + return outUs; +} + +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h new file mode 100644 index 00000000000..6e29abeff75 --- /dev/null +++ b/src/main/flight/hover_throttle.h @@ -0,0 +1,62 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include + +#include "config/parameter_group.h" + +// Hover throttle: while PROP HANG is held (nose near vertical) the thrust +// carries the weight and owns the altitude axis. A dedicated throttle PID +// holds the altitude captured at engage; its I-term is seeded from the +// pilot's throttle (learning the hover throttle online) and the output is +// tilt compensated. Moving the throttle stick out of the mid deadband +// hands control back to the pilot and re-captures the target. + +// The altitude/vz loop gains are NOT settings: throttle-us per unit of +// motion all share one airframe fact - the throttle-to-thrust slope, which +// at the hover point is (hover throttle - idle) per 1 g. The controller +// learns the hover point online (I-term seeded from the pilot's stick), so +// every gain derives from it at runtime with fixed dimensionless loop +// constants; no pilot can pick "microseconds per meter per second" better +// than that identity does. +typedef struct hoverThrottleConfig_s { + uint8_t hoverBaroWeight; // baro position weight (x100) while the hover + // throttle owns the altitude: hovering thrust + // pollutes the accelerometer Z, the baro + // deserves more trust than in forward flight. + // Applied as a floor over inav_w_z_baro_p; + // 0 keeps the global weight. +} hoverThrottleConfig_t; + +PG_DECLARE(hoverThrottleConfig_t, hoverThrottleConfig); + +// Called from the mixer throttle path; returns the pilot throttle when the +// hover throttle is not active, the controller output otherwise. +int16_t hoverThrottleApply(int16_t pilotThrottle); + +// True while the hover throttle controller owns the altitude (used by the +// position estimator to raise the baro weight in the hover regime) +bool hoverThrottleIsEngaged(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e5d2e3be75f..e93c97dd751 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -121,8 +121,10 @@ FASTRAM bool imuUpdated = false; static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF); static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_slope_multipiler); +static void imuUpdateGpsAidingTiltWeight(float dT); +static float gpsAidingTiltWeight = 1.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 4); PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .dcm_kp_acc = SETTING_AHRS_DCM_KP_DEFAULT, // 0.20 * 10000 @@ -400,6 +402,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe if (magBF && vectorNormSquared(magBF) > 0.01f) { wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); + // MAG TILT GATE (flight contract): beyond the aerobatic tilt the + // flat-projection heading is unusable - the body-frame field + // lines are rotated (knife edge is 90 deg either side, inverted + // is 180) and the real earth field dips 60+ deg, so every small + // tilt-estimate error leaks tan(inclination)-amplified into the + // heading (measured in SITL: est-vs-truth walked 30..168 deg in + // an inverted hold, while the gyro coasts it at ~0 deg/s). Same + // internal gate as the GPS aiding; feature-gated - the weight is + // pinned 1.0 without FEATURE_FW_AEROBATICS. + wMag *= gpsAidingTiltWeight; fpVector3_t vMag; // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). @@ -427,6 +439,61 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth); + // Antipode handling, gated behind FW_AEROBATICS: the core + // Mahony mag correction above stays byte-identical for + // every other user. The failure mode is 3D-only - a + // fixed wing only parks the heading estimate on the + // antipode after a sustained sub-cruise flat spin, never + // in normal flight - so the extra pull belongs to the + // aerobatics feature, not the shared estimator. + if (feature(FEATURE_FW_AEROBATICS) && gpsAidingTiltWeight >= 0.99f) { + // (Gated on the tilt weight too: the escape and the hard + // re-seed exist for the POST-SPIN case - a flat attitude, + // gate open. At knife/inverted the mag heading itself is + // unusable (see the tilt gate above) and a re-seed there + // would snap the estimate onto projection garbage.) + // Antipode escape: the cross-product torque scales with + // sin(error) and VANISHES as the heading error approaches + // 180 deg even though the error is maximal - after a flat + // spin the estimate can park there (measured: 184 deg off, + // stable for 60+ s, GPS-COG equally blind since it uses + // the same idiom). Past 90 deg (dot < 0) rescale the error + // to full pull so the estimate walks off the saddle; below + // 90 deg the natural sin scaling is untouched. + const float magNorthDot = vectorDotProduct(&vMag, &vCorrectedMagNorth); + if (magNorthDot < 0.0f && vectorNormSquared(&vMagErr) > 1.0e-6f) { + vectorNormalize(&vMagErr, &vMagErr); + } + + // HARD RE-SEED (Daniel's go): if the heading error stays + // beyond 90 deg for a full second while the mag is clean, + // the gentle kp pull is losing (a circling aircraft turns + // faster than kp 0.2 corrects - measured 30..135 deg of + // wandering error through a whole loiter). Rotate the + // estimate about earth Z so mag north snaps into place - + // the same philosophy as the existing GPS yaw reset for + // multirotors, driven by the mag instead. + static float magAntipodeTimeS = 0.0f; + if (magNorthDot < 0.0f) { + magAntipodeTimeS += dt; + if (magAntipodeTimeS > 1.0f) { + const float yawErrRad = atan2_approx( + vMag.x * vCorrectedMagNorth.y - vMag.y * vCorrectedMagNorth.x, + magNorthDot); + fpAxisAngle_t seed = { .axis = { .v = { 0.0f, 0.0f, 1.0f } }, + .angle = yawErrRad }; + fpQuaternion_t qSeed; + axisAngleToQuaternion(&qSeed, &seed); + quaternionMultiply(&orientation, &qSeed, &orientation); + quaternionNormalize(&orientation, &orientation); + vMagErr.x = vMagErr.y = vMagErr.z = 0.0f; + magAntipodeTimeS = 0.0f; + } + } else { + magAntipodeTimeS = 0.0f; + } + } + // Rotate error back into body frame quaternionRotateVector(&vMagErr, &vMagErr, &orientation); } @@ -459,6 +526,13 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero wCoG = 0.0f; } + if (STATE(AIRPLANE)) { + // attitude gate: yaw-from-course is meaningless with the + // nose far from the horizon (course != heading in a hang + // or knife edge; the pull-up entry corrupts yaw exactly + // when the figure begins) + wCoG *= gpsAidingTiltWeight; + } if (STATE(MULTIROTOR)) { //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); @@ -862,6 +936,22 @@ static void imuCalculateEstimatedAttitude(float dT) if (STATE(AIRPLANE)) { imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); } + // NOTE (aerobatic estimator contract, layer 3 "acc-cut on GPS loss"): + // a tightened rate-ignore for the GPS-less case was implemented and + // A/B-measured here - REDUNDANT: the nearness bellCurve plus the + // rate-ignore already cut the effective acc weight to a mean of 0.15 + // (min 0) through a GPS-less inverted spin, attitude divergence + // identical with and without the extra cut. The intent of the layer + // is in-tree; no additional code. + + // attitude gate (see imuUpdateGpsAidingTiltWeight): beyond the tilt + // limit the centrifugal models are wrong - fade them out entirely, + // the raw accelerometer is the lesser error there + imuUpdateGpsAidingTiltWeight(dT); + if (STATE(AIRPLANE) && gpsAidingTiltWeight < 1.0f) { + vectorScale(&vEstcentrifugalAccelBF_velned, &vEstcentrifugalAccelBF_velned, gpsAidingTiltWeight); + vectorScale(&vEstcentrifugalAccelBF_turnrate, &vEstcentrifugalAccelBF_turnrate, gpsAidingTiltWeight); + } if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { //pick the best centrifugal acceleration between velned and turnrate @@ -899,6 +989,14 @@ static void imuCalculateEstimatedAttitude(float dT) float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compensatedGravityBF); accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler); const bool useAcc = (accWeight > 0.001f); +#if defined(SITL_BUILD) + // bench instrumentation (SITL debug slot 6): the EFFECTIVE acc weight, + // 1000 = full trust - answers whether the estimator still listens to + // the (centrifugally poisoned) accelerometer during sustained spins. + // SITL only: a raw debug[] write on a real target would clobber the + // user's selected debug channel (review finding). + debug[6] = lrintf(accWeight * 1000.0f); +#endif const float magWeight = imuGetPGainScaleFactor() * 1.0f; fpVector3_t measuredMagBF = {.v = {mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}}; @@ -968,6 +1066,36 @@ float calculateCosTiltAngle(void) { return 1.0f - 2.0f * sq(orientation.q1) - 2.0f * sq(orientation.q2); } + +// ATTITUDE GATE for every GPS-derived aiding on an airplane: yaw-from- +// course and the centrifugal compensation both assume coordinated forward +// flight (heading follows course, lateral acceleration is v x omega). +// Beyond the tilt threshold - hang, knife edge, inverted, spins - the +// assumption is broken and the aiding actively BENDS the attitude +// (measured in SITL with truth GPS: +4.6 deg pitch bias and 19 deg tilt +// divergence in a prop hang that is clean without GPS). Drop instantly on +// entering the aerobatic domain, fade back over 2 s after returning; the +// normal flight regime keeps full GPS support. +// Tilt beyond which GPS aiding is fully faded out [deg from level]. A fixed +// property of the coordinated-flight assumption, not a pilot tuning knob; +// on/off is governed by the FW_AEROBATICS feature bit, not by this value. +#define GPS_AIDING_MAX_TILT_DEG 60 + +static void imuUpdateGpsAidingTiltWeight(float dT) +{ + // the gate exists for the aerobatic envelope; without the feature + // the estimator behaves exactly like upstream (weight pinned at 1) + if (!feature(FEATURE_FW_AEROBATICS) || !STATE(AIRPLANE)) { + gpsAidingTiltWeight = 1.0f; + return; + } + const float cosLimit = cos_approx(DEGREES_TO_RADIANS(GPS_AIDING_MAX_TILT_DEG)); + if (calculateCosTiltAngle() < cosLimit) { + gpsAidingTiltWeight = 0.0f; + } else { + gpsAidingTiltWeight = MIN(1.0f, gpsAidingTiltWeight + dT / 2.0f); + } +} #if defined(USE_GPS) bool isYawZeroResetAllowed(void) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..09beeb2643f 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -46,6 +46,12 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/altitude_floor.h" +#include "flight/rotor_guard.h" +#include "flight/crash_detection.h" +#include "flight/hover_throttle.h" +#include "flight/orientation_hold.h" +#include "flight/soaring.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -588,6 +594,23 @@ void FAST_CODE mixTable(void) #endif } else { mixerThrottleCommand = rcCommand[THROTTLE]; +#ifdef USE_SOARING + // while circling a thermal the motor is idled so the glider soars on + // the lift (a throttle-to-idle override, not a motor stop). Applied + // before the recovery paths below so an altitude-floor / rotor-guard + // climb can still override the idle; normal throttle also returns on + // thermal exit or below soar_alt_min (see soaring.c) + mixerThrottleCommand = soaringThrottleApply(mixerThrottleCommand); +#endif +#ifdef USE_FW_AEROBATICS + // hover throttle owns the altitude axis while PROP HANG is held + mixerThrottleCommand = hoverThrottleApply(mixerThrottleCommand); + // the load governor bleeds throttle while a governed figure or spin + // exceeds the load budget (see orientation_hold.c) + mixerThrottleCommand = orientationHoldLoadGovernorThrottle(mixerThrottleCommand); +#endif + // (a detected crash stops the motor via getMotorStatus() above, the + // only path that also holds a multirotor's PID-mixed motors down) throttleRangeMin = throttleIdleValue; throttleRangeMax = getMaxThrottle(); @@ -670,6 +693,18 @@ uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop) motorStatus_e getMotorStatus(void) { +#ifdef USE_CRASH_DETECTION + // After a detected crash the motor stays cut until the pilot re-allows + // it. Stopping via the motor status (not just the throttle command) is + // what actually holds a MULTIROTOR still: the throttle command is added + // to the per-motor PID mix, so lowering it alone would let the attitude + // loops keep spinning motors on a crashed copter - the stopped status + // forces every motor to idle directly. + if (crashDetectionMotorCut()) { + return MOTOR_STOPPED_USER; + } +#endif + if (STATE(NAV_MOTOR_STOP_OR_IDLE)) { return MOTOR_STOPPED_AUTO; } @@ -677,6 +712,14 @@ motorStatus_e getMotorStatus(void) const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) { +#ifdef USE_FW_AEROBATICS + // throttle_rule (cap-only, NO exceptions - Daniel 2026-07-23): the + // recoveries no longer keep the motor alive through a chopped + // stick - the pilot's thumb is the catch's power budget, a chopped + // stick is an unpowered attitude-only catch. PILOT WARNING in the + // manual (fat print): KEEP THE THROTTLE UP during a floor or + // rotor-guard catch. (The former recovery keep-alive lived here.) +#endif if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) { // If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS // and either on a plane or on a quad with inactive airmode - stop motor diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c new file mode 100644 index 00000000000..5c7225ebcf7 --- /dev/null +++ b/src/main/flight/orientation_hold.c @@ -0,0 +1,1482 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#ifdef USE_FW_AEROBATICS + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/quaternion.h" +#include "common/utils.h" +#include "common/vector.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/config.h" +#include "fc/control_profile.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "drivers/time.h" + +#include "flight/altitude_floor.h" +#include "flight/rotor_guard.h" +#include "flight/figure_sequencer.h" + +#include "navigation/navigation.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/orientation_hold.h" +#include "flight/pid.h" + +#include "flight/servos.h" + +#include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/pitotmeter.h" +#include "sensors/sensors.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 4); + +PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, + // The per-regime pitch trims and the knife speed FF are feed-forwards the + // integrating knife/inverted throttle assist regulates away (it drives + // vz -> 0), so they are not CLI settings - they seed at neutral (0). + .invertedPitchTrim = 0, + .knifeLeftPitchTrim = 0, + .knifeRightPitchTrim = 0, + .knifeSpeedFF = 0, + // The regime angle-gain scales are maintained by the limit-cycle learner + // (written at regime exit, saved on disarm) and read back on the + // DEBUG_FW_AEROBATICS blackbox channel - not hand-set. 100 % = the + // reference gains, the seed the learner starts from. + .hoverGainLearned = 100, + .invertedGainLearned = 100, + .knifeGainLearned = 100, + .figureGainLearned = 100, + // CLI-tunable per airframe / pilot. + .entryRateDps = SETTING_OHOLD_ENTRY_RATE_DEFAULT, + .stickAngleMaxDeg = SETTING_OHOLD_STICK_ANGLE_DEFAULT, + .stickReturnRateDps = SETTING_OHOLD_STICK_RETURN_RATE_DEFAULT, + .turnRollLimitDeg = SETTING_OHOLD_TURN_ROLL_LIMIT_DEFAULT, + .turnRollReturnMs = SETTING_OHOLD_TURN_ROLL_RETURN_DEFAULT, + .loadLimitG = SETTING_OHOLD_LOAD_LIMIT_DEFAULT, +); + +typedef struct { + boxId_e box; + float rollDeg; + float pitchDeg; +} orientationHoldPreset_t; + +// First matching box wins. Targets are tilt only (yaw = 0); heading is +// always left free by the twist removal in the error computation. +static const orientationHoldPreset_t orientationHoldPresets[] = { + { BOXINVERTED, 180.0f, 0.0f }, + { BOXKNIFELEFT, -90.0f, 0.0f }, + { BOXKNIFERIGHT, 90.0f, 0.0f }, + { BOXPROPHANG, 0.0f, 90.0f }, +}; + +// FLAT SPIN is a spin BEHAVIOR, not one fixed attitude: the tilt is +// regulated onto the held target while the pilot's rudder commands a +// rotation about the EARTH VERTICAL - the axis the reduced attitude error +// leaves free by construction, in every attitude. The attitude selector +// picks the held target (none = flat): INVERTED = inverted flat spin, +// KNIFE = knife edge spin, PROP HANG = torque roll. Releasing the rudder +// stops the rotation with the attitude still held; releasing the box +// recovers normally. No altitude assist: a spin descends by design (the +// altitude floor still preempts globally). +static const orientationHoldPreset_t orientationHoldSpinPresets[] = { + { BOXFSPIN, 180.0f, 0.0f }, // + INVERTED + { BOXFSPIN, -90.0f, 0.0f }, // + KNIFE LEFT + { BOXFSPIN, 90.0f, 0.0f }, // + KNIFE RIGHT + { BOXFSPIN, 0.0f, 0.0f }, // alone: flat spin + // P-HANG cannot be combined (flight contract): the torque roll is the + // hang's own aileron function, the spin mode has no job there +}; + +static const orientationHoldPreset_t * orientationHoldActivePreset(void) +{ + // A knife edge is physically held on the yaw effector (rudder or TVC + // yaw vane). On a mixer without one (flying wing) the knife boxes are + // not offered (fc_msp_box), and a stale configuration that still maps + // them is ignored here - the laws of aerodynamics outrank the switch. + const bool knifePossible = servoMixerHasYawControl(); + if (IS_RC_MODE_ACTIVE(BOXFSPIN)) { + if (IS_RC_MODE_ACTIVE(BOXINVERTED)) return &orientationHoldSpinPresets[0]; + if (IS_RC_MODE_ACTIVE(BOXKNIFELEFT) && knifePossible) return &orientationHoldSpinPresets[1]; + if (IS_RC_MODE_ACTIVE(BOXKNIFERIGHT) && knifePossible) return &orientationHoldSpinPresets[2]; + // + P-HANG is not a combination (contract) - FSPIN with the hang box + // still selected flies the plain flat spin + return &orientationHoldSpinPresets[3]; + } + for (unsigned i = 0; i < ARRAYLEN(orientationHoldPresets); i++) { + if ((orientationHoldPresets[i].box == BOXKNIFELEFT + || orientationHoldPresets[i].box == BOXKNIFERIGHT) && !knifePossible) { + continue; + } + if (IS_RC_MODE_ACTIVE(orientationHoldPresets[i].box)) { + return &orientationHoldPresets[i]; + } + } + return NULL; +} + +// Rate-loop I-term reset on target-source switches (e.g. prop hang -> +// knife edge): the accumulated I trims the OLD attitude's holding load +// (propwash vs knife rudder load) and would discharge as a disturbance +// into the new attitude. Within a figure (continuous trajectory) the +// source stays the same and the I-term is kept. +#define OHOLD_SOURCE_NONE (-1) +#define OHOLD_SOURCE_FLOOR (-2) +#define OHOLD_SOURCE_FIGURE (-3) +#define OHOLD_SOURCE_LOCK (-4) +#define OHOLD_SOURCE_EXIT (-5) +#define OHOLD_SOURCE_ROTOR (-6) + +// FLOOR CATCH LATCH (Daniel's contract): the pilot sets the floor high +// enough; if it has to CATCH while a hold/figure box is active (switch +// forgotten), that box is latched OUT - the figure must not restart on +// recovery release and dive into the floor again in a loop. The latch +// clears only when the pilot moves the switch away from that mode; +// re-selecting it afterwards starts fresh. +static int floorLatchedSource = OHOLD_SOURCE_NONE; + +static bool orientationHoldFloorLatchBlocks(int source) +{ + return floorLatchedSource != OHOLD_SOURCE_NONE && source == floorLatchedSource; +} + +static void orientationHoldFloorLatchTick(void) +{ + if (floorLatchedSource == OHOLD_SOURCE_NONE) { + return; + } + bool stillSelected; + switch (floorLatchedSource) { + case OHOLD_SOURCE_FIGURE: + stillSelected = figureSequencerRequested(); + break; + case OHOLD_SOURCE_LOCK: + stillSelected = IS_RC_MODE_ACTIVE(BOXATTLOCK); + break; + default: { + const orientationHoldPreset_t *p = orientationHoldActivePreset(); + stillSelected = (p != NULL && p->box == floorLatchedSource); + break; + } + } + if (!stillSelected) { + floorLatchedSource = OHOLD_SOURCE_NONE; + } +} + +bool orientationHoldIsRequested(void) +{ + // a floor-latched box does not count as a request: the mode falls + // back to ANGLE until the pilot switches away (see floorLatchedSource) + orientationHoldFloorLatchTick(); + if (figureSequencerRequested() && !orientationHoldFloorLatchBlocks(OHOLD_SOURCE_FIGURE)) { + return true; + } + const orientationHoldPreset_t *preset = orientationHoldActivePreset(); + if (preset != NULL && !orientationHoldFloorLatchBlocks(preset->box)) { + return true; + } + if (IS_RC_MODE_ACTIVE(BOXATTLOCK) && !orientationHoldFloorLatchBlocks(OHOLD_SOURCE_LOCK)) { + return true; + } + return false; +} + +static bool orientationHoldSticksDeflected(void) +{ + return ABS(rcCommand[ROLL]) > rcControlsConfig()->deadband + || ABS(rcCommand[PITCH]) > rcControlsConfig()->deadband + || ABS(rcCommand[YAW]) > rcControlsConfig()->yaw_deadband; +} + +// Deadbanded stick position normalized to -1..1 +static float orientationHoldStickNorm(int16_t rc, uint8_t deadband) +{ + if (ABS(rc) <= deadband) { + return 0.0f; + } + const float span = 500.0f - deadband; + return (rc > 0 ? (rc - deadband) : (rc + deadband)) / span; +} + +// Same Euler to quaternion convention as imuComputeQuaternionFromRPY (yaw = 0) +void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float pitchDeg) +{ + const float cosRoll = cos_approx(DEGREES_TO_RADIANS(rollDeg) * 0.5f); + const float sinRoll = sin_approx(DEGREES_TO_RADIANS(rollDeg) * 0.5f); + const float cosPitch = cos_approx(DEGREES_TO_RADIANS(pitchDeg) * 0.5f); + const float sinPitch = sin_approx(DEGREES_TO_RADIANS(pitchDeg) * 0.5f); + + qTarget->q0 = cosRoll * cosPitch; + qTarget->q1 = sinRoll * cosPitch; + qTarget->q2 = cosRoll * sinPitch; + qTarget->q3 = -sinRoll * sinPitch; +} + +// Earth vertical (up) expressed in the body frame, normalized. Works for a +// slightly denormalized quaternion as well. +static void earthUpInBodyFrame(fpVector3_t *up, const fpQuaternion_t *q) +{ + fpVector3_t v = { .v = { 0.0f, 0.0f, 1.0f } }; + quaternionRotateVector(&v, &v, q); + const float norm = fast_fsqrtf(sq(v.x) + sq(v.y) + sq(v.z)); + up->x = v.x / norm; + up->y = v.y / norm; + up->z = v.z / norm; +} + +void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget) +{ + // Reduced attitude control: regulate the direction of the earth vertical + // in the body frame instead of the full rotation. The rotation about the + // vertical axis (heading in normal/inverted flight, body roll at prop + // hang) is free by construction. Unlike a swing-twist decomposition + // about earth Z this has no degenerate region near inverted, where + // w^2 + z^2 vanishes for every heading and the extracted twist direction + // is noise driven. Large errors do not occur in operation: the target + // is a persistent state slewed toward the requested attitude, never + // stepped (the entry path is chosen by the slew, see + // orientationHoldSlewTarget), so no axis preference is needed here. + fpVector3_t upEst, upTarget; + earthUpInBodyFrame(&upEst, qEst); + earthUpInBodyFrame(&upTarget, qTarget); + + // Shortest rotation taking upEst to upTarget; operand order gives the + // pidLevel() sign convention (error = target - attitude), pinned by the + // host convention tests + fpVector3_t cross = { .v = { + upTarget.y * upEst.z - upTarget.z * upEst.y, + upTarget.z * upEst.x - upTarget.x * upEst.z, + upTarget.x * upEst.y - upTarget.y * upEst.x, + }}; + const float crossNorm = fast_fsqrtf(sq(cross.x) + sq(cross.y) + sq(cross.z)); + const float dot = upEst.x * upTarget.x + upEst.y * upTarget.y + upEst.z * upTarget.z; + const float angle = atan2_approx(crossNorm, dot); + + fpVector3_t axis; + if (crossNorm > 1e-6f) { + axis.x = cross.x / crossNorm; + axis.y = cross.y / crossNorm; + axis.z = cross.z / crossNorm; + } else if (dot < 0.0f) { + // Exactly 180 deg of tilt error: rotation axis is ambiguous, pick a + // deterministic body axis orthogonal to the target up direction + // (the one least aligned with it) + fpVector3_t seed = { .v = { 0.0f, 0.0f, 0.0f } }; + if (fabsf(upTarget.x) <= fabsf(upTarget.y) && fabsf(upTarget.x) <= fabsf(upTarget.z)) { + seed.x = 1.0f; + } else if (fabsf(upTarget.y) <= fabsf(upTarget.z)) { + seed.y = 1.0f; + } else { + seed.z = 1.0f; + } + axis.x = upTarget.y * seed.z - upTarget.z * seed.y; + axis.y = upTarget.z * seed.x - upTarget.x * seed.z; + axis.z = upTarget.x * seed.y - upTarget.y * seed.x; + const float norm = fast_fsqrtf(sq(axis.x) + sq(axis.y) + sq(axis.z)); + axis.x /= norm; + axis.y /= norm; + axis.z /= norm; + } else { + errDeg->x = errDeg->y = errDeg->z = 0.0f; + return; + } + + // Sign matches pidLevel(): error = target - attitude + errDeg->x = RADIANS_TO_DEGREES(axis.x * angle); + errDeg->y = RADIANS_TO_DEGREES(axis.y * angle); + errDeg->z = RADIANS_TO_DEGREES(axis.z * angle); +} + +// Rotation vector (body frame, deg) to quaternion. Same sign convention as +// quaternionToAxisAngle / orientationHoldTargetFromRP: positive x = positive +// roll. Do NOT use axisAngleToQuaternion here, it negates the axis. +static void quatFromRotVecDeg(fpQuaternion_t *q, const fpVector3_t *rotVecDeg) +{ + const float angleDeg = fast_fsqrtf(sq(rotVecDeg->x) + sq(rotVecDeg->y) + sq(rotVecDeg->z)); + if (angleDeg < 1e-4f) { + quaternionInitUnit(q); + return; + } + const float halfRad = DEGREES_TO_RADIANS(angleDeg) * 0.5f; + const float s = sin_approx(halfRad) / angleDeg; + q->q0 = cos_approx(halfRad); + q->q1 = rotVecDeg->x * s; + q->q2 = rotVecDeg->y * s; + q->q3 = rotVecDeg->z * s; +} + +// The persistent target attitude q_soll: seeded from the estimated attitude +// when a target source engages, then SLEWED toward the source's requested +// attitude instead of stepping there. The regulator error therefore stays +// small at all times; the entry path is an explicit target trajectory. +static fpQuaternion_t qSollState; + +// Rotate the tilt of qSoll toward qDesired by at most maxStepDeg. +// Returns the tilt angle (deg) still remaining AFTER the step. +// +// Toward the antipode (engaging inverted from level) the shortest-rotation +// cross product barely rises above noise, so the entry path would be an +// arbitrary mix of roll and yaw (seen as a heading swing while rolling in). +// Do what a pilot does and roll about body X: blend the slew axis +// CONTINUOUSLY from the cross product (tilt <= 120 deg) to body X projected +// orthogonal to the desired up (>= 150 deg). The ramp avoids chattering at +// a hard threshold. Because this shapes the TARGET trajectory, the +// regulator error itself needs no axis preference. +static float orientationHoldSlewTarget(fpQuaternion_t *qSoll, const fpQuaternion_t *qDesired, float maxStepDeg) +{ + fpVector3_t upSoll, upDes; + earthUpInBodyFrame(&upSoll, qSoll); + earthUpInBodyFrame(&upDes, qDesired); + + fpVector3_t cross = { .v = { + upDes.y * upSoll.z - upDes.z * upSoll.y, + upDes.z * upSoll.x - upDes.x * upSoll.z, + upDes.x * upSoll.y - upDes.y * upSoll.x, + }}; + const float crossNorm = fast_fsqrtf(sq(cross.x) + sq(cross.y) + sq(cross.z)); + const float dot = upSoll.x * upDes.x + upSoll.y * upDes.y + upSoll.z * upDes.z; + const float angleDeg = RADIANS_TO_DEGREES(atan2_approx(crossNorm, dot)); + + fpVector3_t axis; + bool axisValid = false; + const float wPref = constrainf((-dot - 0.5f) / 0.37f, 0.0f, 1.0f); + if (wPref > 0.0f) { + fpVector3_t pref = { .v = { + 1.0f - upDes.x * upDes.x, + -upDes.x * upDes.y, + -upDes.x * upDes.z, + }}; + const float prefNorm = fast_fsqrtf(sq(pref.x) + sq(pref.y) + sq(pref.z)); + if (prefNorm > 1e-3f) { + float s = 1.0f / prefNorm; + if (crossNorm > 1e-3f + && (pref.x * cross.x + pref.y * cross.y + pref.z * cross.z) < 0.0f) { + s = -s; // keep the roll direction the cross product started + } + const float wCross = (crossNorm > 1e-6f) ? (1.0f - wPref) / crossNorm : 0.0f; + axis.x = wPref * s * pref.x + wCross * cross.x; + axis.y = wPref * s * pref.y + wCross * cross.y; + axis.z = wPref * s * pref.z + wCross * cross.z; + const float n = fast_fsqrtf(sq(axis.x) + sq(axis.y) + sq(axis.z)); + if (n > 1e-6f) { + axis.x /= n; axis.y /= n; axis.z /= n; + axisValid = true; + } + } + // body X parallel to the desired up (prop hang entry from a dive): + // fall through to the shortest rotation below + } + if (!axisValid) { + if (crossNorm > 1e-6f) { + axis.x = cross.x / crossNorm; + axis.y = cross.y / crossNorm; + axis.z = cross.z / crossNorm; + } else { + // aligned (nothing to do) or exact antipode with body X vertical: + // leave the target where it is, the next cycle disambiguates + return (dot < 0.0f) ? angleDeg : 0.0f; + } + } + + const float stepDeg = MIN(angleDeg, maxStepDeg); + if (stepDeg > 1e-3f) { + const fpVector3_t stepVec = { .v = { axis.x * stepDeg, axis.y * stepDeg, axis.z * stepDeg } }; + fpQuaternion_t qStep; + quatFromRotVecDeg(&qStep, &stepVec); + quaternionMultiply(qSoll, qSoll, &qStep); + quaternionNormalize(qSoll, qSoll); + } + return angleDeg - stepDeg; +} + +static void orientationHoldComputeFullAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget); + +// Full-attitude slew (rate-limited slerp) for the figure line-hold: the +// reduced slew above works on the up vectors only and is heading-free by +// design - correct for pilot holds, but it can never close the heading gap +// to a yaw-anchored figure target (the slewed target simply inherits the +// drifted heading). Anchored figures slew the FULL rotation instead. No +// antipode axis preference needed: figures start on the current attitude +// and the trajectory is carried forward from there, the relative angle +// stays small. +static float orientationHoldSlewTargetFull(fpQuaternion_t *qSoll, const fpQuaternion_t *qDesired, float maxStepDeg) +{ + fpVector3_t errDeg; + orientationHoldComputeFullAttitudeError(&errDeg, qSoll, qDesired); + const float angleDeg = fast_fsqrtf(sq(errDeg.x) + sq(errDeg.y) + sq(errDeg.z)); + const float stepDeg = MIN(angleDeg, maxStepDeg); + if (stepDeg > 1e-3f) { + const float s = stepDeg / angleDeg; + const fpVector3_t stepVec = { .v = { errDeg.x * s, errDeg.y * s, errDeg.z * s } }; + fpQuaternion_t qStep; + quatFromRotVecDeg(&qStep, &stepVec); + quaternionMultiply(qSoll, qSoll, &qStep); + quaternionNormalize(qSoll, qSoll); + } + return angleDeg - stepDeg; +} + +// Error leash (ArduPlane qacro pattern): the target never runs further +// ahead of the attitude than the rate loop can catch up within this time. +// Clamping the error BEFORE the re-anchor below pulls the target back by +// the excess -- anti-windup at the target level. It only binds when the +// aircraft cannot follow (saturation, stall, blocked surfaces): in normal +// operation the slewed target keeps the error far smaller. +#define OHOLD_LEASH_TIME_S 0.2f + +// Regulator core: tilt error between the estimated attitude and q_soll, then +// re-anchor q_soll on the attitude composed with that error. The twist (the +// free axis: heading in level/inverted flight, body roll at prop hang) of +// the target thereby follows the actual attitude every cycle -- axis +// compliance w_yaw = 0. Yaw-anchored figures (figureLineAnchored) feed the +// FULL rotation through orientationHoldComputeFullAttitudeError instead; +// for them the re-anchoring below is a mathematical no-op. +static bool figureLineAnchored = false; +static fpQuaternion_t qFigureYawAnchor; +// NO FREE AXIS (flight contract): pilot holds regulate the FULL attitude, yaw +// included. The target heading is a persistent ANCHOR: captured at engage, +// rotated by the stick component along the earth vertical (proportional turn +// rate), FROZEN hands-off. While a tilt carve or a spin runs it FOLLOWS the +// aircraft (a commanded curve is never fought); releasing holds the heading +// where the input left it. Tilt returns to the pose, yaw stays - sollage_rule. +static float pilotAnchorPsiDeg = 0.0f; +static bool pilotAnchored = false; +static bool pilotFullError = false; // set per cycle by the target sources +static float pilotLeanDeg = 0.0f; // slewed curve lean (turn_roll_limit/return) +static void orientationHoldComputeFullAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget); + +static void orientationHoldRegulate(fpVector3_t *errDeg) +{ + if (figureLineAnchored || pilotFullError) { + // full attitude error: figures on a line AND the pilot holds (no free + // axis - the yaw component holds the anchored heading) + orientationHoldComputeFullAttitudeError(errDeg, &orientation, &qSollState); + } else { + orientationHoldComputeAttitudeError(errDeg, &orientation, &qSollState); + } + + // leash: slowest axis rate bounds what the rate loop can catch up + // (the tilt error can sit on any body axis, yaw included at the hang) + uint16_t slowestRate = currentControlProfile->stabilized.rates[FD_ROLL]; + slowestRate = MIN(slowestRate, currentControlProfile->stabilized.rates[FD_PITCH]); + slowestRate = MIN(slowestRate, currentControlProfile->stabilized.rates[FD_YAW]); + const float leashDeg = slowestRate * 10.0f * OHOLD_LEASH_TIME_S; + const float errMag = fast_fsqrtf(sq(errDeg->x) + sq(errDeg->y) + sq(errDeg->z)); + if (errMag > leashDeg) { + const float s = leashDeg / errMag; + errDeg->x *= s; + errDeg->y *= s; + errDeg->z *= s; + } + + fpQuaternion_t qErr; + quatFromRotVecDeg(&qErr, errDeg); + quaternionMultiply(&qSollState, &orientation, &qErr); + quaternionNormalize(&qSollState, &qSollState); +} + +// Exit handover thresholds: engage only when the released attitude is far +// enough from level that the instant Euler error would command full rates; +// hand to ANGLE once the attitude has followed the target to the horizon +#define OHOLD_EXIT_SLEW_ENGAGE_DEG 30.0f +#define OHOLD_EXIT_SLEW_DONE_DEG 10.0f +#define OHOLD_EXIT_SLEW_TIMEOUT_S 3.0f + +static int activeTargetSource = OHOLD_SOURCE_NONE; +static bool exitSlewActive = false; +static float exitSlewTimeS = 0.0f; + +static float holdRefAltCm = 0.0f; // altitude assist reference, captured at hold entry +static bool presetSlewCaptured = false; // entry slew has reached the preset once +static void orientationHoldCheckSourceSwitch(int source) +{ + if (source != activeTargetSource) { + if (activeTargetSource != OHOLD_SOURCE_NONE || source != OHOLD_SOURCE_NONE) { + pidResetErrorAccumulators(); + } + activeTargetSource = source; + // any real hold taking over cancels a pending exit handover + if (source != OHOLD_SOURCE_EXIT) { + exitSlewActive = false; + } + figureLineAnchored = false; + // capture the altitude reference for the hold altitude assist at the + // moment the target engages (same pattern as the figure sequencer) + holdRefAltCm = getEstimatedActualPosition(Z); + // seed the persistent target on the actual attitude: the regulator + // error starts at zero and the entry happens as a target slew + qSollState = orientation; + presetSlewCaptured = false; + pilotAnchored = false; // heading anchor re-captures on the new source + pilotLeanDeg = 0.0f; + } +} + +bool orientationHoldIsPropHang(void) +{ + return activeTargetSource == BOXPROPHANG; +} + +// A hold that is deliberately TURNING needs its coordinated yaw rate fed +// forward (the heading-free error otherwise regulates the physical turn +// rate to zero). Sources: the sequencer's WAIT_POS leg, and the altitude +// floor's orbit around the breach point. +bool orientationHoldTurnCoordinationBank(float *bankDeg) +{ + if (figureSequencerGetTurnBank(bankDeg)) { + return true; + } + if (altitudeFloorRecoveryActive() && altitudeFloorOrbitActive() + && !altitudeFloorOrbitViaNav()) { + // degraded (GPS-less) constant-bank circle only - the nav loiter + // flies its own coordination + *bankDeg = altitudeFloorRecoveryRollDeg(); + return true; + } + return false; +} + +bool orientationHoldIsKnifeOrInverted(void) +{ + return activeTargetSource == BOXINVERTED + || activeTargetSource == BOXKNIFELEFT + || activeTargetSource == BOXKNIFERIGHT; +} + +bool orientationHoldIsSpinAboutVertical(void) +{ + return activeTargetSource == BOXFSPIN; +} + +void orientationHoldUpInBody(fpVector3_t *upBody) +{ + const fpVector3_t upEarth = { .v = { 0.0f, 0.0f, 1.0f } }; + quaternionRotateVector(upBody, &upEarth, &orientation); +} + +// ---- Figure line-hold ------------------------------------------------------ +// +// Pilot holds are heading-free by design (the reduced attitude error drops +// the rotation about the earth vertical), but a FIGURE flown on a line must +// not be: with the target rotating about the body axis and heading free, a +// slow roll wandered ~15 deg of course per roll in SITL - the roll axis +// follows wherever the nose drifts and nothing pulls it back. Figures +// therefore anchor their trajectory to the heading captured at figure start +// and regulate the FULL attitude error. Verified identity (bench +// math_verify G3): 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. State lives next to the source tracking above. +static void figureCaptureYawAnchor(void) +{ + // NEGATED-heading frame: INAV's attitude quaternion carries -yaw + // (imu.c) - see the pilot anchor for the measured failure of +yaw + const float halfPsiRad = -DECIDEGREES_TO_RADIANS(attitude.values.yaw) * 0.5f; + qFigureYawAnchor.q0 = cos_approx(halfPsiRad); + qFigureYawAnchor.q1 = 0.0f; + qFigureYawAnchor.q2 = 0.0f; + qFigureYawAnchor.q3 = sin_approx(halfPsiRad); +} + +// Full attitude error: q_err = conj(q_est) (x) q_target, as a rotation +// vector in the body frame (deg). Same sign convention as the reduced +// error / pidLevel; quaternionToAxisAngle wraps to the shortest path. +// Bench mirror: math_verify.py section G (checked against scipy). +static void orientationHoldComputeFullAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget) +{ + fpQuaternion_t qConj, qErr; + quaternionConjugate(&qConj, qEst); + quaternionMultiply(&qErr, &qConj, qTarget); + fpAxisAngle_t aa; + quaternionToAxisAngle(&aa, &qErr); + const float angleDeg = RADIANS_TO_DEGREES(aa.angle); + errDeg->x = aa.axis.x * angleDeg; + errDeg->y = aa.axis.y * angleDeg; + errDeg->z = aa.axis.z * angleDeg; +} + +bool orientationHoldSticksAreTargetOffsets(void) +{ + // preset sources carry the box id (positive); the special sources + // (floor/figure/lock/none) are negative + return activeTargetSource >= 0 + && orientationHoldConfig()->stickAngleMaxDeg > 0; +} + +// ---- Learned damping reserve per regime ------------------------------------ +// +// The NORMAL-FLIGHT gains are the reference; every hold regime runs on a +// single learned SCALE of them instead of its own gain set. Aerobatic +// regimes change the plant gain (prop wash instead of airflow at the hang, +// fuselage lift at the knife edge, transients at figure boundaries), so +// gains that are well damped in forward flight can limit cycle there - a +// growing oscillation with the surfaces far from saturation. Instead of +// hand tuning each regime the controller LEARNS its damping reserve: +// - detect the limit cycle per tilt axis: decisive zero crossings of the +// attitude error at 0.4..8 Hz with amplitude above a floor +// - each detected half wave backs the angle-gain scale off fast (attack) +// - quiet time recovers it slowly toward 1.0 (release) +// One scale per regime (hover / inverted / knife / figure), each persisted: +// flying the same figure repeatedly converges its scale, so the figures +// get better with every flight. The scale settles just below the stability +// boundary for the actual airframe, CG and battery state. Spins are +// excluded: their rotation is not a limit cycle to tune away. + +// Wide band on purpose: a 1.5 m aerobat limit cycles at 1-2 Hz, a 0.7 m +// model with its small inertia rather at 4-8 Hz. Noise rejection is the +// job of the amplitude gates below, not of the band. +#define HOVER_OSC_MIN_HALFWAVE_S 0.06f // 0.4..8 Hz band +#define HOVER_OSC_MAX_HALFWAVE_S 1.2f +#define HOVER_OSC_AMPLITUDE_DEG 2.0f // ignore noise-level wobble +#define HOVER_OSC_CROSS_DEG 0.5f // decisive zero crossing +#define HOVER_GAIN_ATTACK 0.85f // per detected half wave +#define HOVER_GAIN_FLOOR 0.3f +#define HOVER_GAIN_RELEASE_TAU_S 4.0f + +typedef struct { + float sign; // sign of the current half wave + float peakDeg; // amplitude seen since the last crossing + float sinceFlipS; +} hoverOscDetector_t; + +// The scales PERSIST: each freezes at regime exit (the next entry starts at +// the learned value instead of oscillating its way down again), is written +// back to the config at exit and saved to EEPROM on disarm. A value learned +// under worse conditions self-corrects upward through the release while +// flying that regime quietly. +typedef enum { + OHOLD_REGIME_NONE = -1, + OHOLD_REGIME_HOVER = 0, + OHOLD_REGIME_INVERTED, + OHOLD_REGIME_KNIFE, + OHOLD_REGIME_FIGURE, + OHOLD_REGIME_COUNT +} oholdRegime_e; + +typedef struct { + float scale; + bool wasActive; + hoverOscDetector_t osc[2]; +} regimeGainState_t; + +static regimeGainState_t regimeGain[OHOLD_REGIME_COUNT]; +static bool regimeGainInitialized = false; +static bool regimeGainDirty = false; // learned value awaiting the disarm save + +static uint8_t * regimeGainConfigField(oholdRegime_e regime) +{ + switch (regime) { + case OHOLD_REGIME_HOVER: return &orientationHoldConfigMutable()->hoverGainLearned; + case OHOLD_REGIME_INVERTED: return &orientationHoldConfigMutable()->invertedGainLearned; + case OHOLD_REGIME_KNIFE: return &orientationHoldConfigMutable()->knifeGainLearned; + default: return &orientationHoldConfigMutable()->figureGainLearned; + } +} + +static bool hoverOscDetectAxis(hoverOscDetector_t *d, float sigDeg, float dT) +{ + d->sinceFlipS += dT; + d->peakDeg = MAX(d->peakDeg, fabsf(sigDeg)); + if (d->sign == 0.0f) { + d->sign = (sigDeg >= 0.0f) ? 1.0f : -1.0f; + return false; + } + if (sigDeg * d->sign < 0.0f && fabsf(sigDeg) > HOVER_OSC_CROSS_DEG) { + const bool osc = d->sinceFlipS > HOVER_OSC_MIN_HALFWAVE_S + && d->sinceFlipS < HOVER_OSC_MAX_HALFWAVE_S + && d->peakDeg > HOVER_OSC_AMPLITUDE_DEG; + d->sign = (sigDeg > 0.0f) ? 1.0f : -1.0f; + d->peakDeg = fabsf(sigDeg); + d->sinceFlipS = 0.0f; + return osc; + } + return false; +} + +// freeze the learned value when a regime ends; write it back once so the +// disarm save picks it up. The limit-cycle detector dies with the regime: +// a frozen sinceFlipS/peakDeg would otherwise register a spurious +// half-wave on re-entry and back the learned gain off once (review +// finding). +static void regimeGainFreeze(oholdRegime_e regime) +{ + regimeGainState_t *g = ®imeGain[regime]; + if (g->wasActive) { + const uint8_t learned = lrintf(g->scale * 100.0f); + if (learned != *regimeGainConfigField(regime)) { + *regimeGainConfigField(regime) = learned; + regimeGainDirty = true; + } + g->wasActive = false; + } + g->osc[0] = g->osc[1] = (hoverOscDetector_t){ 0 }; +} + +static void regimeGainFreezeAll(void) +{ + for (int r = 0; r < OHOLD_REGIME_COUNT; r++) { + regimeGainFreeze((oholdRegime_e)r); + } +} + +// nose-elevation threshold shared by the hover regime gate and the +// authority scale (one constant, one concept - review finding: the same +// 45 existed once named and once as a bare literal) +#define AUTHORITY_HOVER_ELEVATION_DEG 45.0f + +// which learning regime the current target source belongs to; spins and +// the special sources (lock / floor / exit handover) learn nothing +static oholdRegime_e regimeGainActiveRegime(void) +{ + switch (activeTargetSource) { + case BOXPROPHANG: { + // nose elevation gate, same release threshold as the hover throttle + fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; + quaternionRotateVectorInv(&nose, &nose, &orientation); + return RADIANS_TO_DEGREES(asin_approx(constrainf(-nose.z, -1.0f, 1.0f))) > AUTHORITY_HOVER_ELEVATION_DEG + ? OHOLD_REGIME_HOVER : OHOLD_REGIME_NONE; + } + case BOXINVERTED: + return OHOLD_REGIME_INVERTED; + case BOXKNIFELEFT: + case BOXKNIFERIGHT: + return OHOLD_REGIME_KNIFE; + case OHOLD_SOURCE_FIGURE: + return OHOLD_REGIME_FIGURE; + default: + return OHOLD_REGIME_NONE; + } +} + +static void regimeGainUpdate(const fpVector3_t *errDeg, float dT) +{ + if (!regimeGainInitialized) { + for (int r = 0; r < OHOLD_REGIME_COUNT; r++) { + regimeGain[r].scale = constrainf(*regimeGainConfigField((oholdRegime_e)r) / 100.0f, + HOVER_GAIN_FLOOR, 1.0f); + } + regimeGainInitialized = true; + } + + // blackbox readback of the learned regime gains (no longer CLI settings): + // scale [%] hover / inverted / knife / figure. A no-op unless + // debug_mode = FW_AEROBATICS. + DEBUG_SET(DEBUG_FW_AEROBATICS, 0, lrintf(regimeGain[OHOLD_REGIME_HOVER].scale * 100.0f)); + DEBUG_SET(DEBUG_FW_AEROBATICS, 1, lrintf(regimeGain[OHOLD_REGIME_INVERTED].scale * 100.0f)); + DEBUG_SET(DEBUG_FW_AEROBATICS, 2, lrintf(regimeGain[OHOLD_REGIME_KNIFE].scale * 100.0f)); + DEBUG_SET(DEBUG_FW_AEROBATICS, 3, lrintf(regimeGain[OHOLD_REGIME_FIGURE].scale * 100.0f)); + + const oholdRegime_e active = regimeGainActiveRegime(); + for (int r = 0; r < OHOLD_REGIME_COUNT; r++) { + if (r != active && regimeGain[r].wasActive) { + regimeGainFreeze((oholdRegime_e)r); // clears the osc detector too + } + } + if (active == OHOLD_REGIME_NONE) { + return; + } + regimeGainState_t *g = ®imeGain[active]; + g->wasActive = true; + + // the hang limit cycles on body pitch/yaw (prop wash axes); the other + // regimes on the tilt axes roll/pitch + const float sigA = (active == OHOLD_REGIME_HOVER) ? errDeg->y : errDeg->x; + const float sigB = (active == OHOLD_REGIME_HOVER) ? errDeg->z : errDeg->y; + bool osc = hoverOscDetectAxis(&g->osc[0], sigA, dT); + osc = hoverOscDetectAxis(&g->osc[1], sigB, dT) || osc; + + if (osc) { + g->scale = MAX(HOVER_GAIN_FLOOR, g->scale * HOVER_GAIN_ATTACK); + } else { + g->scale += (1.0f - g->scale) * MIN(dT / HOVER_GAIN_RELEASE_TAU_S, 1.0f); + } +} + +bool orientationHoldRegimeOscillating(void) +{ + // true while the ACTIVE regime's limit-cycle detector recently fired: + // the learned scale sits measurably below the reference. At the knife + // edge instability usually means "too slow, the surfaces are starving" - + // the throttle assist raises the speed on this signal (more airflow is + // the physical cure, backing the gain off only treats the symptom). + if (!regimeGainInitialized) { + return false; + } + const oholdRegime_e active = regimeGainActiveRegime(); + return active != OHOLD_REGIME_NONE + && regimeGain[active].wasActive + && regimeGain[active].scale < 0.9f; +} + +float orientationHoldLevelGainScale(void) +{ + // the learned damping reserve of the ACTIVE regime; everything else + // (normal flight, lock, spins, handover) runs the reference gains. + // The scale of the target regime applies from the moment the source + // switches, so the entry slew already flies with it. + if (!regimeGainInitialized) { + return 1.0f; + } + const oholdRegime_e active = regimeGainActiveRegime(); + return (active != OHOLD_REGIME_NONE && regimeGain[active].wasActive) + ? regimeGain[active].scale : 1.0f; +} + +void orientationHoldSyncTargetToAttitude(void) +{ + // open-loop flying (figure IMPULSE): the persistent target must not go + // stale while the regulator is bypassed -- re-seed it on the attitude so + // the catch afterwards slews from where the aircraft actually is + qSollState = orientation; +} + +void orientationHoldResetSourceTracking(void) +{ + // mode left: also reset, the attitude's holding-load trim in the I-term + // would discharge into the pilot's manual/acro flying otherwise + if (activeTargetSource != OHOLD_SOURCE_NONE) { + pidResetErrorAccumulators(); + activeTargetSource = OHOLD_SOURCE_NONE; + } + exitSlewActive = false; + // The floor latch must OUTLIVE the hold going un-requested. The latch is + // the REASON the hold is un-requested (it blocks the interrupted box), and + // fc_core calls this whenever ORIENTATION_HOLD_MODE is inactive - so + // clearing it here let the interrupted box re-engage the instant the + // recovery ended: the fly-up / fall-back loop. It clears only when the + // pilot DESELECTS the latched mode (orientationHoldFloorLatchTick) or on + // disarm - never just because the latch made the hold un-requested. + if (!ARMING_FLAG(ARMED)) { + floorLatchedSource = OHOLD_SOURCE_NONE; + } + + // leaving the mode ends every learning regime: freeze the learned + // gains (landing straight out of a hold and disarming must not lose them) + regimeGainFreezeAll(); + + // persist the learned regime gains once the aircraft is on the ground + // (never write EEPROM while armed, the flight loop would stall) + if (regimeGainDirty && !ARMING_FLAG(ARMED)) { + regimeGainDirty = false; + saveConfigAndNotify(); + } +} + +// Exit handover toward ANGLE: releasing a hold far from level must not drop +// the full attitude error onto the Euler level controller at once. A prop +// hang exit otherwise whips: ~90 deg of error at near zero airspeed commands +// the full rates, the airframe has no authority to arrest the resulting +// pitch rate at the horizon and slices through to nose down before ANGLE +// catches it. Instead the hold keeps the aircraft one more transition and +// slews its target to level; ANGLE takes over once the attitude is there. +bool orientationHoldExitSlewPending(void) +{ + if (exitSlewActive) { + return true; + } + // only a released real hold (preset / figure / lock) hands over; floor + // recovery ends level by construction and an idle mode has nothing to do + if (activeTargetSource == OHOLD_SOURCE_NONE || activeTargetSource == OHOLD_SOURCE_FLOOR + || activeTargetSource == OHOLD_SOURCE_EXIT) { + return false; + } + if (orientationHoldIsRequested()) { + return false; + } + fpQuaternion_t qLevel; + fpVector3_t tiltErr; + orientationHoldTargetFromRP(&qLevel, 0.0f, 0.0f); + orientationHoldComputeAttitudeError(&tiltErr, &orientation, &qLevel); + if (fabsf(tiltErr.x) < OHOLD_EXIT_SLEW_ENGAGE_DEG && fabsf(tiltErr.y) < OHOLD_EXIT_SLEW_ENGAGE_DEG) { + return false; + } + exitSlewActive = true; + exitSlewTimeS = 0.0f; + return true; +} + +bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) +{ + fpQuaternion_t qDesired; + // Preset entries slew the target with their own rate: same mechanism as + // a figure segment, but a snappy entry and a deliberate slow roll figure + // are different intents with different rates + float slewRateDegS = orientationHoldConfig()->entryRateDps; + // full-attitude regulation is opted into by the pilot-hold / lock branches + pilotFullError = false; + + // Altitude floor recovery overrides any selected preset: upright + climb. + // Safety recovery tracks the requested attitude directly, no entry slew. + if (altitudeFloorRecoveryActive()) { + // the catch LATCHES the hold/figure box it interrupted (pilot + // forgot the switch): without this the figure re-engages on + // recovery release and dives straight back into the floor + if (activeTargetSource != OHOLD_SOURCE_NONE + && activeTargetSource != OHOLD_SOURCE_FLOOR + && activeTargetSource != OHOLD_SOURCE_ROTOR + && activeTargetSource != OHOLD_SOURCE_EXIT) { + floorLatchedSource = activeTargetSource; + } + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FLOOR); + // climb: wings level + climb pitch; orbit: gentle bank circling + // at the floor while the pilot collects themselves + orientationHoldTargetFromRP(&qDesired, altitudeFloorRecoveryRollDeg(), + altitudeFloorRecoveryPitchDeg()); + slewRateDegS = 0.0f; + } else if (rotorGuardRecoveryActive()) { + // Autogyro tip-over catch: wings level, nose slightly DOWN - the + // disk needs inflow before the lateral tilt has any authority + // again; the throttle floor (hover_throttle) provides the thrust. + // The floor outranks this: height beats rotor rpm. + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_ROTOR); + orientationHoldTargetFromRP(&qDesired, 0.0f, rotorGuardRecoveryPitchDeg()); + slewRateDegS = 0.0f; + } else if (figureSequencerRequested() + && !orientationHoldFloorLatchBlocks(OHOLD_SOURCE_FIGURE)) { + float figRoll, figPitch; + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE); + figureSequencerGetTarget(&figRoll, &figPitch); + orientationHoldTargetFromRP(&qDesired, figRoll, figPitch); + // line-hold: fly the figure about the heading captured at figure + // start instead of wherever the nose currently points. Segments + // that change the heading on purpose (WAIT_POS banks toward home) + // or fly open loop (impulse, spin) release the anchor; it + // re-captures on the CURRENT heading when they complete. + if (figureSequencerHeadingAnchored()) { + if (!figureLineAnchored) { + figureCaptureYawAnchor(); + figureLineAnchored = true; + } + quaternionMultiply(&qDesired, &qFigureYawAnchor, &qDesired); + } else { + figureLineAnchored = false; + } + // the trajectory is already rate shaped; the slew only smooths the + // engage and absolute HOLD segment steps. The slew must OUTRUN the + // trajectory: at exactly the figure rate it chases the rotating + // target saturated, the entire budget goes into the figure and the + // line-hold's heading correction never closes - the slewed target + // absorbs the heading drift instead of holding the line (seen as + // 12 deg of FC-frame course walk during one slow roll) + slewRateDegS = MAX(figureSequencerConfig()->rollRate, figureSequencerConfig()->loopRate) + 90.0f; + } else if (orientationHoldActivePreset() == NULL && IS_RC_MODE_ACTIVE(BOXATTLOCK) + && !orientationHoldFloorLatchBlocks(OHOLD_SOURCE_LOCK)) { + // 3D LOCK: sticks centered = hold the attitude captured at release; + // sticks deflected = pure rate flying, the lock target follows the + // aircraft and freezes on the NEW attitude when the sticks center + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_LOCK); + if (orientationHoldSticksDeflected()) { + qSollState = orientation; + } + qDesired = qSollState; + slewRateDegS = 0.0f; + // the captured target contains the full attitude - hold ALL of it + // (no free axis), heading included + pilotFullError = true; + } else if (exitSlewActive && orientationHoldActivePreset() == NULL) { + // exit handover: slew the target to level at the entry rate, then + // hand to ANGLE. The pilot deflecting a stick takes over instantly. + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_EXIT); + orientationHoldTargetFromRP(&qDesired, 0.0f, 0.0f); + fpVector3_t tiltErr; + orientationHoldComputeAttitudeError(&tiltErr, &orientation, &qDesired); + exitSlewTimeS += dT; + if (orientationHoldSticksDeflected() + || exitSlewTimeS > OHOLD_EXIT_SLEW_TIMEOUT_S + || (presetSlewCaptured + && fabsf(tiltErr.x) < OHOLD_EXIT_SLEW_DONE_DEG + && fabsf(tiltErr.y) < OHOLD_EXIT_SLEW_DONE_DEG)) { + exitSlewActive = false; + return false; + } + } else { + const orientationHoldPreset_t *preset = orientationHoldActivePreset(); + if (!preset || orientationHoldFloorLatchBlocks(preset->box)) { + return false; + } + orientationHoldCheckSourceSwitch(preset->box); + // Per attitude pitch trim, as Euler pitch of the target: positive is + // always "nose above the horizon" regardless of the attitude's roll + float pitchTrim = 0.0f; + if (preset->box == BOXINVERTED) { + pitchTrim = orientationHoldConfig()->invertedPitchTrim; + } else if (preset->box == BOXKNIFELEFT || preset->box == BOXKNIFERIGHT) { + pitchTrim = (preset->box == BOXKNIFELEFT) + ? orientationHoldConfig()->knifeLeftPitchTrim + : orientationHoldConfig()->knifeRightPitchTrim; + // Knife edge speed feedforward: the fuselage side force carries + // the weight and scales with v^2, so LESS speed needs MORE nose + // 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 hyperbola, + // so a linear term around the mid-throttle trim point is the + // honest model. 0 disables (default). + if (orientationHoldConfig()->knifeSpeedFF > 0) { + const float uGas = constrainf((rcCommand[THROTTLE] - 1000) / 1000.0f, 0.0f, 1.0f); + pitchTrim += orientationHoldConfig()->knifeSpeedFF * (0.5f - uGas); + } + } + // Active altitude hold on top of the static trim: same assist as the + // figure sequencer, referenced to the entry altitude. The cos-blend + // inside fades it out toward nose-vertical (prop hang), where + // altitude is owned by the hover throttle controller instead. + // The assist only engages once the attitude has captured the preset: + // during the entry the transient altitude error would deflect the + // target (seen as a knife-edge entry stalling at half the bank) and + // the entry itself must stay a pure attitude move. + // Pilot sticks SHIFT the target (sollage_rule, flight contract): + // roll/pitch deflection = proportional body-frame angle offset from + // the preset; the stick component that lies ALONG the earth vertical + // rotates the heading ANCHOR instead (a commanded turn) - the + // projection picks the right stick per pose by geometry: the rudder + // at level/inverted, the elevator at the knife, the aileron at the + // hang. While these act on the target the rate path must not feed + // them again as rates, see orientationHoldSticksAreTargetOffsets(). + float rollOffDeg = 0.0f; + float pitchOffDeg = 0.0f; + if (orientationHoldConfig()->stickAngleMaxDeg > 0) { + rollOffDeg = orientationHoldStickNorm(rcCommand[ROLL], rcControlsConfig()->deadband) + * orientationHoldConfig()->stickAngleMaxDeg; + pitchOffDeg = orientationHoldStickNorm(rcCommand[PITCH], rcControlsConfig()->deadband) + * orientationHoldConfig()->stickAngleMaxDeg; + } + fpVector3_t upB; + orientationHoldUpInBody(&upB); + if (!pilotAnchored) { + pilotAnchorPsiDeg = DECIDEGREES_TO_DEGREES((float)attitude.values.yaw); + pilotAnchored = true; + } + // commanded turn rate about the vertical: proportional to the stick + // deflection along up_body (proportionality_rule) + const float vertRateDps = + orientationHoldStickNorm(rcCommand[ROLL], rcControlsConfig()->deadband) + * currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f * upB.x + + orientationHoldStickNorm(rcCommand[PITCH], rcControlsConfig()->deadband) + * currentControlProfile->stabilized.rates[FD_PITCH] * 10.0f * upB.y + + orientationHoldStickNorm(rcCommand[YAW], rcControlsConfig()->yaw_deadband) + * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f * upB.z; + // tilt carve = the roll/pitch offset share ORTHOGONAL to the vertical + // (the parallel share is the turn, consumed by the anchor above) + fpVector3_t carveVec = { .v = { rollOffDeg, pitchOffDeg, 0.0f } }; + const float carveVert = carveVec.x * upB.x + carveVec.y * upB.y; + carveVec.x -= carveVert * upB.x; + carveVec.y -= carveVert * upB.y; + carveVec.z = -carveVert * upB.z; + const bool tiltCarve = fabsf(carveVec.x) > 0.5f || fabsf(carveVec.y) > 0.5f + || fabsf(carveVec.z) > 0.5f; + float leanTargetDeg = 0.0f; + if (preset->box == BOXFSPIN || tiltCarve) { + // a spin or a held bank flies the curve itself: the anchor + // FOLLOWS the aircraft, a commanded curve is never fought; + // releasing holds the heading where the curve ended + pilotAnchorPsiDeg = DECIDEGREES_TO_DEGREES((float)attitude.values.yaw); + } else if (fabsf(vertRateDps) > 1.0f) { + pilotAnchorPsiDeg += vertRateDps * dT; + while (pilotAnchorPsiDeg > 180.0f) { pilotAnchorPsiDeg -= 360.0f; } + while (pilotAnchorPsiDeg < -180.0f) { pilotAnchorPsiDeg += 360.0f; } + // flight model: the curve's coordinated lean, tan(bank) = + // omega * v / g, commanded INTO the target (the physics is + // flown, not fought), clamped to the configurable limit. + // The lean exists ONLY while yaw is commanded. + if (orientationHoldConfig()->turnRollLimitDeg > 0) { + float vCms = pidProfile()->fixedWingReferenceAirspeed; +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + vCms = getAirspeedEstimate(); + } +#endif + leanTargetDeg = constrainf( + RADIANS_TO_DEGREES(atan2_approx(DEGREES_TO_RADIANS(fabsf(vertRateDps)) * vCms, GRAVITY_CMSS)), + 0.0f, orientationHoldConfig()->turnRollLimitDeg) + * (vertRateDps > 0.0f ? 1.0f : -1.0f) + * (calculateCosTiltAngle() >= 0.0f ? 1.0f : -1.0f); + } + } + // the lean eases in AND out over the configurable return time + // (gentle, no snap - contract: ~1 s after the yaw stick centres) + { + const float leanStepDeg = orientationHoldConfig()->turnRollLimitDeg + * dT * 1000.0f / MAX((uint16_t)100, orientationHoldConfig()->turnRollReturnMs); + pilotLeanDeg += constrainf(leanTargetDeg - pilotLeanDeg, -leanStepDeg, leanStepDeg); + carveVec.x += pilotLeanDeg; + } + + orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim); + fpVector3_t entryErr; + orientationHoldComputeAttitudeError(&entryErr, &orientation, &qDesired); + // The altitude assist yields to a deliberate pitch input (same + // pattern as the hover throttle stick override): the pilot owns the + // altitude while the pitch stick is deflected, and the reference + // tracks so the release locks the NEW altitude. The FLAT SPIN mode + // never gets the assist: a spin descends by design. + // FULL error length gates the assist (review finding: |x|,|y| alone + // are BOTH zero in a 90-deg roll excursion - the z component holds + // the whole error there, and the assist then engaged mid-excursion + // and pushed pitch onto the target, driving deeper into the trough) + if (vectorNormSquared(&entryErr) < 625.0f && pitchOffDeg == 0.0f + && preset->box != BOXFSPIN) { + const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); + orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); + } else { + // still capturing or pilot pitching: keep the altitude reference + // tracking so the assist later holds the altitude where the + // attitude settled / the pilot leveled off + holdRefAltCm = getEstimatedActualPosition(Z); + } + + if (fabsf(carveVec.x) > 0.01f || fabsf(carveVec.y) > 0.01f || fabsf(carveVec.z) > 0.01f) { + fpQuaternion_t qOff; + quatFromRotVecDeg(&qOff, &carveVec); + quaternionMultiply(&qDesired, &qDesired, &qOff); + // carving / leaning: keep following at the entry rate + } else if (orientationHoldConfig()->stickAngleMaxDeg > 0 && presetSlewCaptured) { + // sticks centered after capture: the TILT eases back to the + // perfect pose (sollage_rule); the heading anchor stays put + slewRateDegS = orientationHoldConfig()->stickReturnRateDps; + } + // heading anchor composes in FRONT (earth frame), exactly like the + // figure line anchor: the full attitude error then HOLDS the heading. + // SIGN: INAV's attitude quaternion carries the NEGATED heading + // (imuComputeQuaternionFromRPY feeds -yaw, imu.c; the bench mirrors + // it in q_from_rpy(r,p,-y)) - an anchor built with +heading targets + // the MIRRORED heading, a 2*psi yaw error that the full-error + // regulator pours into the yaw channel (measured: rudder saturated, + // ele/ail at zero despite 20 deg tilt errors). + { + const float halfPsiRad = -DEGREES_TO_RADIANS(pilotAnchorPsiDeg) * 0.5f; + fpQuaternion_t qAnchor; + qAnchor.q0 = cos_approx(halfPsiRad); + qAnchor.q1 = 0.0f; + qAnchor.q2 = 0.0f; + qAnchor.q3 = sin_approx(halfPsiRad); + quaternionMultiply(&qDesired, &qAnchor, &qDesired); + } + if (preset->box == BOXFSPIN && presetSlewCaptured) { + // spin: the anchor follows the rotation, so the target must track + // it 1:1 - a slew lag would regulate AGAINST the spin (measured: + // the commanded spin rate halved) + slewRateDegS = 0.0f; + } + // NOTE (P-HANG entry, review history): with the anchor built in + // the WRONG heading frame (+psi instead of -psi, fixed above) the + // geodesic slew to the vertical ran through the roll/yaw diagonal + // and a rudderless airframe hung in the knife trough. With the + // sign corrected, level -> hang is a pure pitch+roll rotation and + // the plain governed slew flies it; an attitude-driven pitch ramp + // (lead-limited under the regulator leash) was measured as a + // working alternative and lives in the session notes should a + // future entry ever leave the pitch plane again. + pilotFullError = true; + } + + if (slewRateDegS > 0.0f) { + // the load governor also paces the target slew: the hardest load of + // a maneuver is the catch-up pull toward a distant target (the loop + // exit's level recapture read 13 g ungoverned) - and that pull is + // set by the slew rate, not by the rate clamp + const float governedStepDeg = slewRateDegS * orientationHoldLoadGovernorScale() * dT; + const float remainingDeg = (figureLineAnchored || pilotFullError) + ? orientationHoldSlewTargetFull(&qSollState, &qDesired, governedStepDeg) + : orientationHoldSlewTarget(&qSollState, &qDesired, governedStepDeg); + if (remainingDeg < 1.0f) { + presetSlewCaptured = true; + } + } else { + qSollState = qDesired; + } + + orientationHoldRegulate(errDeg); + regimeGainUpdate(errDeg, dT); + return true; +} + +// ---- Load governor ---------------------------------------------------------- +// +// A display figure flies "fast AND tight" - and both are the same boundary: +// the load budget (ohold_load_limit, a fact about the airframe). Centripetal +// load a = v * omega, radius r = v^2 / a, so at a given speed the budget is +// simultaneously the fastest rotation and the tightest radius the airframe +// pulls (ungoverned SITL loops read 13 g at the exit pull). Two channels: +// +// - rotation/slew scale, INTEGRAL: trims down while the filtered load sits +// above budget, recovers while below - no proportional droop, the load +// converges ON the budget instead of somewhere above it. +// - throttle scale, PROPORTIONAL, only while a figure or spin flies: a +// governed rotation at full power just converts into speed (a = v*omega +// stays, the loop only widens - measured); bleeding throttle with the +// overload breaks that energy feedback. Plain holds at 1 g never touch +// the pilot's throttle. +#define LOAD_GOVERNOR_TAU_S 0.15f +#define LOAD_GOVERNOR_MIN_SCALE 0.2f +#define LOAD_GOVERNOR_TRIM_PER_S 1.5f // integral trim speed at 100% overload +#define LOAD_GOVERNOR_RECOVER_PER_S 0.5f +#define LOAD_GOVERNOR_THR_MIN_SCALE 0.6f + +static float loadGovScale = 1.0f; +static float loadGovThrScale = 1.0f; +static float loadGovAccG = 1.0f; +static timeMs_t loadGovLastMs; + +void orientationHoldLoadGovernorUpdate(void) +{ + const timeMs_t nowMs = millis(); + const float dT = constrainf((nowMs - loadGovLastMs) * 0.001f, 0.0f, 0.1f); + loadGovLastMs = nowMs; + + fpVector3_t accG; + accGetMeasuredAcceleration(&accG); // cm/s^2 + const float magG = fast_fsqrtf(sq(accG.x) + sq(accG.y) + sq(accG.z)) / GRAVITY_CMSS; + loadGovAccG += (magG - loadGovAccG) * MIN(dT / LOAD_GOVERNOR_TAU_S, 1.0f); + + if (orientationHoldConfig()->loadLimitG == 0) { + loadGovScale = 1.0f; + loadGovThrScale = 1.0f; + return; + } + const float budgetG = orientationHoldConfig()->loadLimitG / 10.0f; + const float over = loadGovAccG / budgetG - 1.0f; + if (over > 0.0f) { + loadGovScale -= LOAD_GOVERNOR_TRIM_PER_S * over * dT; + } else { + loadGovScale += LOAD_GOVERNOR_RECOVER_PER_S * dT; + } + loadGovScale = constrainf(loadGovScale, LOAD_GOVERNOR_MIN_SCALE, 1.0f); + loadGovThrScale = constrainf(1.0f / (1.0f + MAX(over, 0.0f)), + LOAD_GOVERNOR_THR_MIN_SCALE, 1.0f); +} + +float orientationHoldLoadGovernorScale(void) +{ + // The governor owns MANEUVERS: figures and spins, including their exit + // pulls while the box is still up. A plain hold fighting a gust keeps + // its full slew and rate authority - the load spikes there ARE the + // gust, not a commanded trajectory, and throttling the recovery lets + // the disturbance win (a governed TVC hang oscillated itself into + // never recapturing the hold in the SITL gust matrix). + if (!(figureSequencerRequested() || orientationHoldIsSpinAboutVertical())) { + return 1.0f; + } + return loadGovScale; +} + +int16_t orientationHoldLoadGovernorThrottle(int16_t throttle) +{ + // only a governed maneuver (figure or spin) bleeds throttle; plain + // holds and normal flight pass through untouched + if (loadGovThrScale >= 1.0f + || !(figureSequencerRequested() || orientationHoldIsSpinAboutVertical())) { + return throttle; + } + const int16_t idle = getThrottleIdleValue(); + return idle + lrintf((throttle - idle) * loadGovThrScale); +} + +// ---- Thrust-first-guess authority scaling ------------------------------------ +// +// The moment a control surface produces scales with the airflow over it +// squared - forward speed at cruise, prop wash in the slow regimes. Without +// an airspeed sensor (not carried; GPS is gone in aerobatic attitudes) the +// THRUST is the first guess for that airflow: above the airframe's cruise +// throttle (an existing fact the floor recovery already uses) the commanded +// hold authority is scaled back, so a hot 3D throw does not command +// over-deflection at speed - the "45 deg throws vs a smooth cruise tune" +// dilemma. At or below cruise the full authority applies, and the HOVER +// regime always gets it (high thrust but zero forward speed: the surfaces +// need their full throw against the wash alone). The guess only bounds the +// COMMAND; the closed rate loop refines it - it deflects no further than +// the achieved rate demands. +#define AUTHORITY_MIN_SCALE 0.5f +// AUTHORITY_HOVER_ELEVATION_DEG is defined once, further up (shared with +// the hover learning-regime gate) + +float orientationHoldAuthorityScale(void) +{ + // hover/harrier band: nose high, airflow = wash, full throw + fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; + quaternionRotateVectorInv(&nose, &nose, &orientation); + const float elevDeg = RADIANS_TO_DEGREES(asin_approx(constrainf(-nose.z, -1.0f, 1.0f))); + if (elevDeg > AUTHORITY_HOVER_ELEVATION_DEG) { + return 1.0f; + } + + const int16_t idle = getThrottleIdleValue(); + const float cruiseSpan = MAX(currentBatteryProfile->nav.fw.cruise_throttle - idle, 100); + const float thrustNorm = (mixerThrottleCommand - idle) / cruiseSpan; + if (thrustNorm <= 1.0f) { + return 1.0f; + } + // surface moment ~ airflow^2 ~ thrust: scale the command with 1/thrust + return constrainf(1.0f / thrustNorm, AUTHORITY_MIN_SCALE, 1.0f); +} + +// The full rate-target controller, moved here from pid.c so the upstream +// hook stays a thin adapter. Body-identical to the historical pid.c +// implementation; the per-axis pid state is reached through the +// oholdAxisRate_t view (stick rate in, LEVEL PT1 shared, target out). +bool orientationHoldApplyRateTargets(oholdAxisRate_t axes[XYZ_AXIS_COUNT], float dT) +{ + fpVector3_t errDeg; + + // open-loop rate impulse (figure sequencer snap/spin entry): command + // the profile's full rates directly, saturating the surfaces + float impulseNorm[3]; + if (figureSequencerGetRateCommand(impulseNorm)) { + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + axes[axis].rateTargetDps = constrainf( + impulseNorm[axis] * currentControlProfile->stabilized.rates[axis] * 10.0f, + -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + } + // keep the persistent hold target on the attitude while flying + // open loop, so the catch segment slews from where the spin ends + orientationHoldSyncTargetToAttitude(); + return true; + } + + if (!orientationHoldComputeError(&errDeg, dT)) { + return false; + } + + // learned damping reserve: backs the angle gain off while a hover + // limit cycle is detected (1.0 anywhere outside the hang) + const float levelGainScale = orientationHoldLevelGainScale(); + // when the sticks act as target offsets (preset holds), the rate path + // must not also feed roll/pitch as rate commands -- yaw stays a rate, + // it is the free axis. The altitude floor recovery suppresses them too: + // it must catch AGAINST a panic-held down-elevator (the pilot override + // is switching the floor box off), yaw stays live for steering + const bool stickOffsets = orientationHoldSticksAreTargetOffsets() + || altitudeFloorRecoveryActive() + || rotorGuardRecoveryActive(); + // Controlled spin (FLAT SPIN family or figure SPIN segment). Flight + // contract: the spin axis is the BODY YAW (rudder) axis of the held + // pose - vertical at flat/inverted, HORIZONTAL at the knife edge. The + // rudder defines the rotation direction and rate (proportional); the + // sense is aircraft-referenced by construction (right rudder = nose + // right in the body frame, so seen from above an inverted spin + // reverses, like a real aircraft). Exception +P-HANG: the torque roll + // rotates about body ROLL (the prop axis) - contract question on the + // combination still open, current behaviour kept. + float spinYawNorm; + const bool spinSegment = figureSequencerGetSpinCommand(&spinYawNorm); + const bool spinPreset = orientationHoldIsSpinAboutVertical(); + float spinRateDps = 0.0f; + if (spinSegment) { + spinRateDps = spinYawNorm * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f; + } else if (spinPreset) { + // the pilot's rudder rate command becomes the spin rate + spinRateDps = axes[FD_YAW].stickRateDps; + } + // A controlled spin is a display maneuver, not a tumble: full rudder + // commands at most half a turn per second (360 deg in 2 s), regardless + // of the yaw rate the ACRO tune allows. The stalled airframe can still + // autorotate beyond the command (SITL: median 330 deg/s, peaks 875 - + // at idle the rudder has little authority to hold it back); the cap + // keeps a hot ACRO yaw tune from actively driving it faster, and the + // load governor below backs the command off with the measured load. + #define SPIN_ABOUT_VERTICAL_MAX_DPS 180.0f + spinRateDps = constrainf(spinRateDps, -SPIN_ABOUT_VERTICAL_MAX_DPS, SPIN_ABOUT_VERTICAL_MAX_DPS) + * orientationHoldLoadGovernorScale(); + // the spin axis is ALWAYS the body yaw (rudder) axis of the pose: + // vertical at flat/inverted, horizontal at the knife (contract; the + // P-HANG combination does not exist - torque roll = hang + aileron) + const uint8_t spinAxis = FD_YAW; + + // Two scale factors bound the RATE CLAMP of the hold: the load governor + // (the hardest load of a figure is not the rotation but the catch-up + // pull toward a distant target - a loop exit's level recapture pulled + // 13 g ungoverned; load a ~ v * omega, so backing the allowed rate off + // caps the pull the same way it caps the figure) and the thrust-first- + // guess authority scale (above cruise thrust the surfaces bite hard - + // the same commanded rate needs less deflection, so command less). + const float rateClampScale = orientationHoldLoadGovernorScale() + * orientationHoldAuthorityScale(); + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + // Same gain and rate limit handling as pidLevel() + float rateTarget = constrainf(errDeg.v[axis] * levelGainScale * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + -currentControlProfile->stabilized.rates[axis] * 10.0f * rateClampScale, + currentControlProfile->stabilized.rates[axis] * 10.0f * rateClampScale); + + if (pidBank()->pid[PID_LEVEL].I) { + // I8[PIDLEVEL] is used as a PT1 cutoff frequency (Hz), same as pidLevel() + rateTarget = pt1FilterApply4(axes[axis].levelFilter, rateTarget, pidBank()->pid[PID_LEVEL].I, dT); + } + + // While the sticks act on the TARGET they must not also feed the rate + // path: roll/pitch always (carve offsets); yaw too for the anchored + // pilot holds (the rudder rotates the heading anchor - no free axis). + // The floor keeps yaw live for steering (pilotFullError false there); + // 3D LOCK keeps all sticks as rates by design (its source is not a + // target-offset source). + const bool stickConsumed = stickOffsets && (axis != FD_YAW || pilotFullError); + float stickRate = stickConsumed ? 0.0f : axes[axis].stickRateDps; + if (spinSegment || spinPreset) { + if (axis == FD_YAW) { + stickRate = 0.0f; // the rudder is consumed by the spin command + } + if (axis == spinAxis) { + rateTarget += spinRateDps; // body-axis rotation per contract + } + } + axes[axis].rateTargetDps = constrainf(stickRate + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + } + return true; +} + +#if defined(SITL_BUILD) +uint32_t orientationHoldDebugSafetyWord(void) +{ + // Safety-state word for the bench (SITL debug slot 7): the replay and + // the gates must SEE when a recovery owns the aircraft - an engaged + // floor is invisible in the box readback and a figure silently flown + // under recovery override would fake the figure's proof. SITL only: + // on a real target a raw debug[] write would clobber whatever debug + // channel the user selected (review finding). + return (altitudeFloorArmed() ? 1 : 0) + | (altitudeFloorRecoveryActive() ? 2 : 0) + | (rotorGuardRecoveryActive() ? 4 : 0) + | (navigationPositionEstimateIsHealthy() ? 8 : 0) + | (altitudeFloorOrbitActive() ? 16 : 0) + | (altitudeFloorOrbitViaNav() ? 32 : 0); +} +#endif + +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h new file mode 100644 index 00000000000..4cdc59465b9 --- /dev/null +++ b/src/main/flight/orientation_hold.h @@ -0,0 +1,213 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include + +#include "common/axis.h" +#include "common/filter.h" +#include "common/quaternion.h" +#include "common/vector.h" + +#include "config/parameter_group.h" + +// Orientation hold: quaternion based attitude controller that can stabilise +// arbitrary target attitudes (inverted, knife edge, prop hang) on fixed wing. +// Unlike the Euler based ANGLE controller it has no singularity at +// pitch = +/-90 deg. Heading (rotation about the earth vertical axis) is +// always left free, matching ANGLE mode behaviour in normal flight. + +// Per attitude pitch trim on the hold target, applied as the Euler pitch of +// the target (before the attitude's roll): positive = nose above the +// horizon in every attitude. Inverted flight needs it to hold altitude +// (down-elevator bias), knife edge to carry the fuselage lift. +typedef struct orientationHoldConfig_s { + int8_t invertedPitchTrim; // deg, nose above horizon in inverted flight + int8_t knifeLeftPitchTrim; // deg, nose above horizon in left knife edge + int8_t knifeRightPitchTrim; // deg, nose above horizon in right knife edge + // (separate per side: prop effects break the symmetry) + uint8_t hoverGainLearned; // %, LEARNED angle-gain scale per regime: + // the normal-flight gains are the reference + // (100), the limit-cycle detector backs the + // scale off while that regime oscillates and + // writes it back at regime exit; saved on + // disarm. The next entry (and the next + // flight) starts at the learned value + // instead of oscillating its way down again. + uint8_t invertedGainLearned; // %, learned scale for the inverted hold + uint8_t knifeGainLearned; // %, learned scale for the knife edge holds + uint8_t figureGainLearned; // %, learned scale while a figure flies + uint16_t entryRateDps; // deg/s target slew for PRESET entries. + // Separate from fig_roll_rate on purpose: + // a deliberate slow roll figure and a snappy + // entry into a hold are different intents. + uint8_t stickAngleMaxDeg; // deg of body-frame target offset at full + // roll/pitch stick: ANGLE semantics around + // the rotated reference (deflection = held + // angle offset, release = slow return). + // 0 = sticks stay raw rate commands. + uint8_t stickReturnRateDps; // deg/s the target returns to the preset + // after the sticks center + uint8_t turnRollLimitDeg; // deg of automatic roll lean allowed while + // a commanded turn (vertical-axis stick) + // flies the curve - the lean IS the curve + // physics (tan(bank) = omega*v/g) and is + // commanded into the target, not fought. + // 0 = no lean (flat turns only). + uint16_t turnRollReturnMs; // ms the lean eases back out after the + // yaw stick returns to 0 (gentle, no snap) + uint8_t knifeSpeedFF; // deg of extra knife-edge nose angle per + // half-throttle of speed deficit: the + // fuselage side force scales with v^2, + // throttle is the v^2 proxy. 0 = off. + uint8_t loadLimitG; // display load budget [g x 10] the load + // governor holds figures and spins to; + // 0 = governor off +} orientationHoldConfig_t; + +PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig); + +// Compute the body frame attitude error (deg, per body axis) between qEst +// and the tilt part of qTarget. The rotation of qEst about the earth +// vertical axis (heading / twist) is removed before the error is formed, so +// the returned error never asks for a heading change. Pure shortest-tilt +// rotation: large-error entry paths are shaped by the target slew inside +// orientationHoldComputeError(), not here. +void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget); + +// Build a target quaternion from roll/pitch (deg, yaw = 0) using the same +// Euler convention as the attitude estimator. +void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float pitchDeg); + +// True when any orientation hold box (INVERTED / KNIFE EDGE / PROP HANG) is +// selected on the transmitter. +bool orientationHoldIsRequested(void); + +// True while a released hold still owns the exit handover toward ANGLE: +// the hold slews its target to level first so the Euler level controller +// never sees the full attitude error as one step (a prop hang exit whips +// through nose down otherwise). Latches on the release edge when the +// attitude is far from level; clears when level is captured, the pilot +// deflects a stick, or a timeout expires. +bool orientationHoldExitSlewPending(void); + +// Coordinated-turn feedforward for deliberately turning holds (sequencer +// WAIT_POS, altitude-floor orbit): true with the current turn bank in +// *bankDeg when the yaw rate must follow the banked turn instead of +// being regulated to zero by the heading-free hold. +bool orientationHoldTurnCoordinationBank(float *bankDeg); + +// Body frame attitude error (deg) for the currently selected target. +// The target is a persistent attitude quaternion seeded on the actual +// attitude at engage and slewed toward the requested attitude +// (ohold_entry_rate for preset entries - separate from fig_roll_rate on +// purpose, see entryRateDps), so the error stays small and the entry path +// is an explicit trajectory. Returns false when no orientation hold box is +// active. +bool orientationHoldComputeError(fpVector3_t *errDeg, float dT); + +// Re-seed the persistent target on the actual attitude. Call every cycle +// the regulator is bypassed while a source is active (figure IMPULSE +// segments), so the catch afterwards starts from the actual attitude. +void orientationHoldSyncTargetToAttitude(void); + +// Call while ORIENTATION_HOLD_MODE is inactive: resets the target-source +// tracking (and the rate-loop I accumulators once on the exit edge) +void orientationHoldResetSourceTracking(void); + +// True while the PROP HANG preset is the active hold target (used by the +// hover throttle to own the altitude axis) +bool orientationHoldIsPropHang(void); + +// True while a knife edge or inverted preset is the active hold target +// (used by the knife/inverted throttle assist, criterion vz -> 0) +bool orientationHoldIsKnifeOrInverted(void); + +// True while the active regime's limit-cycle detector holds the learned +// gain scale measurably below the reference (the hold is oscillating); +// the knife/inverted throttle assist raises the speed on this signal +bool orientationHoldRegimeOscillating(void); + +// True while the FLAT SPIN family owns the target (flat / inverted / knife / +// torque roll): the pilot's rudder commands rotation about the earth +// vertical instead of the body yaw axis +bool orientationHoldIsSpinAboutVertical(void); + +// Earth-up direction expressed in the body frame (the spin distribution +// axis; also the axis the reduced attitude error leaves free) +void orientationHoldUpInBody(fpVector3_t *upBody); + +// True while roll/pitch sticks act as TARGET OFFSETS around the rotated +// reference (preset hold active and ohold_stick_angle > 0): the rate path +// must then not also feed them as rate commands. Yaw stays a rate command, +// it is the free axis. +bool orientationHoldSticksAreTargetOffsets(void); + +// Learned damping reserve for the hover regime: scale factor (0.3..1.0) on +// the angle-loop gain. A detected limit cycle around the vertical backs it +// off fast, quiet time recovers it slowly; 1.0 outside the hang. Apply to +// the angle error before the LEVEL P gain. +float orientationHoldLevelGainScale(void); + +// Load governor: figures and spins fly "fast AND tight" - at the load +// budget (ohold_load_limit), never beyond it. Update once per +// figure-sequencer tick (self-timed). The scale (0.2..1.0) multiplies the +// COMMANDED rotation rate of figures, the spin about the vertical and the +// target slew; the throttle hook bleeds power while a governed maneuver +// exceeds the budget (full power would just convert the governed rotation +// into speed and keep the load). +void orientationHoldLoadGovernorUpdate(void); +float orientationHoldLoadGovernorScale(void); +// Throttle hook (mixer): bleeds power while a governed figure or spin +// exceeds the budget; passes through unchanged otherwise +int16_t orientationHoldLoadGovernorThrottle(int16_t throttle); + +// Thrust-first-guess authority scaling (0.5..1.0): surface moment scales +// with airflow^2, and thrust is the airflow proxy without a pitot. Above +// the airframe's cruise throttle the commanded hold authority backs off; +// the hover regime always keeps full throw (wash-only airflow). Multiplies +// the orientation-hold rate clamp. +float orientationHoldAuthorityScale(void); + +// The full rate-target controller (error -> per-axis rate targets, spin +// distribution about the earth vertical, figure impulse passthrough). +// The pid loop adapts its per-axis state through this view: the pilot's +// rate command in, pid's LEVEL PT1 filter state shared, the hold's final +// rate target out. Returns false when the hold has nothing to command +// (no active source) - the caller keeps its own targets untouched. +typedef struct oholdAxisRate_s { + float stickRateDps; // in: pilot rate command for this axis + pt1Filter_t *levelFilter; // pid's per-axis LEVEL filter state + float rateTargetDps; // out: the hold's final rate target +} oholdAxisRate_t; +bool orientationHoldApplyRateTargets(oholdAxisRate_t axes[XYZ_AXIS_COUNT], float dT); + +#if defined(SITL_BUILD) +// Safety-state word for the SITL bench (debug slot 7): bit0 floor armed, +// bit1 floor recovery, bit2 rotor guard, bit3 estimate healthy, bit4 +// orbit, bit5 orbit via nav loiter. +uint32_t orientationHoldDebugSafetyWord(void); +#endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9beda7ae225..67b477170f0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -44,6 +44,8 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" +#include "flight/orientation_hold.h" +#include "flight/rotor_guard.h" #include "flight/rpm_filter.h" #include "flight/kalman.h" #include "flight/smith_predictor.h" @@ -646,6 +648,18 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) { // Limit max bank angle for multirotor during Nav mode Angle controlled position adjustment uint16_t maxBankAngle = STATE(MULTIROTOR) && navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && isAdjustingPosition() ? DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle) : pidProfile()->max_angle_inclination[axis]; +#ifdef USE_FW_AEROBATICS + // Autogyro attitude limiter (flight contract): with the GYRO mode on, + // the COMMANDED curve flight is limited - bank and pitch are clamped so + // a commanded attitude never reaches the region where the rotor's + // vertical lift collapses. The tip-AWAY is the rotor guard's catch. + if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXROTORGUARD)) { + const uint16_t gyroLimit = DEGREES_TO_DECIDEGREES( + (axis == FD_ROLL) ? rotorGuardConfig()->rollLimitDeg + : rotorGuardConfig()->pitchLimitDeg); + maxBankAngle = MIN(maxBankAngle, gyroLimit); + } +#endif #ifdef USE_PROGRAMMING_FRAMEWORK float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), maxBankAngle); @@ -725,6 +739,28 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } } +#ifdef USE_FW_AEROBATICS +// Quaternion based attitude hold for arbitrary target attitudes (inverted, +// knife edge, prop hang). Works on all three body axes and stays defined at +// pitch = +/-90 deg where the Euler based pidLevel() is singular. The full +// controller lives in flight/orientation_hold.c; this adapter only maps the +// per-axis pid state (stick rate in, shared LEVEL filter, target out). +static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) +{ + oholdAxisRate_t axes[XYZ_AXIS_COUNT]; + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + axes[axis].stickRateDps = pidStates[axis].rateTarget; + axes[axis].levelFilter = &pidStates[axis].angleFilterState; + } + if (!orientationHoldApplyRateTargets(axes, dT)) { + return; // no active hold source: the stick rate targets stand + } + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + pidStates[axis].rateTarget = axes[axis].rateTargetDps; + } +} +#endif + /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */ static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { @@ -1277,6 +1313,14 @@ void FAST_CODE pidController(float dT) const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; angleHoldIsLevel = false; +#ifdef USE_FW_AEROBATICS + if (FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { + // Quaternion attitude hold replaces the Euler level controllers on all three axes + pidOrientationHold(pidState, dT); + restartAngleHoldMode = true; + canUseFpvCameraMix = false; // not compatible with FPVANGLEMIX + } else +#endif for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) { // If axis angle override, get the correct angle from Logic Conditions @@ -1306,6 +1350,20 @@ void FAST_CODE pidController(float dT) pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } +#ifdef USE_FW_AEROBATICS + else if (FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { + // Turning holds (WAIT_POS banks toward home, the floor orbit + // circles the breach point): feed the coordinated turn rates + // forward, otherwise the heading-free hold regulates the physical + // turn yaw rate back to zero and the aircraft never turns + // (measured on the floor orbit: 35 deg bank held on a frozen + // heading, a knife-edge-style straight slip away from the anchor) + float bankDeg; + if (orientationHoldTurnCoordinationBank(&bankDeg)) { + pidTurnAssistant(pidState, DEGREES_TO_RADIANS(bankDeg), 0.0f); + } + } +#endif // Apply FPV camera mix if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) { diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c new file mode 100644 index 00000000000..2e87f75faff --- /dev/null +++ b/src/main/flight/rotor_guard.c @@ -0,0 +1,216 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include + +#ifdef USE_FW_AEROBATICS + +#include "common/axis.h" +#include "common/maths.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/rotor_guard.h" + +#include "navigation/navigation.h" + +#include "sensors/battery.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, PG_ROTOR_GUARD_CONFIG, 2); + +PG_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, + .bankDeg = SETTING_ROTOR_GUARD_BANK_DEFAULT, + .sinkCms = SETTING_ROTOR_GUARD_SINK_DEFAULT, + .recoveryPitchDeg = SETTING_ROTOR_GUARD_PITCH_DEFAULT, + .throttleBoostPct = SETTING_ROTOR_GUARD_THROTTLE_BOOST_DEFAULT, + .minHeightM = SETTING_ROTOR_GUARD_MIN_HEIGHT_DEFAULT, + .rollLimitDeg = SETTING_ROTOR_GUARD_ROLL_LIMIT_DEFAULT, + .pitchLimitDeg = SETTING_ROTOR_GUARD_PITCH_LIMIT_DEFAULT, +); + +// The excursion must persist: a gust or a crisp figure entry crosses the +// bank line for a moment, a tip-over stays there (authority is gone) +#define ROTOR_GUARD_TRIP_MS 300 +// Release hysteresis: wings back under this bank, held for the window +#define ROTOR_GUARD_RELEASE_BANK_DEG 20 +// Minimum time on the recovery before any release: the wings answering is +// the CATCH dynamics, not proof the rotor is healthy - the rpm rebuilds +// from inflow with a seconds-long time constant, and a release into a +// still-starved rotor re-tips DEEPER (measured: -48 caught, released +// after 1.5 s, re-tipped to -138 and into the ground). Time on the +// throttle floor is the honest rpm proxy when no rpm feedback exists. +#define ROTOR_GUARD_MIN_HOLD_MS 5000 +// Length of the initial max-throttle burst of the recovery +#define ROTOR_GUARD_BURST_MS 2000 + +static bool guardRecovery = false; +static timeMs_t tripStartMs = 0; +static timeMs_t recoveryStartMs = 0; +static timeMs_t levelSinceMs = 0; +static bool sticksSeenCentered = false; +static int16_t preTripThrottleUs = 0; // the operating throttle when the trip + // fired - the RELATIVE boost baseline + +void rotorGuardUpdate(void) +{ + if (!IS_RC_MODE_ACTIVE(BOXROTORGUARD) || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE)) { + guardRecovery = false; + tripStartMs = 0; + return; + } + + // THE PILOT OVERRIDES THE AUTOPILOT, throttle included: an idle stick + // is landing intent - the guard must never spin the thrust up against + // it (a tip in the flare would otherwise force a go-around, and a + // rollout tip would get POWER on the ground). No trip at idle, and + // pulling the throttle to idle releases a running recovery instantly. + // The guard's only lever is thrust anyway - without permission to use + // it a recovery is pointless. + if (throttleStickIsLow()) { + guardRecovery = false; + tripStartMs = 0; + return; + } + + const float bankDeg = ABS(attitude.values.roll) / 10.0f; + const float vz = getEstimatedActualVelocity(Z); // cm/s + + if (!guardRecovery) { + // Tip-over signature: rolled past anything an autogyro flies on + // purpose AND sinking - the soft-tilt rolloff, not a figure. Must + // persist ROTOR_GUARD_TRIP_MS to reject transients. + const bool tripping = bankDeg > rotorGuardConfig()->bankDeg + && vz < -(float)rotorGuardConfig()->sinkCms; + if (tripping) { + if (tripStartMs == 0) { + tripStartMs = millis(); + } else if (millis() - tripStartMs > ROTOR_GUARD_TRIP_MS) { + guardRecovery = true; + recoveryStartMs = millis(); + levelSinceMs = 0; + sticksSeenCentered = false; + // the RELATIVE boost baseline: the throttle the aircraft was + // operating on when the trip fired (a headwind day flies on + // a higher trim throttle and the recovery scales with it), + // never below the cruise value + preTripThrottleUs = MAX(rcCommand[THROTTLE], + currentBatteryProfile->nav.fw.cruise_throttle); + } + } else { + tripStartMs = 0; + } + } else { + // Release when the tilt authority is visibly back: wings held + // level-ish for a sustained window. Height is deliberately NOT a + // release condition - that is the altitude floor's job, and a + // T/W<1 autogyro settles in a slow descent at any sane recovery + // attitude (measured: an arrest-the-sink condition pinned the + // recovery active all the way to the ground). The box stays + // armed; a renewed excursion simply trips it again. + if (millis() - recoveryStartMs > ROTOR_GUARD_MIN_HOLD_MS + && bankDeg < ROTOR_GUARD_RELEASE_BANK_DEG) { + if (levelSinceMs == 0) { + levelSinceMs = millis(); + } else if (millis() - levelSinceMs > 1500) { + guardRecovery = false; + tripStartMs = 0; + levelSinceMs = 0; + } + } else { + levelSinceMs = 0; + } + // ... or the pilot takes over: sticks must return to center ONCE + // (a deflection held through the tip-over is not a takeover), a + // fresh roll/pitch input then releases immediately. Yaw stays + // steering - same contract as the altitude floor. + const bool deflected = ABS(rcCommand[ROLL]) > rcControlsConfig()->deadband + || ABS(rcCommand[PITCH]) > rcControlsConfig()->deadband; + if (!sticksSeenCentered) { + sticksSeenCentered = !deflected; + } else if (deflected) { + guardRecovery = false; + tripStartMs = 0; + } + } +} + +bool rotorGuardRecoveryActive(void) +{ + return guardRecovery; +} + +int16_t rotorGuardThrottleFloorUs(void) +{ + // Thrust (with the rotor LOADED) is the lever that brings the rotor + // rpm - and with it the roll authority - back. The floor is RELATIVE: + // the pre-trip operating throttle raised by rotor_guard_throttle_boost + // percent, so a headwind trim point scales the recovery with it. + // HEIGHT-GATED: below rotor_guard_min_height (baro above the start + // altitude) no aggressive recovery power is flown - near the ground it + // is wings-level + cushion only. 0 = no claim on the throttle. + if (!guardRecovery) { + return 0; + } + if (getEstimatedActualPosition(Z) < rotorGuardConfig()->minHeightM * 100.0f) { + return 0; + } + // the FIRST moments of the recovery fly a BRIEF MAX-THROTTLE burst: with + // the rotor LOADED that is the fastest way back to authority (thrust -> + // speed -> inflow -> rpm; contract). After the burst the relative boost + // floor holds until the release conditions clear the recovery. + if (millis() - recoveryStartMs < ROTOR_GUARD_BURST_MS) { + return getMaxThrottle(); + } + return preTripThrottleUs + + (preTripThrottleUs - 1000) * rotorGuardConfig()->throttleBoostPct / 100; +} + +float rotorGuardRecoveryPitchDeg(void) +{ + // REAL-GYRO DOCTRINE: the rotor must stay LOADED - a nose-down push + // unloads the disk and decays the rpm FASTER (power push-over). The + // recovery pitch therefore defaults to 0 (hold the load, wings level, + // let the max-throttle boost rebuild speed -> inflow -> rpm). The + // parameter stays for airframes that need a small bias; keep it >= 0. + // (The old -5 nose-down default came from a plant whose rpm model + // coupled to airspeed only, without the load term.) + if (ABS(attitude.values.roll) / 10.0f > ROTOR_GUARD_RELEASE_BANK_DEG + 10) { + return (float)rotorGuardConfig()->recoveryPitchDeg; + } + return 0.0f; +} + +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/rotor_guard.h b/src/main/flight/rotor_guard.h new file mode 100644 index 00000000000..38b8105afda --- /dev/null +++ b/src/main/flight/rotor_guard.h @@ -0,0 +1,69 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "config/parameter_group.h" + +// Tip-over guard for autogyros. The rotor is the wing: its lift and its +// roll authority both scale with rotor rpm squared, and the rpm lives on +// the inflow through the disk. In slow flight or a botched launch the rpm +// decays, the lateral tilt goes soft, and the aircraft rolls away with the +// stick already at the stop - unrecoverable by attitude control alone. +// There is no rpm feedback on these airframes; the ONLY lever that restores +// authority is THRUST (thrust -> speed -> inflow -> rpm). The guard detects +// the uncommanded roll excursion while sinking and flies the recovery: +// wings level, nose slightly DOWN (feed the disk), and a throttle floor. +typedef struct rotorGuardConfig_s { + uint8_t bankDeg; // an autogyro never flies beyond this bank on + // purpose - excursion past it while sinking is + // the tip-over signature + uint16_t sinkCms; // minimum sink rate [cm/s] to qualify + int8_t recoveryPitchDeg; // recovery pitch target; keep >= 0 per real-gyro + // doctrine (nose-down UNLOADS the rotor and + // decays the rpm faster - power-push-over) + uint8_t throttleBoostPct; // recovery throttle floor = the pre-trip + // operating throttle (at least cruise) raised + // by this PERCENTAGE - relative, so a headwind + // trim point scales the recovery with it + uint8_t minHeightM; // below this height (baro above the START + // altitude) no aggressive recovery power - + // wings level + cushion only + // Attitude LIMITER (flight contract): with the GYRO mode on, the + // COMMANDED curve flight is limited - the bank clamp keeps a commanded + // attitude out of the region where the rotor's vertical lift collapses; + // the tip-AWAY (uncommanded excursion) is what the guard above catches. + uint8_t rollLimitDeg; // max commanded bank while the mode is on + uint8_t pitchLimitDeg; // max commanded pitch while the mode is on +} rotorGuardConfig_t; + +PG_DECLARE(rotorGuardConfig_t, rotorGuardConfig); + +void rotorGuardUpdate(void); +bool rotorGuardRecoveryActive(void); +float rotorGuardRecoveryPitchDeg(void); + +// Recovery throttle floor [us] (cruise + rotor_guard_throttle_add); +// 0 while inactive. Consumed as a MAX claim by the throttle path. +int16_t rotorGuardThrottleFloorUs(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index e72dd66ea7d..0e2a5ed8c37 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -53,6 +53,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/thrust_vectoring.h" #include "io/gps.h" @@ -204,6 +205,29 @@ int getServoCount(void) } } +bool servoMixerHasYawControl(void) +{ + // Does the active servo mixer route ANY yaw command to an effector - + // a rudder (stabilized yaw, plus/minus variants) or a thrust-vectoring + // yaw vane? A flying wing has none: it cannot hold a knife edge, no + // matter what the attitude controller commands, so knife-edge modes + // are not offered on such a model. + for (int i = 0; i < servoRuleCount; i++) { + switch (currentServoMixer[i].inputSource) { + case INPUT_STABILIZED_YAW: + case INPUT_STABILIZED_YAW_PLUS: + case INPUT_STABILIZED_YAW_MINUS: +#ifdef USE_THRUST_VECTORING + case INPUT_TVC_YAW: +#endif + return true; + default: + break; + } + } + return false; +} + void loadCustomServoMixer(void) { @@ -353,6 +377,10 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] +#ifdef USE_THRUST_VECTORING + thrustVectoringApplyInputs(input, mixerThrottleCommand); +#endif + input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 3f8ebf1b4e8..cc2ee80c29b 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -85,6 +85,9 @@ typedef enum { INPUT_RC_CH33 = 58, INPUT_RC_CH34 = 59, INPUT_MIXER_SWITCH_HELPER = 60, + INPUT_TVC_ROLL = 61, + INPUT_TVC_PITCH = 62, + INPUT_TVC_YAW = 63, INPUT_SOURCE_COUNT } inputSource_e; @@ -201,3 +204,7 @@ void servoComputeScalingFactors(uint8_t servoIndex); void servosInit(void); int getServoCount(void); uint8_t getMinServoIndex(void); +// True when the active servo mixer routes a yaw command to any effector +// (rudder or TVC yaw vane). A flying wing has none - knife-edge modes are +// physically impossible there and are not offered. +bool servoMixerHasYawControl(void); diff --git a/src/main/flight/soaring.c b/src/main/flight/soaring.c new file mode 100644 index 00000000000..7d8683b059e --- /dev/null +++ b/src/main/flight/soaring.c @@ -0,0 +1,402 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include + +#include + +#ifdef USE_SOARING + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/vector.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/soaring.h" +#include "flight/wind_estimator.h" + +#include "navigation/navigation.h" + +#include "rx/rx.h" + +#include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/pitotmeter.h" +#include "sensors/sensors.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(soaringConfig_t, soaringConfig, PG_SOARING_CONFIG, 0); + +PG_RESET_TEMPLATE(soaringConfig_t, soaringConfig, + .varioTriggerCms = SETTING_SOAR_VARIO_TRIGGER_DEFAULT, + .varioExitCms = SETTING_SOAR_VARIO_EXIT_DEFAULT, + .altMinM = SETTING_SOAR_ALT_MIN_DEFAULT, + .altMaxM = SETTING_SOAR_ALT_MAX_DEFAULT, + .bankDeg = SETTING_SOAR_BANK_DEFAULT, + .sinkLevelCms = SETTING_SOAR_SINK_LEVEL_DEFAULT, + .centreGainPct = SETTING_SOAR_CENTRE_GAIN_DEFAULT, +); + +// Centre shift rate [cm/s] along the history gradient at centreGainPct = 100. +// Small, because "slowly" - a circle that chases noise loses the thermal. +#define SOAR_CENTRE_GAIN_SCALE 60.0f +// Never let the estimate run more than this from where the climb started +// (a runaway would walk the loiter out of the sky). Sized to the lift grid: +// the map may legitimately lead the circle up to its own half-width. +#define SOAR_CENTRE_MAX_DRIFT_CM 45000.0f // 450 m ~ grid half-width +// The trigger must be SUSTAINED: a transient crossing of a weak lift edge +// never holds this long, the wide band around a real core does (measured: +// an instant trigger anchored the circle 520 m off-core on a transient). +#define SOAR_TRIGGER_SUSTAIN_S 2.0f +// ---- Lift-grid centering (flight contract, centering v3) ------------------- +// A COARSE CHECKERBOARD of max-netto cells (Daniel): 32 x 32 cells of ~30 m +// = 1 KB covering ~1 km x 1 km. The MAP remembers where the lift was - a +// handful of ring samples is degenerate for a gradient (measured: anchored +// 600 m off-core, local gradient zero), the grid steers toward the best +// KNOWN lift even where the local gradient is blind. The grid window RIDES +// THE WIND (origin moves with the air mass - zero data movement) and ROLLS +// with the aircraft: flying out of the window discards the farthest line and +// reuses its memory for the new near side (toroidal indexing, no copying). +#define SOAR_GRID_N 32 +#define SOAR_GRID_CELL_CM 3000.0f // ~30 m cells +#define SOAR_GRID_LIFT_FLOOR 130 // cell value that counts as real lift (~ +0.2 m/s) +#define SOAR_GRID_DECAY_S 2.0f // 0.1 m/s fade per this period: dead thermals age out +#define SOAR_EXPLORE_FRAC 0.2f // exploration offset, fraction of the loiter radius +#define SOAR_EXPLORE_ADVANCE_RAD 1.9f // exploration direction advance per round (never repeats) + +static bool soarActive = false; +static bool thermalling = false; +static fpVector3_t thermalCentre; // earth frame, cm from home (XY loiter) +static fpVector3_t breachAnchor; // where the climb was first found +static float vPrev = 0.0f; +static float netVarioCms = 0.0f; +// lift grid (rolling, wind-riding) + exploration + circle-averaged exit +static uint8_t liftGrid[SOAR_GRID_N][SOAR_GRID_N]; // max netto seen; 0 = unknown, + // else clamp(netto*10 + 128) +static fpVector3_t gridOriginCm; // ground pos of window corner [0][0]; RIDES THE WIND +static int16_t gridBaseI = 0, gridBaseJ = 0; // toroidal base of the rolling window +static float gridDecayTimerS = 0.0f; +static float exploreAngleRad = 0.0f; +static float roundAccumRad = 0.0f; +static float exitMeanMs = 0.0f; // circle-averaged netto for the exit + +// rolling the window costs exactly ONE line: the line that falls out of the +// far side is re-initialised and becomes the new near side - nothing copies +static void soarGridClearI(int16_t arrI) +{ + for (int j = 0; j < SOAR_GRID_N; j++) { + liftGrid[arrI][j] = 0; + } +} +static void soarGridClearJ(int16_t arrJ) +{ + for (int i = 0; i < SOAR_GRID_N; i++) { + liftGrid[i][arrJ] = 0; + } +} + +static bool netVarioValid = false; // false while the motor blinds the vario + +static float computeNetVarioMs(float dT) +{ + // total-energy variometer: the air's vertical motion, our own polar + // sink compensated out. e = h + v^2/2g ; the pitot gives v, so airspeed + // transients (the phantom climb on a pull-out) cancel - the reason a + // pitot is required. sink uses the EXACT cos of the actual bank (ArduSoar + // uses a small-angle approximation that drifts at the 35-45 deg thermal + // bank; this does not). + float v = getAirspeedEstimate() / 100.0f; // m/s + v = constrainf(v, 3.0f, 100.0f); + const float hdot = getEstimatedActualVelocity(Z) / 100.0f; // m/s, up + + const float vdot = (v - vPrev) / dT; + vPrev = v; + float cosRoll = fabsf(cos_approx(DECIDEGREES_TO_RADIANS(attitude.values.roll))); + cosRoll = MAX(0.2f, cosRoll); + const float energyRate = hdot + v * vdot / GRAVITY_MSS; // m/s + // MOTOR-AWARE (the entry gate, flight contract: only TRUE AIR LIFT may + // trigger or paint the grid): the polar-sink compensation is only valid + // with the motor off - at cruise the motor cancels the airframe's sink + // and the vario would read +sink of phantom lift in plain level flight + // (measured: thermalling triggered 600 m from any thermal on exactly + // that). While THERMALLING the FW forces the motor off -> full + // compensation; otherwise blend it out toward the cruise throttle, and + // above cruise (a deliberate powered climb) the vario is blind. + float motorFactor = 0.0f; + if (!thermalling) { + const int16_t thrUs = rcCommand[THROTTLE]; + const int16_t idleUs = getThrottleIdleValue(); + const int16_t cruiseUs = currentBatteryProfile->nav.fw.cruise_throttle; + motorFactor = constrainf((float)(thrUs - idleUs) / MAX(1, cruiseUs - idleUs), 0.0f, 1.0f); + netVarioValid = thrUs <= cruiseUs + 25; + } else { + netVarioValid = true; + } + const float sink = (soaringConfig()->sinkLevelCms / 100.0f) + / (cosRoll * fast_fsqrtf(cosRoll)) // /cos^1.5 + * (1.0f - motorFactor); + return energyRate + sink; // air w [m/s] +} + +void soaringUpdate(float dT) +{ + // The pilot's SOARING mode arms it; a pitot is required (the net vario + // is meaningless without airspeed) and only fixed wing soars. Without + // any of these the module is inert - the FC behaves exactly as upstream. + if (!IS_RC_MODE_ACTIVE(BOXSOARING) || !ARMING_FLAG(ARMED) + || !STATE(AIRPLANE) || !sensors(SENSOR_PITOT)) { + if (thermalling) { + navForcedPosholdClear(); // release the loiter + } + soarActive = false; + thermalling = false; + return; + } + soarActive = true; + + const float netVario = computeNetVarioMs(dT); + netVarioCms = netVario * 100.0f; + + const float alt = getEstimatedActualPosition(Z) / 100.0f; // m + const float triggerMs = soaringConfig()->varioTriggerCms / 100.0f; + const float exitMs = soaringConfig()->varioExitCms / 100.0f; + + if (!thermalling) { + // enter a thermal: TRUE AIR lift over the trigger (netVarioValid - + // a powered climb never triggers), SUSTAINED (a transient edge + // crossing never holds SOAR_TRIGGER_SUSTAIN_S), inside the band + static float triggerHoldS = 0.0f; + if (netVarioValid && netVario > triggerMs + && alt > soaringConfig()->altMinM && alt < soaringConfig()->altMaxM) { + triggerHoldS += dT; + } else { + triggerHoldS = 0.0f; + } + if (triggerHoldS >= SOAR_TRIGGER_SUSTAIN_S) { + triggerHoldS = 0.0f; + thermalling = true; + breachAnchor.x = thermalCentre.x = getEstimatedActualPosition(X); + breachAnchor.y = thermalCentre.y = getEstimatedActualPosition(Y); + breachAnchor.z = thermalCentre.z = getEstimatedActualPosition(Z); + memset(liftGrid, 0, sizeof(liftGrid)); + gridBaseI = gridBaseJ = 0; + gridDecayTimerS = 0.0f; + gridOriginCm.x = thermalCentre.x - (SOAR_GRID_N / 2) * SOAR_GRID_CELL_CM; + gridOriginCm.y = thermalCentre.y - (SOAR_GRID_N / 2) * SOAR_GRID_CELL_CM; + exploreAngleRad = roundAccumRad = 0.0f; + exitMeanMs = netVario; // seed: never an instant exit at entry + // hand the loiter to the real nav machinery, anchored here + navForcedPosholdActivateAt(&thermalCentre); + } + return; + } + + // circle period from the commanded bank and the current speed: + // omega = g * tan(bank) / v, T = 2 pi / omega + const float vMs = constrainf(getAirspeedEstimate() / 100.0f, 5.0f, 60.0f); + const float omega = GRAVITY_MSS + * tan_approx(DEGREES_TO_RADIANS((float)soaringConfig()->bankDeg)) / vMs; + const float periodS = 2.0f * M_PIf / omega; + + // CENTERING v3 (flight contract): the coarse lift grid. The window rides + // the WIND (origin moves with the air mass - the map is anchored to the + // column by construction, zero data movement) ... + gridOriginCm.x += getEstimatedWindSpeed(X) * dT; + gridOriginCm.y += getEstimatedWindSpeed(Y) * dT; + // ... and ROLLS with the aircraft: leaving the window costs exactly one + // line - the farthest falls out, its memory is re-initialised as the new + // near side (toroidal base index, nothing copies) + int16_t ci = (int16_t)floorf((getEstimatedActualPosition(X) - gridOriginCm.x) / SOAR_GRID_CELL_CM); + int16_t cj = (int16_t)floorf((getEstimatedActualPosition(Y) - gridOriginCm.y) / SOAR_GRID_CELL_CM); + while (ci >= SOAR_GRID_N) { + soarGridClearI(gridBaseI); + gridBaseI = (gridBaseI + 1) % SOAR_GRID_N; + gridOriginCm.x += SOAR_GRID_CELL_CM; + ci--; + } + while (ci < 0) { + gridBaseI = (gridBaseI - 1 + SOAR_GRID_N) % SOAR_GRID_N; + soarGridClearI(gridBaseI); + gridOriginCm.x -= SOAR_GRID_CELL_CM; + ci++; + } + while (cj >= SOAR_GRID_N) { + soarGridClearJ(gridBaseJ); + gridBaseJ = (gridBaseJ + 1) % SOAR_GRID_N; + gridOriginCm.y += SOAR_GRID_CELL_CM; + cj--; + } + while (cj < 0) { + gridBaseJ = (gridBaseJ - 1 + SOAR_GRID_N) % SOAR_GRID_N; + soarGridClearJ(gridBaseJ); + gridOriginCm.y -= SOAR_GRID_CELL_CM; + cj++; + } + // record: each cell keeps the MAX netto seen there (robust against the + // turbulent instant value - the best pass through a cell is the truth) + { + const uint8_t ai = (gridBaseI + ci) % SOAR_GRID_N; + const uint8_t aj = (gridBaseJ + cj) % SOAR_GRID_N; + const uint8_t val = constrain(lrintf(netVario * 10.0f) + 128, 1, 255); + if (val > liftGrid[ai][aj]) { + liftGrid[ai][aj] = val; + } + } + // slow fade so a dead thermal ages out of the map (0.1 m/s per period) + gridDecayTimerS += dT; + if (gridDecayTimerS >= SOAR_GRID_DECAY_S) { + gridDecayTimerS = 0.0f; + for (int i = 0; i < SOAR_GRID_N; i++) { + for (int j = 0; j < SOAR_GRID_N; j++) { + if (liftGrid[i][j] > 0) { + liftGrid[i][j]--; + } + } + } + } + // steer toward the BEST KNOWN lift on the map (argmax over the window): + // works even where the local gradient is blind - the map remembers + { + uint8_t best = SOAR_GRID_LIFT_FLOOR; + int16_t bwi = -1, bwj = -1; + for (int16_t wi = 0; wi < SOAR_GRID_N; wi++) { + const int16_t ai = (gridBaseI + wi) % SOAR_GRID_N; + for (int16_t wj = 0; wj < SOAR_GRID_N; wj++) { + const uint8_t v = liftGrid[ai][(gridBaseJ + wj) % SOAR_GRID_N]; + if (v > best) { + best = v; + bwi = wi; + bwj = wj; + } + } + } + if (bwi >= 0) { + const float tx = gridOriginCm.x + (bwi + 0.5f) * SOAR_GRID_CELL_CM; + const float ty = gridOriginCm.y + (bwj + 0.5f) * SOAR_GRID_CELL_CM; + const float dvx = tx - thermalCentre.x; + const float dvy = ty - thermalCentre.y; + const float dist = calc_length_pythagorean_2D(dvx, dvy); + if (dist > SOAR_GRID_CELL_CM * 0.5f) { + const float k = (soaringConfig()->centreGainPct / 100.0f) * SOAR_CENTRE_GAIN_SCALE; + thermalCentre.x += k * (dvx / dist) * dT; // cm + thermalCentre.y += k * (dvy / dist) * dT; + } + } + } + // the circle itself always rides the wind (exact drift of the column) + thermalCentre.x += getEstimatedWindSpeed(X) * dT; + thermalCentre.y += getEstimatedWindSpeed(Y) * dT; + + // EXPLORATION (contract: "mit jeder Runde etwas daneben fliegen"): each + // completed round the loiter anchor moves a little off the centre in a + // new direction, so the next round samples fresh gradient information + roundAccumRad += omega * dT; + if (roundAccumRad >= 2.0f * M_PIf) { + roundAccumRad -= 2.0f * M_PIf; + exploreAngleRad += SOAR_EXPLORE_ADVANCE_RAD; + } + fpVector3_t anchor = thermalCentre; + const float exploreCm = SOAR_EXPLORE_FRAC * navConfig()->fw.loiter_radius; + anchor.x += exploreCm * cos_approx(exploreAngleRad); + anchor.y += exploreCm * sin_approx(exploreAngleRad); + // keep the loiter anchored on the moving estimate (the POSHOLD + // initialize re-fires per RX cycle and would otherwise re-anchor "here") + navForcedPosholdAssert(&anchor); + + // clamp the estimate to a sane radius around where the climb was found + const float driftX = thermalCentre.x - breachAnchor.x; + const float driftY = thermalCentre.y - breachAnchor.y; + const float drift = calc_length_pythagorean_2D(driftX, driftY); + if (drift > SOAR_CENTRE_MAX_DRIFT_CM) { + const float s = SOAR_CENTRE_MAX_DRIFT_CM / drift; + thermalCentre.x = breachAnchor.x + driftX * s; + thermalCentre.y = breachAnchor.y + driftY * s; + } + +#if defined(SITL_BUILD) + debug[0] = lrintf(netVarioCms); // net vario [cm/s] + debug[1] = lrintf(drift); // |centre - anchor| [cm] + debug[2] = lrintf(driftX); // centre shift north [cm] + debug[3] = lrintf(driftY); // centre shift east [cm] + debug[4] = PWM_RANGE_MIN; // applied throttle: motor OFF [us] +#endif + + // leave the thermal on the CIRCLE-AVERAGED climb (flight contract, + // "mitteln ist gut": one turbulent half-circle never bails out; the time + // constant is one round, derived from bank + speed - no extra parameter), + // or on the altitude band + exitMeanMs += (netVario - exitMeanMs) * MIN(dT / periodS, 1.0f); + if (exitMeanMs < exitMs + || alt > soaringConfig()->altMaxM || alt < soaringConfig()->altMinM) { + thermalling = false; + navForcedPosholdClear(); // hand the loiter back to the pilot / cruise + } +} + +bool soaringActive(void) +{ + return soarActive; +} + +bool soaringThermalling(void) +{ + return thermalling; +} + +void soaringThermalCentre(fpVector3_t *centre) +{ + *centre = thermalCentre; +} + +float soaringNetVarioCms(void) +{ + return netVarioCms; +} + +int16_t soaringThrottleApply(int16_t throttle) +{ + // MOTOR FULLY OFF while thermalling (flight contract): a folding prop + // needs a minimum rpm - idling below it lets the blades flutter and beat + // the fuselage; fully off they fold cleanly against it. The control + // surfaces keep flying the loiter. Normal throttle returns the instant + // the thermal is left or the aircraft sinks below soar_alt_min, both of + // which drop 'thermalling'. + if (thermalling) { + return PWM_RANGE_MIN; + } + return throttle; +} + +#endif // USE_SOARING diff --git a/src/main/flight/soaring.h b/src/main/flight/soaring.h new file mode 100644 index 00000000000..442df58c977 --- /dev/null +++ b/src/main/flight/soaring.h @@ -0,0 +1,75 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include + +#include "common/vector.h" + +#include "config/parameter_group.h" + +// Thermal soaring: motor off, ride the thermal. A net (total-energy) vario +// built from the pitot airspeed and the vertical estimate finds the rising +// air; the loiter circle is centred on the thermal by a sin/cos gradient +// over each turn PLUS a wind-drift shift (the thermal is locked to the air +// mass, so the circle slides with the wind - the piece ArduSoar scales by +// climb/strength without physical basis, done here as a plain wind * dt). +// A pilot-selected SOARING mode (BOXSOARING) activates it; it cuts the +// motor while circling and hands the loiter target back to the nav layer. + +typedef struct soaringConfig_s { + uint16_t varioTriggerCms; // net vario [cm/s] over which cruise -> thermal + uint16_t varioExitCms; // net vario [cm/s] under which thermal -> cruise + uint16_t altMinM; // do not thermal below this altitude [m] + uint16_t altMaxM; // leave the thermal when this is reached [m] + uint8_t bankDeg; // thermalling bank angle [deg] + uint16_t sinkLevelCms; // level-flight sink at the tuning airspeed [cm/s] + uint8_t centreGainPct; // gradient-shift gain [% per turn] +} soaringConfig_t; + +PG_DECLARE(soaringConfig_t, soaringConfig); + +// Call once per main PID loop iteration (after the nav update) +void soaringUpdate(float dT); + +// True while the pilot's SOARING mode is engaged and armed in the air +bool soaringActive(void); + +// True while circling a thermal (motor off, loiter centred on the estimate) +bool soaringThermalling(void); + +// The estimated thermal centre to loiter (earth frame, cm from home). Only +// meaningful while soaringThermalling(); the nav layer drives the forced +// poshold onto it (the wandering-anchor mechanism). +void soaringThermalCentre(fpVector3_t *centre); + +// The net (total-energy compensated) variometer [cm/s], for telemetry/OSD +float soaringNetVarioCms(void); + +// Mixer throttle hook (FW only): idles the motor while circling a thermal so +// the glider soars on the lift - a throttle-to-idle override, not a motor +// stop; returns the throttle unchanged otherwise. +int16_t soaringThrottleApply(int16_t throttle); diff --git a/src/main/flight/thrust_vectoring.c b/src/main/flight/thrust_vectoring.c new file mode 100644 index 00000000000..d4e65d6fa89 --- /dev/null +++ b/src/main/flight/thrust_vectoring.c @@ -0,0 +1,71 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include + +#ifdef USE_THRUST_VECTORING + +#include "common/maths.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/settings.h" + +#include "flight/servos.h" +#include "flight/thrust_vectoring.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(thrustVectoringConfig_t, thrustVectoringConfig, PG_THRUST_VECTORING_CONFIG, 0); + +PG_RESET_TEMPLATE(thrustVectoringConfig_t, thrustVectoringConfig, + .gain = SETTING_TVC_GAIN_DEFAULT, + .thrustComp = SETTING_TVC_THRUST_COMP_DEFAULT, +); + +// Below this thrust fraction the compensation stops growing: vane authority +// is gone anyway and the servos should not flail against the stops +#define TVC_THRUST_COMP_FLOOR 0.25f + +float thrustVectoringGain(float thrustFraction) +{ + const float t = constrainf(thrustFraction, TVC_THRUST_COMP_FLOOR, 1.0f); + const float fullComp = 1.0f / t; // 1 .. 1/floor + const float comp = 1.0f + (fullComp - 1.0f) * (thrustVectoringConfig()->thrustComp / 100.0f); + return (thrustVectoringConfig()->gain / 100.0f) * comp; +} + +void thrustVectoringApplyInputs(int16_t *input, int16_t mixerThrottleCommand) +{ + // Same stabilized commands as the surfaces, but with inverse thrust + // compensation so vectoring vane / tilt motor authority stays + // roughly constant across the throttle range + const float tvcGain = thrustVectoringGain((mixerThrottleCommand - 1000) / 1000.0f); + input[INPUT_TVC_ROLL] = constrain(lrintf(input[INPUT_STABILIZED_ROLL] * tvcGain), -1000, 1000); + input[INPUT_TVC_PITCH] = constrain(lrintf(input[INPUT_STABILIZED_PITCH] * tvcGain), -1000, 1000); + input[INPUT_TVC_YAW] = constrain(lrintf(input[INPUT_STABILIZED_YAW] * tvcGain), -1000, 1000); +} + +#endif // USE_THRUST_VECTORING diff --git a/src/main/flight/thrust_vectoring.h b/src/main/flight/thrust_vectoring.h new file mode 100644 index 00000000000..dca3c095335 --- /dev/null +++ b/src/main/flight/thrust_vectoring.h @@ -0,0 +1,50 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include + +#include "config/parameter_group.h" + +// Thrust vectoring: dedicated servo mixer input sources (INPUT_TVC_*) that +// carry the same stabilized commands as the control surfaces but with a +// thrust dependent gain. The torque a vectoring vane / tilting motor can +// produce scales with thrust, so the deflection is compensated inversely +// (capped at low thrust) to keep the control loop gain roughly constant -- +// full authority in a prop hang, no overcontrol at full power. + +typedef struct thrustVectoringConfig_s { + uint16_t gain; // % overall TVC deflection gain at full thrust + uint8_t thrustComp; // % inverse thrust compensation: 0 = none, 100 = full 1/thrust +} thrustVectoringConfig_t; + +PG_DECLARE(thrustVectoringConfig_t, thrustVectoringConfig); + +// Combined TVC gain for the current thrust fraction [0..1] +float thrustVectoringGain(float thrustFraction); + +// Feed the TVC mixer input rows from the stabilized commands, scaled by +// the thrust dependent gain (servo mixer hook) +void thrustVectoringApplyInputs(int16_t *input, int16_t mixerThrottleCommand); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 4e2bda3157d..54316a7b734 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -341,10 +341,69 @@ void updateEstimatedGPSFix(void) #endif +// Latency-free loss detection, layer 1 of the aerobatic GPS contract: an +// antenna turned away from the sky collapses the C/N0 of the STRONGEST +// satellites together, within fractions of a second - long before the +// receiver's coasted solution degrades or its quality numbers react +// (fixType keeps claiming 3D while the internal filter free-runs; measured +// on the SITL bench: at typical speeds that erroneous coast walks the +// position estimate tens of meters). Detecting the collapse against a slow +// baseline and degrading the fix to NO_FIX right here hands the SAME cycle +// over to the estimated-fix layer below - no timeout latency, and honest +// re-acquisition releases it just as fast. +#define GPS_CNO_COLLAPSE_DB 12 // drop below baseline that declares shading +#define GPS_CNO_RECOVER_DB 6 // release hysteresis +#define GPS_CNO_BASELINE_TAU_S 20.0f // healthy-signal EMA time constant + +static void processCnoGate(void) +{ + static float cnoBaseline = 0.0f; + static timeMs_t lastUpdateMs = 0; + static bool collapsed = false; + + if (!gpsSol.flags.validCno || gpsSol.cnoMean == 0) { + collapsed = false; + cnoBaseline = 0.0f; + return; + } + + const timeMs_t t = millis(); + const float dt = MIN((t - lastUpdateMs) * 0.001f, 1.0f); + lastUpdateMs = t; + + if (gpsSol.fixType != GPS_FIX_3D) { // honest loss needs no help + collapsed = false; + return; + } + + if (cnoBaseline <= 0.0f) { + cnoBaseline = gpsSol.cnoMean; + return; + } + + if (!collapsed) { + // baseline learns only while healthy - it must not follow the collapse down + cnoBaseline += (gpsSol.cnoMean - cnoBaseline) * (dt / GPS_CNO_BASELINE_TAU_S); + collapsed = gpsSol.cnoMean < cnoBaseline - GPS_CNO_COLLAPSE_DB; + } else { + collapsed = gpsSol.cnoMean < cnoBaseline - GPS_CNO_RECOVER_DB; + } + + if (collapsed) { + gpsSol.fixType = GPS_NO_FIX; + gpsSol.hdop = 9999; + gpsSol.numSat = 0; + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; + } +} + void gpsProcessNewDriverData(void) { gpsSol = gpsSolDRV; + processCnoGate(); #ifdef USE_GPS_FIX_ESTIMATION processDisableGPSFix(); updateEstimatedGPSFix(); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index c14db4a7630..c184504fa1d 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -129,10 +129,14 @@ typedef struct gpsSolutionData_s { bool validVelD; bool validEPE; // EPH/EPV values are valid - actual accuracy bool validTime; + bool validCno; // cnoMean carries live signal-strength data } flags; gpsFixType_e fixType; uint8_t numSat; + uint8_t cnoMean; // mean C/N0 of the strongest tracked signals [dBHz]; the + // shading discriminant - collapses BEFORE the receiver's + // solution degrades (0 = no data) gpsLocation_t llh; int16_t velNED[3]; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 4fd0c332a21..017e9f2ea91 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -591,6 +591,40 @@ static uint8_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) return UBX_HW_VERSION_UNKNOWN; } +// Mean C/N0 of the 5 strongest tracked signals - the shading discriminant +// (aerobatic GPS contract, layer 1): an antenna turned away from the sky +// collapses the STRONGEST satellites together, before the receiver's +// solution or its quality numbers degrade. Consumed by the C/N0 gate in +// gps.c (processCnoGate). +static void updateTopCnoMean(void) +{ + uint8_t top[5] = {0}; + for (int i = 0; i < UBLOX_MAX_SIGNALS; i++) { + if (satelites[i].svId == 0xFF) { + continue; + } + uint8_t c = satelites[i].cno; + for (int j = 0; j < 5; j++) { + if (c > top[j]) { + for (int k = 4; k > j; k--) { + top[k] = top[k - 1]; + } + top[j] = c; + break; + } + } + } + int n = 0, sum = 0; + for (int j = 0; j < 5; j++) { + if (top[j]) { + sum += top[j]; + n++; + } + } + gpsSolDRV.cnoMean = n ? (uint8_t)(sum / n) : 0; + gpsSolDRV.flags.validCno = n > 0; +} + static bool gpsParseFrameUBLOX(void) { switch (_msg_id) { @@ -749,6 +783,7 @@ static bool gpsParseFrameUBLOX(void) satelites[i].gnssId = 0xFF; satelites[i].svId = 0xFF; } + updateTopCnoMean(); } break; case MSG_NAV_SIG: @@ -770,6 +805,7 @@ static bool gpsParseFrameUBLOX(void) satelites[i].gnssId = 0xFF; } } + updateTopCnoMean(); } break; case MSG_ACK_ACK: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index e50115d99ed..97e94f081d8 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -132,4 +132,8 @@ #define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start) #define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999) -#define MSP2_INAV_SET_AUX_RC 0x2230 \ No newline at end of file +#define MSP2_INAV_SET_AUX_RC 0x2230 + +#define MSP2_INAV_FIGURE_SEQUENCE 0x2240 +#define MSP2_INAV_SET_FIGURE_SEQUENCE 0x2241 +#define MSP2_INAV_ORIENTATION_HOLD_TEST 0x2242 //in/out (SITL only): level-1 test injection, 8x float32 (q_est wxyz, q_target wxyz) -> 6x float32 (err_deg xyz, rate_target_dps xyz); pure computation diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 67487606da0..78912432a4c 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -57,13 +57,22 @@ typedef enum { } mspPendingSystemRequest_e; #define MSP_PORT_INBUF_SIZE 192 +#ifndef MSP_PORT_OUTBUF_SIZE #ifdef USE_FLASHFS #define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096 #define MSP_PORT_DATAFLASH_INFO_SIZE 16 #define MSP_PORT_OUTBUF_SIZE (MSP_PORT_DATAFLASH_BUFFER_SIZE + MSP_PORT_DATAFLASH_INFO_SIZE) // WARNING! Must fit in stack! +#elif defined(USE_FW_AEROBATICS) +// the FW_AEROBATICS boxes push the full box-name list past 512 bytes +// (~736 B with everything active) - without FLASHFS the reply buffer +// must still hold it or serializeBoxNamesReply() fails and the Modes +// tab breaks (review finding; comfortably stack-safe vs the 4 KB +// FLASHFS variant) +#define MSP_PORT_OUTBUF_SIZE 1024 #else #define MSP_PORT_OUTBUF_SIZE 512 #endif +#endif typedef struct __attribute__((packed)) { uint8_t size; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eda064bd68b..26281e221bf 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -253,6 +253,14 @@ navigationPosControl_t posControl; navSystemStatus_t NAV_Status; static bool landingDetectorIsActive; +#if defined(USE_FW_AEROBATICS) || defined(USE_SOARING) +// Forced-poshold anchor shared by the altitude-floor orbit and thermal +// soaring: consumed by the POSHOLD initialize while the forced hold is +// active (see navForcedPosholdActivateAt). +static fpVector3_t navForcedPosholdAnchor; +static bool navForcedPosholdAnchorValid = false; +#endif + EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; // Blackbox states @@ -1345,6 +1353,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n fpVector3_t targetHoldPos; calculateInitialHoldPosition(&targetHoldPos); +#if defined(USE_FW_AEROBATICS) || defined(USE_SOARING) + // Forced-poshold anchor (floor orbit / soaring thermal): loiter + // the anchored point, not "here". The forced-poshold event + // re-fires every RX cycle and re-runs this initialize - without + // the override the loiter re-anchors on the current position each + // time and circles itself (measured: clean 52 m circle drifting + // 250 m from the anchor). + if (posControl.flags.forcedPosholdActive && navForcedPosholdAnchorValid) { + setDesiredPosition(&navForcedPosholdAnchor, posControl.actualState.yaw, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_SUCCESS; + } +#endif setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); } @@ -4595,8 +4616,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) if (posControl.flags.sendToActive) { return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; } +#endif - +#if defined(USE_GEOZONE) || defined(USE_FW_AEROBATICS) || defined(USE_SOARING) + // geozone avoidance hold, the altitude-floor orbit, or thermal + // soaring - all loitering an anchored point via forced poshold if (posControl.flags.forcedPosholdActive) { return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; } @@ -4761,6 +4785,11 @@ bool navigationPositionEstimateIsHealthy(void) return posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE && STATE(GPS_FIX_HOME); } +bool navIsAltitudeEstimateTrusted(void) +{ + return posControl.flags.estAltStatus >= EST_USABLE; +} + navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || @@ -5209,6 +5238,37 @@ void abortForcedPosHold(void) } #endif +#if defined(USE_FW_AEROBATICS) || defined(USE_SOARING) +/*----------------------------------------------------------- + * Forced position hold on an anchored point - the REAL fixed-wing + * loiter (Daniel: use the loitering machinery, not a hand-rolled orbit). + * Shared by the altitude-floor orbit (breach point) and thermal soaring + * (thermal centre). Reuses the forcedPoshold flag/FSM path the geozone + * built; the anchor is injected in the POSHOLD initialize (the forced + * event re-fires per RX cycle and would otherwise re-anchor "here"). + *-----------------------------------------------------------*/ +void navForcedPosholdActivateAt(const fpVector3_t *pos) +{ + navForcedPosholdAnchor = *pos; + navForcedPosholdAnchorValid = true; + posControl.flags.forcedPosholdActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void navForcedPosholdAssert(const fpVector3_t *pos) +{ + // keep the stored anchor fresh (cheap; the initialize consumes it) + navForcedPosholdAnchor = *pos; +} + +void navForcedPosholdClear(void) +{ + navForcedPosholdAnchorValid = false; + posControl.flags.forcedPosholdActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} +#endif + /*----------------------------------------------------------- * Ability to execute Emergency Landing on external event *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index eb1621e9f8d..25abe9e1aed 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -233,6 +233,15 @@ void abortForcedPosHold(void); #endif +#if defined(USE_FW_AEROBATICS) || defined(USE_SOARING) +// Forced fixed-wing loiter anchored on a point (the real nav loiter - +// wind-corrected, nav_fw_loiter_radius). Shared by the altitude-floor +// orbit (breach point) and thermal soaring (thermal centre estimate). +void navForcedPosholdActivateAt(const fpVector3_t *pos); +void navForcedPosholdAssert(const fpVector3_t *pos); +void navForcedPosholdClear(void); +#endif + #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 #endif @@ -683,6 +692,7 @@ int8_t navigationGetHeadingControlState(void); navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass); bool navigationPositionEstimateIsHealthy(void); bool navIsCalibrationComplete(void); +bool navIsAltitudeEstimateTrusted(void); bool navigationTerrainFollowingEnabled(void); /* Access to estimated position and velocity */ diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 188ab7d25e3..9455f9d4801 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -38,6 +38,7 @@ #include "fc/settings.h" #include "fc/rc_modes.h" +#include "flight/hover_throttle.h" #include "flight/imu.h" #include "io/gps.h" @@ -499,7 +500,24 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) ) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < max_eph_epv)) { - if (posEstimator.gps.epv < max_eph_epv) { + if (posEstimator.gps.epv < max_eph_epv +#ifdef USE_FW_AEROBATICS + // lock-quality gate for the Z axis: aerobatic attitudes shade + // the antenna and the reported epv lags the real degradation. + // The vertical solution degrades first on a thin constellation, + // so GPS altitude requires a MARGIN over the fix threshold + // (gps_min_sats keeps gating the fix/XY as before); below it + // the altitude stays baro-first + && gpsSol.numSat >= gpsConfig()->gpsMinSats + 2 +#endif +#ifdef USE_GPS_FIX_ESTIMATION + // the estimated fix's altitude IS the baro (origin + BaroAlt) - + // routing it through the GPS-Z path would double-count the baro + // at full GPS weight and bypass the ram fade below; Z stays on + // the honest baro path while dead reckoning + && !STATE(GPS_ESTIMATED_FIX) +#endif + ) { newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID; } else { @@ -627,8 +645,58 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); - const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; - const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; + float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; + float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; +#ifdef USE_FW_AEROBATICS + // Aerobatic estimator contract, layer 4: in KNIFE flight and + // fast rolled passes the DYNAMIC PRESSURE reaches the static + // port (ram/venturi) and the baro reads meters off - fade the + // baro toward the IMU-Z integral while the airframe is BOTH + // rolled past ~60 deg AND fast, and hand it back with the + // attitude. Tilt alone must NOT fade (the prop hang is tilted + // 90 deg at zero airspeed - no dynamic pressure, and the hover + // boost below NEEDS the baro); the horizontal estimate speed + // stands in for q, it stays valid on GPS loss via the + // estimated fix. + // TIME-BOUNDED: the IMU-Z integral can only carry the altitude + // for SECONDS (measured: ~24 m drift over a 25 s knife hold vs + // the ~10 m ram error itself) - the fade is strong when the + // shading begins (a normal knife pass is over before the + // integral drifts) and relaxes back to the baro as the lesser + // evil if the attitude persists. + if (STATE(AIRPLANE)) { + static float ramFadeActiveS = 0.0f; + // Ram exposure is the SIDE of the fuselage facing the flow + // (the knife case of the contract) - the wing axis gone + // vertical, |rMat[2][1]|: knife = 1, dive/level/inverted = 0. + // The earlier cosTilt schedule also faded in steep DIVES, + // where the fuselage streams lengthwise and the static + // port sees no dynamic pressure - there the baro is honest + // and cutting it away only costs Z quality for nothing. + const float tiltFactor = scaleRangef(constrainf(fabsf(rMat[2][1]), 0.5f, 0.87f), 0.5f, 0.87f, 0.0f, 1.0f); + const float speedXY = calc_length_pythagorean_2D(posEstimator.est.vel.x, posEstimator.est.vel.y); + const float speedFactor = scaleRangef(constrainf(speedXY, 800.0f, 1200.0f), 800.0f, 1200.0f, 0.0f, 1.0f); + const float ramExposure = tiltFactor * speedFactor; + if (ramExposure > 0.5f) { + ramFadeActiveS += dT; + } else { + ramFadeActiveS = MAX(0.0f, ramFadeActiveS - 4.0f * dT); + } + const float strength = constrainf(1.0f - ramFadeActiveS / 10.0f, 0.0f, 1.0f); + const float wBaroRam = 1.0f - 0.85f * ramExposure * strength; + w_z_baro_p *= wBaroRam; + w_z_baro_v *= wBaroRam; + } +#endif +#ifdef USE_FW_AEROBATICS + // hovering on the prop: the thrust pollutes the accelerometer Z + // and the inertial estimate wanders meters around the truth; the + // baro deserves more trust for as long as the hover throttle + // owns the altitude + if (hoverThrottleIsEngaged()) { + w_z_baro_p = MAX(w_z_baro_p, hoverThrottleConfig()->hoverBaroWeight / 100.0f); + } +#endif ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dT; ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dT; @@ -707,7 +775,26 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y; const float gpsPosResidualMag = calc_length_pythagorean_2D(gpsPosXResidual, gpsPosYResidual); - //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f); + // A soft residual gate (bellCurve of this residual scaling the + // weight to a 0.1 floor) was implemented and A/B-measured here + // against a false-valid receiver model (SITL, 2 s and 5 s + // coasted-fix windows). It does NOT pay in this estimator: + // (a) a short coast (~2 s) drags the estimate <= 1.7 m even + // ungated - the correction bandwidth low-passes the error; + // (b) a long coast (5 s) defeats the gate through the EPE + // machinery: rejected updates still blend eph toward the + // large residual (line below), EST_XY_VALID drops within + // ~2 s and the reset path re-anchors onto the very fix the + // gate was rejecting - same endpoint as no gate (p90 45 m + // vs 48 m); + // (c) the cost is real: with the INERTIAL side wrong and GPS + // honest, recovery crawls at the floor weight (clean-tail + // 6.6 -> 37 m measured). + // The false-valid defence belongs upstream instead: C/N0 + // collapse detection (UBX-NAV-SIG is already parsed) discards + // a shaded fix seconds before the receiver admits the loss, + // and the commanded-figure feed-forward coasts the estimator + // through the maneuver. const float gpsWeightScaler = 1.0f; const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler; diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 2542a352bc1..58a3577048f 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -237,6 +237,8 @@ void printCmdLineOptions(void) fprintf(stderr, "--parity=[Even|None|Odd] Serial receiver parity (default: None).\n"); fprintf(stderr, "--fcproxy Use inav/betaflight FC as a proxy for serial receiver.\n"); fprintf(stderr, "--tcpbaseport=[port] Base TCP port for UART sockets (default: 5760)\n"); + fprintf(stderr, "--lockstep Simulated time advances exactly 1 ms per MSP_SIMULATOR frame\n"); + fprintf(stderr, " (deterministic HITL benches; real time until the first frame)\n"); fprintf(stderr, "--chanmap=[mapstring] Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n"); fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)-,... All numbers must have two digits\n"); fprintf(stderr, " For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n"); @@ -268,6 +270,7 @@ void parseArguments(int argc, char *argv[]) {"parity", required_argument, 0, '4'}, {"fcproxy", no_argument, 0, '5'}, {"tcpbaseport", required_argument, 0, '6'}, + {"lockstep", no_argument, 0, '7'}, {NULL, 0, NULL, 0} }; @@ -353,6 +356,10 @@ void parseArguments(int argc, char *argv[]) case '5': serialFCProxy = true; break; + case '7': + sitlLockstepEnabled = true; + fprintf(stderr, "[SIM] Lockstep: sim time advances 1 ms per MSP_SIMULATOR frame\n"); + break; case '6': { char *endptr = NULL; long basePort = strtol(optarg, &endptr, 10); @@ -387,13 +394,107 @@ void unlockMainPID(void) } // Replacements for system functions -timeUs_t micros(void) { + +// Lockstep (--lockstep): simulated time is driven by the HITL sensor +// injection, one fixed millisecond per MSP_SIMULATOR frame, equidistant and +// independent of host load or wall time. Between frames the clock creeps +// with real time but is CAPPED just below the next tick, so the scheduler +// (and with it the serial task that parses the next frame) keeps running +// while simulated time can never run ahead of the injected sensor data. +// Until the first frame arrives (boot, gyro calibration) time runs on the +// host clock as before. +bool sitlLockstepEnabled = false; +volatile int32_t sitlRxBytesPending = 0; +static bool lockstepActive = false; +static uint64_t lockstepTickTimeUs; // sim time of the last tick (base of the 1 ms grid) +static uint64_t lockstepAnchorSimUs; // sim time the creep window starts from +static uint64_t lockstepAnchorRealUs; // wall clock the creep window starts from +static uint64_t lockstepLastUs; // monotonicity high-water mark +// the anchors are written by the TCP receive thread and read by the main +// thread: unsynchronized 64-bit accesses produced torn values (an anchor +// in the future freezes the clock until the next arrival) +static pthread_mutex_t lockstepMutex = PTHREAD_MUTEX_INITIALIZER; + +static uint64_t realMicros(void) { struct timespec now; clock_gettime(CLOCK_MONOTONIC, &now); return (now.tv_sec - start_time.tv_sec) * 1000000 + (now.tv_nsec - start_time.tv_nsec) / 1000; } +// callers hold lockstepMutex +static uint64_t lockstepMicrosLocked(void) { + // creep window: up to one sub-tick millisecond of real time from the + // last ANCHOR (tick or byte arrival). It keeps the scheduler and the + // serial task alive between frames while simulated time can never + // run more than ~1 ms past the last received data. Every arrival + // re-opens the window (see sitlLockstepRxArrival), so a late frame + // or an interleaved non-simulator request can never starve the + // serial pass that would advance the clock. + const uint64_t nowReal = realMicros(); + uint64_t creepUs = (nowReal > lockstepAnchorRealUs) ? nowReal - lockstepAnchorRealUs : 0; + // while received bytes wait unparsed, the window stays open: the last + // in-window serial pass can otherwise land right at the cap with its + // next execution just beyond it, freezing the clock with work still + // queued (seen as a rare late-flight stall under parallel host load) + if (creepUs > 999 && sitlRxBytesPending <= 0) { + creepUs = 999; + } + uint64_t t = lockstepAnchorSimUs + creepUs; + if (t < lockstepLastUs) { + t = lockstepLastUs; // strictly monotonic across anchors + } else { + lockstepLastUs = t; + } + return t; +} + +timeUs_t micros(void) { + if (lockstepActive) { + pthread_mutex_lock(&lockstepMutex); + const uint64_t t = lockstepMicrosLocked(); + pthread_mutex_unlock(&lockstepMutex); + return t; + } + return realMicros(); +} + +void sitlLockstepTick(void) { + if (!sitlLockstepEnabled) { + return; + } + pthread_mutex_lock(&lockstepMutex); + if (!lockstepActive) { + lockstepTickTimeUs = realMicros(); + lockstepActive = true; + } else { + // ABSOLUTE grid: exactly N milliseconds after N frames, always. + // Creep chains (request bursts between frames) may run micros() + // past this grid point; the monotonicity clamp in micros() then + // holds the OUTPUT flat until the grid catches up. Inheriting the + // high-water mark into the base instead would stretch the grid + // permanently - measured as 14 percent clock inflation against + // the injected sensor stream on a faster-than-realtime bench. + lockstepTickTimeUs += 1000; + } + lockstepAnchorSimUs = lockstepTickTimeUs; + lockstepAnchorRealUs = realMicros(); + pthread_mutex_unlock(&lockstepMutex); +} + +// Called by the TCP receive thread on arriving bytes: restart the creep +// window at the current simulated time so the (frozen) scheduler wakes up +// and the serial task parses what just arrived. +void sitlLockstepRxArrival(void) { + if (!lockstepActive) { + return; + } + pthread_mutex_lock(&lockstepMutex); + lockstepAnchorSimUs = lockstepMicrosLocked(); + lockstepAnchorRealUs = realMicros(); + pthread_mutex_unlock(&lockstepMutex); +} + uint64_t microsISR(void) { return micros(); diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 46ad6d9dad4..56cc43ecc60 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -82,6 +82,14 @@ #define MAX_GEOZONES_IN_CONFIG 63 #define MAX_VERTICES_IN_CONFIG 126 +// The FW_AEROBATICS suite is flash-gated to > 512 KB in common.h; SITL +// has no MCU_FLASH_SIZE, so enable it explicitly here (the bench needs +// it), same as USE_GEOZONE above. +#define USE_FW_AEROBATICS +#define USE_THRUST_VECTORING +#define USE_CRASH_DETECTION +#define USE_SOARING + #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? #undef USE_VCP #undef USE_PPM @@ -200,6 +208,16 @@ typedef enum extern bool lockMainPID(void); extern void unlockMainPID(void); extern void parseArguments(int argc, char *argv[]); +// Lockstep (--lockstep): simulated time advances exactly 1 ms per +// MSP_SIMULATOR frame; sitlLockstepTick() is called from the frame handler, +// sitlLockstepRxArrival() by the TCP receive thread on arriving bytes +extern bool sitlLockstepEnabled; +extern void sitlLockstepTick(void); +extern void sitlLockstepRxArrival(void); +// unparsed received bytes (maintained by the TCP serial driver): while +// nonzero the lockstep creep window stays open so the pending work is +// always reachable by the scheduler +extern volatile int32_t sitlRxBytesPending; extern char *strnstr(const char *s, const char *find, size_t slen); extern int lookupAddress (char *, int, int, struct sockaddr *, socklen_t*); diff --git a/src/main/target/common.h b/src/main/target/common.h index 6990cfd4415..b3ecdb7c442 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -205,6 +205,17 @@ //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) +// FW_AEROBATICS suite (orientation holds, figures, thrust vectoring, +// crash detection): ~13 KB, experimental, runtime-gated by the +// FW_AEROBATICS feature. 512 KB boards keep their flash; SITL enables +// it in its own target.h (no MCU_FLASH_SIZE there). +#define USE_FW_AEROBATICS +#define USE_THRUST_VECTORING +#define USE_CRASH_DETECTION +// Thermal soaring (net-energy vario + wind-shifted thermal centering): +// ~2 KB, experimental, runtime-gated by the SOARING feature. Own PR, +// independent of the aerobatics suite. +#define USE_SOARING #define USE_VTX_FFPV #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index e8214fc8c1f..8ed9218e3ec 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -45,6 +45,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "flight/altitude_floor.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -465,6 +466,24 @@ static void crsfFrameFlightMode(sbuf_t *dst) crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); static uint8_t hrstSent = 0; +#ifdef USE_FW_AEROBATICS + // the radio SPEAKS flight-mode changes (EdgeTX voice): the floor + // announces itself to the pilot (Daniel's telemetry contract). + // Persistent CTCH/ORBT while a recovery owns the aircraft; floor + // armed/disarmed as transient announcements in the HRST idiom (a + // few frames, then back to the normal mode text). + static uint8_t florSent = 0; + static uint8_t flofSent = 0; + static bool floorWasArmed = false; + if (altitudeFloorArmed() && !floorWasArmed) { + florSent = 4; + flofSent = 0; + } else if (!altitudeFloorArmed() && floorWasArmed && ARMING_FLAG(ARMED)) { + flofSent = 4; + florSent = 0; + } + floorWasArmed = altitudeFloorArmed(); +#endif // use same logic as OSD, so telemetry displays same flight text as OSD when armed const char *flightMode = "OK"; @@ -480,6 +499,16 @@ static void crsfFrameFlightMode(sbuf_t *dst) } else if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && hrstSent < 4 && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) { flightMode = "HRST"; hrstSent++; +#ifdef USE_FW_AEROBATICS + } else if (altitudeFloorRecoveryActive()) { + flightMode = altitudeFloorOrbitActive() ? "ORBT" : "CTCH"; + } else if (florSent > 0) { + flightMode = "FLOR"; + florSent--; + } else if (flofSent > 0) { + flightMode = "FLOF"; + flofSent--; +#endif } else if (FLIGHT_MODE(MANUAL_MODE)) { flightMode = "MANU"; #ifdef USE_GEOZONE