From 012d1958962c7db2ae351b6e35056f8f35b895e4 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 08:58:15 +0200 Subject: [PATCH 001/108] Add quaternion based orientation hold for fixed wing (inverted / knife edge / prop hang) New USE_ORIENTATION_HOLD feature: singularity free attitude hold for arbitrary target attitudes, using the existing orientation quaternion and quaternion math. - Error formed in the rotation group (rotation vector of q_target^-1 * q_est), valid for large error angles, shortest path handling at the 180 deg antipode, defined at pitch +/-90 where the Euler based pidLevel() is singular - Heading is always left free via swing/twist decomposition about the earth vertical axis (matches ANGLE mode behaviour in normal flight, free body roll at prop hang) - New boxes INVERTED / KNIFE EDGE LEFT / KNIFE EDGE RIGHT / PROP HANG, airplanes only, priority ANGLE > HORIZON > ORIENTATION HOLD > ANGLEHOLD - Reuses PID_LEVEL P gain, rate limits and PT1 smoothing of pidLevel(), feeds the existing, unchanged rate loop on all three axes; sticks remain live as rate commands --- src/main/CMakeLists.txt | 2 + src/main/fc/fc_core.c | 10 +- src/main/fc/fc_msp_box.c | 16 ++++ src/main/fc/rc_modes.h | 4 + src/main/fc/runtime_config.h | 1 + src/main/flight/orientation_hold.c | 147 +++++++++++++++++++++++++++++ src/main/flight/orientation_hold.h | 54 +++++++++++ src/main/flight/pid.c | 38 ++++++++ src/main/target/common.h | 1 + 9 files changed, 272 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/orientation_hold.c create mode 100644 src/main/flight/orientation_hold.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c243d9215f3..4616d425d60 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -333,6 +333,8 @@ main_sources(COMMON_SRC flight/rate_dynamics.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 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..47a66599e6e 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -87,6 +87,7 @@ #include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" +#include "flight/orientation_hold.h" #include "flight/rate_dynamics.h" #include "flight/failsafe.h" @@ -683,17 +684,24 @@ 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 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_ORIENTATION_HOLD + DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); +#endif if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); +#ifdef USE_ORIENTATION_HOLD + } 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); } diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 65654ccd97b..82b267cfd77 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -109,6 +109,10 @@ 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 = "INVERTED", .permanentId = 69 }, + { .boxId = BOXKNIFELEFT, .boxName = "KNIFE EDGE LEFT", .permanentId = 70 }, + { .boxId = BOXKNIFERIGHT, .boxName = "KNIFE EDGE RIGHT", .permanentId = 71 }, + { .boxId = BOXPROPHANG, .boxName = "PROP HANG", .permanentId = 72 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -283,6 +287,12 @@ void initActiveBoxIds(void) } if (sensors(SENSOR_ACC)) { ADD_ACTIVE_BOX(BOXANGLEHOLD); +#ifdef USE_ORIENTATION_HOLD + ADD_ACTIVE_BOX(BOXINVERTED); + ADD_ACTIVE_BOX(BOXKNIFELEFT); + ADD_ACTIVE_BOX(BOXKNIFERIGHT); + ADD_ACTIVE_BOX(BOXPROPHANG); +#endif } } @@ -449,6 +459,12 @@ 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_ORIENTATION_HOLD + 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); +#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..c84ddb9790f 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -85,6 +85,10 @@ typedef enum { BOXGIMBALRLOCK = 57, BOXGIMBALCENTER = 58, BOXGIMBALHTRK = 59, + BOXINVERTED = 60, + BOXKNIFELEFT = 61, + BOXKNIFERIGHT = 62, + BOXPROPHANG = 63, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index faec9fdef15..428efcae6ac 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; diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c new file mode 100644 index 00000000000..4d1ab7eeaf2 --- /dev/null +++ b/src/main/flight/orientation_hold.c @@ -0,0 +1,147 @@ +/* + * 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_ORIENTATION_HOLD + +#include "common/maths.h" +#include "common/quaternion.h" +#include "common/utils.h" +#include "common/vector.h" + +#include "fc/rc_modes.h" + +#include "flight/imu.h" +#include "flight/orientation_hold.h" + +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 }, +}; + +static const orientationHoldPreset_t * orientationHoldActivePreset(void) +{ + for (unsigned i = 0; i < ARRAYLEN(orientationHoldPresets); i++) { + if (IS_RC_MODE_ACTIVE(orientationHoldPresets[i].box)) { + return &orientationHoldPresets[i]; + } + } + return NULL; +} + +bool orientationHoldIsRequested(void) +{ + return orientationHoldActivePreset() != NULL; +} + +// 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; +} + +void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget) +{ + // Swing-twist decomposition: split qEst into a rotation about the earth + // vertical axis (twist = heading) and the remaining tilt (swing), then + // form the error against the tilt only. Exact for large angles, unlike + // projecting the final rotation vector. + fpQuaternion_t qTwist = { .q0 = qEst->q0, .q1 = 0.0f, .q2 = 0.0f, .q3 = qEst->q3 }; + const float twistNormSq = sq(qTwist.q0) + sq(qTwist.q3); + + fpQuaternion_t qSwing; + if (twistNormSq > 1e-6f) { + const float twistNormInv = 1.0f / sqrtf(twistNormSq); + qTwist.q0 *= twistNormInv; + qTwist.q3 *= twistNormInv; + + // qEst = qTwist (about earth Z) * qSwing => qSwing = qTwist^-1 * qEst + fpQuaternion_t qTwistInv; + quaternionConjugate(&qTwistInv, &qTwist); + quaternionMultiply(&qSwing, &qTwistInv, qEst); + } else { + // Degenerate at 180 deg of twist (e.g. inverted flying the opposite + // heading): treat the full rotation as swing, shortest path handling + // below keeps the error bounded. + qSwing = *qEst; + } + + // Attitude error in the rotation group: qErr = qTarget^-1 * qSwing + fpQuaternion_t qTargetInv, qErr; + quaternionConjugate(&qTargetInv, qTarget); + quaternionMultiply(&qErr, &qTargetInv, &qSwing); + quaternionNormalize(&qErr, &qErr); + + // Shortest path: q and -q encode the same rotation, pick |angle| <= 180 deg + if (qErr.q0 < 0.0f) { + quaternionScale(&qErr, &qErr, -1.0f); + } + + // Rotation vector err = 2 * log(qErr), valid for large error angles + fpAxisAngle_t axisAngle; + quaternionToAxisAngle(&axisAngle, &qErr); + + // Error is "attitude ahead of target", the controller must command the + // opposite rate, matching pidLevel() where error = target - attitude + errDeg->x = -RADIANS_TO_DEGREES(axisAngle.axis.x * axisAngle.angle); + errDeg->y = -RADIANS_TO_DEGREES(axisAngle.axis.y * axisAngle.angle); + errDeg->z = -RADIANS_TO_DEGREES(axisAngle.axis.z * axisAngle.angle); +} + +bool orientationHoldComputeError(fpVector3_t *errDeg) +{ + const orientationHoldPreset_t *preset = orientationHoldActivePreset(); + if (!preset) { + return false; + } + + fpQuaternion_t qTarget; + orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg); + orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget); + return true; +} + +#endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h new file mode 100644 index 00000000000..2b29b3a3f0b --- /dev/null +++ b/src/main/flight/orientation_hold.h @@ -0,0 +1,54 @@ +/* + * 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 "common/quaternion.h" +#include "common/vector.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. + +// 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. +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); + +// Body frame attitude error (deg) for the currently selected target. +// Returns false when no orientation hold box is active. +bool orientationHoldComputeError(fpVector3_t *errDeg); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9beda7ae225..a5c8365bcd2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -44,6 +44,7 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" +#include "flight/orientation_hold.h" #include "flight/rpm_filter.h" #include "flight/kalman.h" #include "flight/smith_predictor.h" @@ -725,6 +726,35 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } } +#ifdef USE_ORIENTATION_HOLD +// 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. Sticks +// remain live as rate commands on top of the stabilisation. +static void pidOrientationHold(pidState_t *pidStates, float dT) +{ + fpVector3_t errDeg; + + if (!orientationHoldComputeError(&errDeg)) { + return; + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + // Same gain and rate limit handling as pidLevel() + float rateTarget = 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); + + if (pidBank()->pid[PID_LEVEL].I) { + // I8[PIDLEVEL] is used as a PT1 cutoff frequency (Hz), same as pidLevel() + rateTarget = pt1FilterApply4(&pidStates[axis].angleFilterState, rateTarget, pidBank()->pid[PID_LEVEL].I, dT); + } + + pidStates[axis].rateTarget = constrainf(pidStates[axis].rateTarget + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + } +} +#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 +1307,14 @@ void FAST_CODE pidController(float dT) const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; angleHoldIsLevel = false; +#ifdef USE_ORIENTATION_HOLD + 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 diff --git a/src/main/target/common.h b/src/main/target/common.h index 6990cfd4415..d1c2dc8635a 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -74,6 +74,7 @@ #define USE_SMITH_PREDICTOR #define USE_RATE_DYNAMICS #define USE_EXTENDED_CMS_MENUS +#define USE_ORIENTATION_HOLD // Allow default rangefinders #define USE_RANGEFINDER From 45f939bc59d4f8506b5792d338110aa2105f600c Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 16:47:19 +0200 Subject: [PATCH 002/108] Replace swing-twist heading removal with reduced attitude error The swing-twist decomposition about earth Z is degenerate for every inverted attitude: w^2 + z^2 vanishes for all headings, so the extracted twist direction is driven by noise. Near roll 180 with a small pitch offset this produced large phantom body-yaw errors (found by the SITL closed-loop bench: 152 deg error for two attitudes 1.1 deg apart). Regulate the direction of the earth vertical in the body frame instead (reduced attitude control): well defined everywhere, heading-free by construction (heading in normal/inverted/knife flight, body roll at prop hang), exact at large angles, deterministic axis choice at the 180 deg antipode. Host convention tests 17/17 (incl. new regressions for the inverted degeneracy), SITL closed loop: all targets, antipode starts and the pitch-90 crossing pass. --- src/main/flight/orientation_hold.c | 106 ++++++++++++++++++----------- 1 file changed, 65 insertions(+), 41 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 4d1ab7eeaf2..d4dd6f7db1a 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -83,52 +83,76 @@ void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float p 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) { - // Swing-twist decomposition: split qEst into a rotation about the earth - // vertical axis (twist = heading) and the remaining tilt (swing), then - // form the error against the tilt only. Exact for large angles, unlike - // projecting the final rotation vector. - fpQuaternion_t qTwist = { .q0 = qEst->q0, .q1 = 0.0f, .q2 = 0.0f, .q3 = qEst->q3 }; - const float twistNormSq = sq(qTwist.q0) + sq(qTwist.q3); - - fpQuaternion_t qSwing; - if (twistNormSq > 1e-6f) { - const float twistNormInv = 1.0f / sqrtf(twistNormSq); - qTwist.q0 *= twistNormInv; - qTwist.q3 *= twistNormInv; - - // qEst = qTwist (about earth Z) * qSwing => qSwing = qTwist^-1 * qEst - fpQuaternion_t qTwistInv; - quaternionConjugate(&qTwistInv, &qTwist); - quaternionMultiply(&qSwing, &qTwistInv, qEst); + // 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. + 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 { - // Degenerate at 180 deg of twist (e.g. inverted flying the opposite - // heading): treat the full rotation as swing, shortest path handling - // below keeps the error bounded. - qSwing = *qEst; + errDeg->x = errDeg->y = errDeg->z = 0.0f; + return; } - // Attitude error in the rotation group: qErr = qTarget^-1 * qSwing - fpQuaternion_t qTargetInv, qErr; - quaternionConjugate(&qTargetInv, qTarget); - quaternionMultiply(&qErr, &qTargetInv, &qSwing); - quaternionNormalize(&qErr, &qErr); - - // Shortest path: q and -q encode the same rotation, pick |angle| <= 180 deg - if (qErr.q0 < 0.0f) { - quaternionScale(&qErr, &qErr, -1.0f); - } - - // Rotation vector err = 2 * log(qErr), valid for large error angles - fpAxisAngle_t axisAngle; - quaternionToAxisAngle(&axisAngle, &qErr); - - // Error is "attitude ahead of target", the controller must command the - // opposite rate, matching pidLevel() where error = target - attitude - errDeg->x = -RADIANS_TO_DEGREES(axisAngle.axis.x * axisAngle.angle); - errDeg->y = -RADIANS_TO_DEGREES(axisAngle.axis.y * axisAngle.angle); - errDeg->z = -RADIANS_TO_DEGREES(axisAngle.axis.z * axisAngle.angle); + // 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); } bool orientationHoldComputeError(fpVector3_t *errDeg) From f7d1b47d8881fa068c0504f1b34ca406954be903 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 17:40:03 +0200 Subject: [PATCH 003/108] Add switchable altitude floor with automatic upright+climb recovery New ALT FLOOR box (permanentId 73): while active and armed above floor + margin once, a predicted floor breach flies an automatic recovery (shortest-path upright + climb pitch via the orientation hold controller) until back above the floor and climbing. Plain switch semantics: box off = off, so the aircraft can land. - Predictive engage z + vz * 3s < floor: the lookahead must cover the Z estimator lag under sustained sink (~vz * 2-3 s with default baro weighting), not just the roll-to-upright time - Arms only after climbing above floor + margin once (switching the box on on the ground never grabs the aircraft during takeoff) - Priority: failsafe/nav auto-ANGLE > floor recovery > pilot modes; inactive in MANUAL passthrough - Settings alt_floor_altitude / alt_floor_margin / alt_floor_climb_pitch (PG_ALTITUDE_FLOOR_CONFIG), new helper navIsAltitudeEstimateTrusted() - SITL closed loop: dive from 67 m at -50 deg pitch caught at 54-61 m (floor 30 m), landing descent with the box off stays untouched --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_core.c | 15 +++- src/main/fc/fc_msp_box.c | 3 + src/main/fc/rc_modes.h | 1 + src/main/fc/settings.yaml | 24 ++++++ src/main/flight/altitude_floor.c | 107 ++++++++++++++++++++++++++ src/main/flight/altitude_floor.h | 53 +++++++++++++ src/main/flight/orientation_hold.c | 17 ++-- src/main/navigation/navigation.c | 5 ++ src/main/navigation/navigation.h | 1 + 11 files changed, 223 insertions(+), 8 deletions(-) create mode 100644 src/main/flight/altitude_floor.c create mode 100644 src/main/flight/altitude_floor.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 4616d425d60..e552dac6839 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -331,6 +331,8 @@ 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/mixer.c flight/mixer.h flight/orientation_hold.c diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 2acb9c8172e..6e060235a10 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -132,7 +132,8 @@ #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_INAV_END PG_ALTITUDE_FLOOR_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 47a66599e6e..36ec82cacc4 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -87,6 +87,7 @@ #include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" +#include "flight/altitude_floor.h" #include "flight/orientation_hold.h" #include "flight/rate_dynamics.h" @@ -684,17 +685,27 @@ 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 > ORIENTATION HOLD > ANGLEHOLD + /* Disable stabilised modes initially, will be enabled as required with priority + * auto ANGLE (failsafe/nav) > ALT FLOOR 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_ORIENTATION_HOLD DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); + altitudeFloorUpdate(); #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_ORIENTATION_HOLD + } else if (STATE(AIRPLANE) && altitudeFloorRecoveryActive()) { + // Automatic floor recovery: upright + climb, overrides the pilot's + // stabilised mode selection until back above the floor + ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); +#endif + } else if (IS_RC_MODE_ACTIVE(BOXANGLE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 82b267cfd77..a2bacc55000 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -113,6 +113,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXKNIFELEFT, .boxName = "KNIFE EDGE LEFT", .permanentId = 70 }, { .boxId = BOXKNIFERIGHT, .boxName = "KNIFE EDGE RIGHT", .permanentId = 71 }, { .boxId = BOXPROPHANG, .boxName = "PROP HANG", .permanentId = 72 }, + { .boxId = BOXALTFLOOR, .boxName = "ALT FLOOR", .permanentId = 73 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -292,6 +293,7 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXKNIFELEFT); ADD_ACTIVE_BOX(BOXKNIFERIGHT); ADD_ACTIVE_BOX(BOXPROPHANG); + ADD_ACTIVE_BOX(BOXALTFLOOR); #endif } } @@ -464,6 +466,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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); #endif #ifdef USE_SERIAL_GIMBAL diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index c84ddb9790f..471433c899e 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -89,6 +89,7 @@ typedef enum { BOXKNIFELEFT = 61, BOXKNIFERIGHT = 62, BOXPROPHANG = 63, + BOXALTFLOOR = 64, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4e8affb0221..f05dd9d352d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4502,3 +4502,27 @@ 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_ORIENTATION_HOLD + members: + - name: alt_floor_altitude + description: "Altitude floor [m above home]. With the ALT FLOOR mode active and armed (climbed above floor + margin once), a predicted floor breach engages an automatic upright + climb recovery. Switch the mode off to land." + default_value: 30 + field: floorAltitude + min: 5 + max: 500 + - name: alt_floor_margin + description: "Margin above the altitude floor [m] to arm the floor after takeoff and to release the recovery" + 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 diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c new file mode 100644 index 00000000000..6f8838cbd9d --- /dev/null +++ b/src/main/flight/altitude_floor.c @@ -0,0 +1,107 @@ +/* + * 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_ORIENTATION_HOLD + +#include "common/axis.h" +#include "common/maths.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/altitude_floor.h" + +#include "navigation/navigation.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, +); + +// How far ahead the sink prediction looks. Must cover the roll-to-upright +// time AND the Z estimator lag: under sustained sink the estimated altitude +// trails the true altitude by roughly vz * estimator time constant (~2-3 s +// with default baro weighting), so a short lookahead catches far too low. +#define ALT_FLOOR_LOOKAHEAD_S 3.0f + +static bool floorArmed = false; // climbed above floor + margin once +static bool floorRecovery = false; + +void altitudeFloorUpdate(void) +{ + if (!IS_RC_MODE_ACTIVE(BOXALTFLOOR) || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE) + || !navIsAltitudeEstimateTrusted()) { + floorArmed = false; + floorRecovery = 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) { + // Predictive engage: catch before the floor, not at it + if (vz < 0.0f && (z + vz * ALT_FLOOR_LOOKAHEAD_S) < floorCm) { + floorRecovery = true; + } + } else { + // Release when back above floor + margin and climbing + if (z > (floorCm + marginCm) && vz > 0.0f) { + floorRecovery = false; + } + } +} + +bool altitudeFloorRecoveryActive(void) +{ + return floorRecovery; +} + +float altitudeFloorRecoveryPitchDeg(void) +{ + return (float)altitudeFloorConfig()->floorClimbPitch; +} + +#endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h new file mode 100644 index 00000000000..1f35703dd18 --- /dev/null +++ b/src/main/flight/altitude_floor.h @@ -0,0 +1,53 @@ +/* + * 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, a predicted floor +// breach 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. 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); + +// Recovery pitch target (deg, nose up) +float altitudeFloorRecoveryPitchDeg(void); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index d4dd6f7db1a..60ca3eb45f8 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -36,6 +36,7 @@ #include "fc/rc_modes.h" +#include "flight/altitude_floor.h" #include "flight/imu.h" #include "flight/orientation_hold.h" @@ -157,13 +158,19 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion bool orientationHoldComputeError(fpVector3_t *errDeg) { - const orientationHoldPreset_t *preset = orientationHoldActivePreset(); - if (!preset) { - return false; + fpQuaternion_t qTarget; + + // Altitude floor recovery overrides any selected preset: upright + climb + if (altitudeFloorRecoveryActive()) { + orientationHoldTargetFromRP(&qTarget, 0.0f, altitudeFloorRecoveryPitchDeg()); + } else { + const orientationHoldPreset_t *preset = orientationHoldActivePreset(); + if (!preset) { + return false; + } + orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg); } - fpQuaternion_t qTarget; - orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg); orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget); return true; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eda064bd68b..641b92aee52 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4761,6 +4761,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) || diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index eb1621e9f8d..3e9b21357ad 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -683,6 +683,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 */ From b295621062ab783ecab2b8dbf01a93023653eb93 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 18:01:43 +0200 Subject: [PATCH 004/108] Add thrust vectoring servo mixer inputs with inverse thrust compensation New INPUT_TVC_ROLL/PITCH/YAW servo mixer sources (61-63): same stabilized commands as the control surfaces, but with a thrust dependent gain. Vectoring vane / tilt motor torque scales with thrust, so the deflection is compensated inversely (capped below 25% thrust) to keep the control loop gain roughly constant -- full authority in a prop hang, no overcontrol at full power. Map TVC servos to these sources instead of statically coupling them to the surface outputs. Settings tvc_gain (overall %, at full thrust) and tvc_thrust_comp (0 = plain coupling, 100 = full 1/thrust), PG_THRUST_VECTORING_CONFIG. SITL verified: TVC/surface deflection ratio 1.00 at full throttle, 3.85 near idle (theoretical cap 4.0). --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/settings.yaml | 18 +++++++++ src/main/flight/servos.c | 13 ++++++ src/main/flight/servos.h | 3 ++ src/main/flight/thrust_vectoring.c | 57 +++++++++++++++++++++++++++ src/main/flight/thrust_vectoring.h | 46 +++++++++++++++++++++ src/main/target/common.h | 1 + 8 files changed, 142 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/thrust_vectoring.c create mode 100644 src/main/flight/thrust_vectoring.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e552dac6839..7099b577530 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -344,6 +344,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/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 6e060235a10..bba0bbd7aca 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -133,7 +133,8 @@ #define PG_GEOZONES 1043 #define PG_GEOZONE_VERTICES 1044 #define PG_ALTITUDE_FLOOR_CONFIG 1045 -#define PG_INAV_END PG_ALTITUDE_FLOOR_CONFIG +#define PG_THRUST_VECTORING_CONFIG 1046 +#define PG_INAV_END PG_THRUST_VECTORING_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f05dd9d352d..024136c3a19 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4526,3 +4526,21 @@ groups: field: floorClimbPitch min: 5 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" + default_value: 100 + field: gain + min: 0 + max: 200 + - 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 diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index e72dd66ea7d..046c454b144 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" @@ -353,6 +354,18 @@ 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 + { + // 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 + 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..6b2872ceb08 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; diff --git a/src/main/flight/thrust_vectoring.c b/src/main/flight/thrust_vectoring.c new file mode 100644 index 00000000000..148ca7b8913 --- /dev/null +++ b/src/main/flight/thrust_vectoring.c @@ -0,0 +1,57 @@ +/* + * 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 + +#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/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; +} + +#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..c44797280af --- /dev/null +++ b/src/main/flight/thrust_vectoring.h @@ -0,0 +1,46 @@ +/* + * 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); diff --git a/src/main/target/common.h b/src/main/target/common.h index d1c2dc8635a..1e2e72f0f15 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -75,6 +75,7 @@ #define USE_RATE_DYNAMICS #define USE_EXTENDED_CMS_MENUS #define USE_ORIENTATION_HOLD +#define USE_THRUST_VECTORING // Allow default rangefinders #define USE_RANGEFINDER From 7d61beca0efdc439ef52fa20c99541d0f8edc770 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 22:18:06 +0200 Subject: [PATCH 005/108] Add per-attitude pitch trim to the orientation hold targets Inverted flight needs a down-elevator bias to hold altitude, knife edge a few degrees of nose above the horizon (doc: fuselage lift). New settings ohold_inverted_pitch_trim / ohold_knife_pitch_trim (deg), applied as the Euler pitch of the hold target before the attitude's roll -- positive is always 'nose above the horizon', in every attitude and for both knife edge sides (PG_ORIENTATION_HOLD_CONFIG). Host tests 18/18 (trim shifts the target exactly); SITL end-to-end: controller output ~0 on the trimmed target, clearly nonzero 10 deg off (I-term-reset measurement with frozen attitude). --- src/main/config/parameter_group_ids.h | 3 ++- src/main/fc/settings.yaml | 18 ++++++++++++++++++ src/main/flight/orientation_hold.c | 21 ++++++++++++++++++++- src/main/flight/orientation_hold.h | 14 ++++++++++++++ 4 files changed, 54 insertions(+), 2 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index bba0bbd7aca..6003661ec40 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -134,7 +134,8 @@ #define PG_GEOZONE_VERTICES 1044 #define PG_ALTITUDE_FLOOR_CONFIG 1045 #define PG_THRUST_VECTORING_CONFIG 1046 -#define PG_INAV_END PG_THRUST_VECTORING_CONFIG +#define PG_ORIENTATION_HOLD_CONFIG 1047 +#define PG_INAV_END PG_ORIENTATION_HOLD_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 024136c3a19..a9f18803a19 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4544,3 +4544,21 @@ groups: field: thrustComp min: 0 max: 100 + + - name: PG_ORIENTATION_HOLD_CONFIG + type: orientationHoldConfig_t + headers: ["flight/orientation_hold.h"] + condition: USE_ORIENTATION_HOLD + members: + - name: ohold_inverted_pitch_trim + description: "Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias)" + default_value: 0 + field: invertedPitchTrim + min: -15 + max: 15 + - name: ohold_knife_pitch_trim + description: "Pitch trim [deg] on the KNIFE EDGE hold targets (both sides), positive = nose above the horizon, held via the rudder" + default_value: 0 + field: knifePitchTrim + min: -15 + max: 15 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 60ca3eb45f8..1ef89387ef6 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -34,12 +34,23 @@ #include "common/utils.h" #include "common/vector.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/rc_modes.h" +#include "fc/settings.h" #include "flight/altitude_floor.h" #include "flight/imu.h" #include "flight/orientation_hold.h" +PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 0); + +PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, + .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT, + .knifePitchTrim = SETTING_OHOLD_KNIFE_PITCH_TRIM_DEFAULT, +); + typedef struct { boxId_e box; float rollDeg; @@ -168,7 +179,15 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) if (!preset) { return false; } - orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg); + // 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 = orientationHoldConfig()->knifePitchTrim; + } + orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim); } orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget); diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 2b29b3a3f0b..bf949fc7d3b 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -25,16 +25,30 @@ #pragma once #include +#include #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 knifePitchTrim; // deg, nose above horizon in knife edge (both sides) +} 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 From 313d54aea05aa236dc070e69de7e64d083958b3f Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 22:30:37 +0200 Subject: [PATCH 006/108] Split knife edge pitch trim per side The body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other knife edge side: the required trim is left/right = shared fuselage-lift part +/- prop part, so one shared value cannot trim both sides. Reversed prop rotation swaps the sides. ohold_knife_pitch_trim -> ohold_knife_left_pitch_trim / ohold_knife_right_pitch_trim. Host tests 19/19 (new per-side check). --- src/main/fc/settings.yaml | 12 +++++++++--- src/main/flight/orientation_hold.c | 9 ++++++--- src/main/flight/orientation_hold.h | 6 ++++-- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a9f18803a19..e018d54f9ca 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4556,9 +4556,15 @@ groups: field: invertedPitchTrim min: -15 max: 15 - - name: ohold_knife_pitch_trim - description: "Pitch trim [deg] on the KNIFE EDGE hold targets (both sides), positive = nose above the horizon, held via the rudder" + - name: ohold_knife_left_pitch_trim + description: "Pitch trim [deg] on the KNIFE EDGE LEFT hold target, positive = nose above the horizon, held via the rudder. Separate per side: the body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other side, so left/right = shared fuselage-lift part +/- prop part. Reversed prop rotation swaps the sides" default_value: 0 - field: knifePitchTrim + field: knifeLeftPitchTrim + min: -15 + max: 15 + - name: ohold_knife_right_pitch_trim + description: "Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the horizon, held via the rudder. See ohold_knife_left_pitch_trim for why the sides differ" + default_value: 0 + field: knifeRightPitchTrim min: -15 max: 15 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 1ef89387ef6..a4291e2b4b0 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -48,7 +48,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT, - .knifePitchTrim = SETTING_OHOLD_KNIFE_PITCH_TRIM_DEFAULT, + .knifeLeftPitchTrim = SETTING_OHOLD_KNIFE_LEFT_PITCH_TRIM_DEFAULT, + .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT, ); typedef struct { @@ -184,8 +185,10 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) float pitchTrim = 0.0f; if (preset->box == BOXINVERTED) { pitchTrim = orientationHoldConfig()->invertedPitchTrim; - } else if (preset->box == BOXKNIFELEFT || preset->box == BOXKNIFERIGHT) { - pitchTrim = orientationHoldConfig()->knifePitchTrim; + } else if (preset->box == BOXKNIFELEFT) { + pitchTrim = orientationHoldConfig()->knifeLeftPitchTrim; + } else if (preset->box == BOXKNIFERIGHT) { + pitchTrim = orientationHoldConfig()->knifeRightPitchTrim; } orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim); } diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index bf949fc7d3b..0817268a463 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -43,8 +43,10 @@ // 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 knifePitchTrim; // deg, nose above horizon in knife edge (both sides) + 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) } orientationHoldConfig_t; PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig); From dad18dbbcda2eeab6285ccf1cdb6e6f6a948a18e Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 6 Jul 2026 09:08:12 +0200 Subject: [PATCH 007/108] Add figure sequencer with altitude assist (roll / loop / 4-point roll) Aerobatic figures as time parameterized orientation-hold targets. The heading-free reduced attitude controller makes figures trivially invariant: no attitude capture needed, a roll always rotates about the current heading, a loop flies in the current heading plane. Boxes FIGURE ROLL / FIGURE LOOP / FIGURE 4PT ROLL (permanentId 74-76): figure starts when the box goes active, holds level when complete, re-arms on release. Altitude assist: a PID on altitude/climb rate adds an earth referenced nose-above-horizon offset to the figure target; the controller distributes it to elevator and rudder as the roll phase demands (the classic slow-roll coordination, for free from the error geometry), blended out with cos(pitch) toward nose-vertical where altitude belongs to the thrust axis. Settings fig_roll_rate / fig_loop_rate / fig_point_dwell / fig_assist_z_gain / fig_assist_vz_gain / fig_assist_max (PG_FIGURE_SEQUENCER_CONFIG). Keep the vz gain low: the climb rate estimate lags and a strong damping term fights fast figures. SITL closed loop: roll and loop complete through inverted; with assist the roll ends 0.4 m from entry altitude vs 4.3 m stuck low without. --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_core.c | 2 + src/main/fc/fc_msp_box.c | 9 ++ src/main/fc/rc_modes.h | 3 + src/main/fc/settings.yaml | 42 ++++++ src/main/flight/figure_sequencer.c | 199 ++++++++++++++++++++++++++ src/main/flight/figure_sequencer.h | 64 +++++++++ src/main/flight/orientation_hold.c | 7 +- 9 files changed, 329 insertions(+), 2 deletions(-) create mode 100644 src/main/flight/figure_sequencer.c create mode 100644 src/main/flight/figure_sequencer.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 7099b577530..433ce97e026 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 diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 6003661ec40..527eb1b3de9 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -135,7 +135,8 @@ #define PG_ALTITUDE_FLOOR_CONFIG 1045 #define PG_THRUST_VECTORING_CONFIG 1046 #define PG_ORIENTATION_HOLD_CONFIG 1047 -#define PG_INAV_END PG_ORIENTATION_HOLD_CONFIG +#define PG_FIGURE_SEQUENCER_CONFIG 1048 +#define PG_INAV_END PG_FIGURE_SEQUENCER_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 36ec82cacc4..1417476afc0 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -88,6 +88,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/altitude_floor.h" +#include "flight/figure_sequencer.h" #include "flight/orientation_hold.h" #include "flight/rate_dynamics.h" @@ -694,6 +695,7 @@ void processRx(timeUs_t currentTimeUs) #ifdef USE_ORIENTATION_HOLD DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); altitudeFloorUpdate(); + figureSequencerUpdate(); #endif if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index a2bacc55000..727d6eda045 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -114,6 +114,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXKNIFERIGHT, .boxName = "KNIFE EDGE RIGHT", .permanentId = 71 }, { .boxId = BOXPROPHANG, .boxName = "PROP HANG", .permanentId = 72 }, { .boxId = BOXALTFLOOR, .boxName = "ALT FLOOR", .permanentId = 73 }, + { .boxId = BOXFIGROLL, .boxName = "FIGURE ROLL", .permanentId = 74 }, + { .boxId = BOXFIGLOOP, .boxName = "FIGURE LOOP", .permanentId = 75 }, + { .boxId = BOXFIGPOINTROLL, .boxName = "FIGURE 4PT ROLL", .permanentId = 76 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -294,6 +297,9 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXKNIFERIGHT); ADD_ACTIVE_BOX(BOXPROPHANG); ADD_ACTIVE_BOX(BOXALTFLOOR); + ADD_ACTIVE_BOX(BOXFIGROLL); + ADD_ACTIVE_BOX(BOXFIGLOOP); + ADD_ACTIVE_BOX(BOXFIGPOINTROLL); #endif } } @@ -467,6 +473,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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(BOXFIGROLL)), BOXFIGROLL); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGLOOP)), BOXFIGLOOP); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGPOINTROLL)), BOXFIGPOINTROLL); #endif #ifdef USE_SERIAL_GIMBAL diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 471433c899e..25cc977f754 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -90,6 +90,9 @@ typedef enum { BOXKNIFERIGHT = 62, BOXPROPHANG = 63, BOXALTFLOOR = 64, + BOXFIGROLL = 65, + BOXFIGLOOP = 66, + BOXFIGPOINTROLL = 67, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e018d54f9ca..2e070f9c1ee 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4568,3 +4568,45 @@ groups: field: knifeRightPitchTrim min: -15 max: 15 + + - name: PG_FIGURE_SEQUENCER_CONFIG + type: figureSequencerConfig_t + headers: ["flight/figure_sequencer.h"] + condition: USE_ORIENTATION_HOLD + 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 diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c new file mode 100644 index 00000000000..63edfc789c1 --- /dev/null +++ b/src/main/flight/figure_sequencer.c @@ -0,0 +1,199 @@ +/* + * 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_ORIENTATION_HOLD + +#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 "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, +); + +typedef enum { + FIGURE_NONE = 0, + FIGURE_ROLL, + FIGURE_LOOP, + FIGURE_POINT_ROLL, +} 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; + +static figureType_e requestedFigure(void) +{ + 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) +static float altitudeAssistDeg(float nosePitchDeg) +{ + if (!navIsAltitudeEstimateTrusted()) { + return 0.0f; + } + + const float zErrM = (startAltitudeCm - 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); + + // cos blend toward nose-vertical + return offset * cos_approx(DEGREES_TO_RADIANS(constrainf(nosePitchDeg, -90.0f, 90.0f))); +} + +void figureSequencerUpdate(void) +{ + const figureType_e req = requestedFigure(); + + if (req == FIGURE_NONE || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE)) { + activeFigure = FIGURE_NONE; + state = FIG_STATE_IDLE; + return; + } + + if (state == FIG_STATE_IDLE || req != activeFigure) { + activeFigure = req; + state = FIG_STATE_RUNNING; + startTimeMs = millis(); + startAltitudeCm = getEstimatedActualPosition(Z); + } + + const float tS = (millis() - startTimeMs) * 0.001f; + float roll = 0.0f; + float pitch = 0.0f; + bool assist = false; + + switch (activeFigure) { + case FIGURE_ROLL: { + const float theta = figureSequencerConfig()->rollRate * tS; + roll = MIN(theta, 360.0f); + assist = true; + if (theta >= 360.0f) { + state = FIG_STATE_DONE; + roll = 0.0f; // 360 == 0, hold level + } + break; + } + + case FIGURE_LOOP: { + const float theta = figureSequencerConfig()->loopRate * tS; + pitch = MIN(theta, 360.0f); + if (theta >= 360.0f) { + state = FIG_STATE_DONE; + pitch = 0.0f; + assist = true; // level again: hold the entry altitude + } + break; + } + + case FIGURE_POINT_ROLL: { + // 4 points: rotate 90 deg at roll rate, dwell, repeat + 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 ? altitudeAssistDeg(pitch) : 0.0f); +} + +bool figureSequencerRequested(void) +{ + return activeFigure != FIGURE_NONE && state != FIG_STATE_IDLE; +} + +void figureSequencerGetTarget(float *rollDeg, float *pitchDeg) +{ + *rollDeg = targetRollDeg; + *pitchDeg = targetPitchDeg; +} + +#endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h new file mode 100644 index 00000000000..56691213901 --- /dev/null +++ b/src/main/flight/figure_sequencer.h @@ -0,0 +1,64 @@ +/* + * 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); + +// 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); + +// Current figure target, valid while requested +void figureSequencerGetTarget(float *rollDeg, float *pitchDeg); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index a4291e2b4b0..cd758d56b0f 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -41,6 +41,7 @@ #include "fc/settings.h" #include "flight/altitude_floor.h" +#include "flight/figure_sequencer.h" #include "flight/imu.h" #include "flight/orientation_hold.h" @@ -79,7 +80,7 @@ static const orientationHoldPreset_t * orientationHoldActivePreset(void) bool orientationHoldIsRequested(void) { - return orientationHoldActivePreset() != NULL; + return figureSequencerRequested() || orientationHoldActivePreset() != NULL; } // Same Euler to quaternion convention as imuComputeQuaternionFromRPY (yaw = 0) @@ -175,6 +176,10 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) // Altitude floor recovery overrides any selected preset: upright + climb if (altitudeFloorRecoveryActive()) { orientationHoldTargetFromRP(&qTarget, 0.0f, altitudeFloorRecoveryPitchDeg()); + } else if (figureSequencerRequested()) { + float figRoll, figPitch; + figureSequencerGetTarget(&figRoll, &figPitch); + orientationHoldTargetFromRP(&qTarget, figRoll, figPitch); } else { const orientationHoldPreset_t *preset = orientationHoldActivePreset(); if (!preset) { From 740af30f00fec944a25decc8eed608dd908207f7 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 6 Jul 2026 10:18:58 +0200 Subject: [PATCH 008/108] Add programmable figure sequences with precondition gates FIGURE SEQ box (permanentId 77) flies a programmable chain of up to 16 segments (PG_FIGURE_SEQUENCE, MSP2_INAV_FIGURE_SEQUENCE 0x2240 / MSP2_INAV_SET_FIGURE_SEQUENCE 0x2241): ROLL/PITCH rotations are cumulative on the running attitude baseline (Immelmann = PITCH +180 then ROLL +180), HOLD holds an absolute attitude, WAIT_ALT gates the chain on reaching a target altitude (wings level, climbing/descending via the assist mechanism), WAIT_TIME dwells. A position wait is reserved -- it needs heading control / nav coupling. Altitude assist fix: the offset now raises the NOSE ELEVATION -- multiplied by cos(pitch), which both blends it out toward nose-vertical and corrects the sign when the accumulated pitch parameter is past +/-90 (base pitch 180 after a half loop acted inverted before). SITL: WAIT_ALT 40m -> Immelmann -> hold plays through with the gate respected (figure starts at 38.6 m) and ends upright. KNOWN ISSUE: the plane noses down ~17 deg for several seconds after the figure before recovering -- looks like slow AHRS recovery after the fast maneuver in the bench sensor model, under investigation. --- src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_msp.c | 33 +++++++ src/main/fc/fc_msp_box.c | 3 + src/main/fc/rc_modes.h | 1 + src/main/flight/figure_sequencer.c | 130 +++++++++++++++++++++++++- src/main/flight/figure_sequencer.h | 29 ++++++ src/main/msp/msp_protocol_v2_inav.h | 5 +- 7 files changed, 197 insertions(+), 7 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 527eb1b3de9..66cdf92c29b 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -136,7 +136,8 @@ #define PG_THRUST_VECTORING_CONFIG 1046 #define PG_ORIENTATION_HOLD_CONFIG 1047 #define PG_FIGURE_SEQUENCER_CONFIG 1048 -#define PG_INAV_END PG_FIGURE_SEQUENCER_CONFIG +#define PG_FIGURE_SEQUENCE 1049 +#define PG_INAV_END PG_FIGURE_SEQUENCE // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3ec3f89f3f0..0a32f92c366 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -75,6 +75,7 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/figure_sequencer.h" #include "flight/imu.h" #include "flight/mixer_profile.h" #include "flight/mixer.h" @@ -565,6 +566,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 0); } break; +#ifdef USE_ORIENTATION_HOLD + 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 +2384,26 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } else return MSP_RESULT_ERROR; break; + +#ifdef USE_ORIENTATION_HOLD + case MSP2_INAV_SET_FIGURE_SEQUENCE: + sbufReadU8Safe(&tmp_u8, src); + if ((dataSize == 9) && (tmp_u8 < MAX_FIGURE_SEQUENCE_SEGMENTS)) { + figureSegment_t *seg = figureSequenceMutable(tmp_u8); + seg->type = sbufReadU8(src); + seg->p1 = sbufReadU16(src); + seg->p2 = sbufReadU16(src); + seg->p3 = sbufReadU16(src); + seg->flags = sbufReadU8(src); + if (seg->type >= FIGSEG_TYPE_COUNT) { + seg->type = FIGSEG_END; + return MSP_RESULT_ERROR; + } + } else + return MSP_RESULT_ERROR; + break; +#endif + #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_SET_LOGIC_CONDITIONS: sbufReadU8Safe(&tmp_u8, src); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 727d6eda045..2e311895f31 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -117,6 +117,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXFIGROLL, .boxName = "FIGURE ROLL", .permanentId = 74 }, { .boxId = BOXFIGLOOP, .boxName = "FIGURE LOOP", .permanentId = 75 }, { .boxId = BOXFIGPOINTROLL, .boxName = "FIGURE 4PT ROLL", .permanentId = 76 }, + { .boxId = BOXFIGSEQ, .boxName = "FIGURE SEQ", .permanentId = 77 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -300,6 +301,7 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXFIGROLL); ADD_ACTIVE_BOX(BOXFIGLOOP); ADD_ACTIVE_BOX(BOXFIGPOINTROLL); + ADD_ACTIVE_BOX(BOXFIGSEQ); #endif } } @@ -476,6 +478,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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); #endif #ifdef USE_SERIAL_GIMBAL diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 25cc977f754..1392e7ed786 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -93,6 +93,7 @@ typedef enum { BOXFIGROLL = 65, BOXFIGLOOP = 66, BOXFIGPOINTROLL = 67, + BOXFIGSEQ = 68, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index 63edfc789c1..abd9bb1cd52 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -56,11 +56,14 @@ PG_RESET_TEMPLATE(figureSequencerConfig_t, figureSequencerConfig, .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 { @@ -76,8 +79,18 @@ static float startAltitudeCm; static float targetRollDeg; static float targetPitchDeg; +// 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 figureType_e requestedFigure(void) { + if (IS_RC_MODE_ACTIVE(BOXFIGSEQ)) { + return FIGURE_SEQUENCE; + } if (IS_RC_MODE_ACTIVE(BOXFIGROLL)) { return FIGURE_ROLL; } @@ -93,21 +106,26 @@ static figureType_e requestedFigure(void) // 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) -static float altitudeAssistDeg(float nosePitchDeg) +static float altitudeAssistDeg(float nosePitchDeg, float refAltCm) { if (!navIsAltitudeEstimateTrusted()) { return 0.0f; } - const float zErrM = (startAltitudeCm - getEstimatedActualPosition(Z)) / 100.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); - // cos blend toward nose-vertical - return offset * cos_approx(DEGREES_TO_RADIANS(constrainf(nosePitchDeg, -90.0f, 90.0f))); + // 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) @@ -125,6 +143,11 @@ void figureSequencerUpdate(void) state = FIG_STATE_RUNNING; startTimeMs = millis(); startAltitudeCm = getEstimatedActualPosition(Z); + seqIndex = 0; + seqSegStartMs = startTimeMs; + seqBaseRoll = 0.0f; + seqBasePitch = 0.0f; + seqSegAltCm = startAltitudeCm; } const float tS = (millis() - startTimeMs) * 0.001f; @@ -155,6 +178,101 @@ void figureSequencerUpdate(void) 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); + const float theta = MIN(figureSequencerConfig()->rollRate * tSeg, 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); + const float theta = MIN(figureSequencerConfig()->loopRate * tSeg, 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; + + default: + segDone = true; + break; + } + + if (!segDone) { + break; + } + seqIndex++; + seqSegStartMs = millis(); + 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 const float rotS = 90.0f / figureSequencerConfig()->rollRate; @@ -182,7 +300,9 @@ void figureSequencerUpdate(void) } targetRollDeg = roll; - targetPitchDeg = pitch + (assist ? altitudeAssistDeg(pitch) : 0.0f); + targetPitchDeg = pitch + (assist + ? altitudeAssistDeg(pitch, activeFigure == FIGURE_SEQUENCE ? seqSegAltCm : startAltitudeCm) + : 0.0f); } bool figureSequencerRequested(void) diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h index 56691213901..dd4b73feafc 100644 --- a/src/main/flight/figure_sequencer.h +++ b/src/main/flight/figure_sequencer.h @@ -54,6 +54,35 @@ typedef struct figureSequencerConfig_s { 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_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); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index e50115d99ed..8247127395c 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -132,4 +132,7 @@ #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 \ No newline at end of file From e91487c0611b5abe6355de283a5a9f4cef1a256d Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 6 Jul 2026 13:46:06 +0200 Subject: [PATCH 009/108] Reset rate-loop I-term on orientation hold target switches The accumulated rate-loop I trims the holding load of the CURRENT attitude (propwash authority in a prop hang, rudder load in knife edge). On a target switch (e.g. prop hang -> knife edge) that charge is wrong for the new attitude and would discharge as a disturbance into the entry. Reset the accumulators exactly once per edge: mode entry, preset/figure/floor source switch, and mode exit (back to the pilot's manual flying). Within a figure (continuous trajectory) the source is stable and the I-term is kept. Together with pid_iterm_limit_percent (default 33%) this bounds the knife-edge saturation windup pragmatically; a direction-aware saturation freeze in the FW rate controller remains a possible follow-up slice. Host tests 20/20 (T17: one reset per edge, none while held); SITL scenarios/sequence/figures regression green. --- src/main/fc/fc_core.c | 6 +++++ src/main/flight/orientation_hold.c | 35 ++++++++++++++++++++++++++++++ src/main/flight/orientation_hold.h | 4 ++++ 3 files changed, 45 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 1417476afc0..2bb000dbcdd 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -720,6 +720,12 @@ void processRx(timeUs_t currentTimeUs) } } +#ifdef USE_ORIENTATION_HOLD + if (!FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { + orientationHoldResetSourceTracking(); + } +#endif + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index cd758d56b0f..46526ffcbe3 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -44,6 +44,7 @@ #include "flight/figure_sequencer.h" #include "flight/imu.h" #include "flight/orientation_hold.h" +#include "flight/pid.h" PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 0); @@ -169,15 +170,48 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion errDeg->z = RADIANS_TO_DEGREES(axis.z * angle); } +// 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) + +static int activeTargetSource = OHOLD_SOURCE_NONE; + +static void orientationHoldCheckSourceSwitch(int source) +{ + if (source != activeTargetSource) { + if (activeTargetSource != OHOLD_SOURCE_NONE || source != OHOLD_SOURCE_NONE) { + pidResetErrorAccumulators(); + } + activeTargetSource = source; + } +} + +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; + } +} + bool orientationHoldComputeError(fpVector3_t *errDeg) { fpQuaternion_t qTarget; // Altitude floor recovery overrides any selected preset: upright + climb if (altitudeFloorRecoveryActive()) { + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FLOOR); orientationHoldTargetFromRP(&qTarget, 0.0f, altitudeFloorRecoveryPitchDeg()); } else if (figureSequencerRequested()) { float figRoll, figPitch; + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE); figureSequencerGetTarget(&figRoll, &figPitch); orientationHoldTargetFromRP(&qTarget, figRoll, figPitch); } else { @@ -185,6 +219,7 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) if (!preset) { 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; diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 0817268a463..6e7fa95a5b4 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -68,3 +68,7 @@ bool orientationHoldIsRequested(void); // Body frame attitude error (deg) for the currently selected target. // Returns false when no orientation hold box is active. bool orientationHoldComputeError(fpVector3_t *errDeg); + +// 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); From 078841769a501a33d3878c985c936c9370557bef Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 6 Jul 2026 19:36:17 +0200 Subject: [PATCH 010/108] Add 3D LOCK mode: attitude lock on stick release New 3D LOCK box (permanentId 78): while the sticks are centered the current attitude (captured through the singularity-free reduced attitude controller, so any attitude incl. knife edge or vertical) is held; stick input flies pure rates with the lock target following the aircraft, and the NEW attitude locks when the sticks center again. Closes the gap to ArduPlane's ACRO attitude lock. Presets/figures/ floor take priority over the lock box. Host tests 21/21 (T18: capture/hold/follow/re-lock edges); SITL: windowed mean attitude drift 0.0 deg over 6 s hold, stick moves the lock by 15 deg, new lock drift 0.8 deg. --- src/main/fc/fc_msp_box.c | 3 +++ src/main/fc/rc_modes.h | 1 + src/main/flight/orientation_hold.c | 22 +++++++++++++++++++++- 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 2e311895f31..70d9cf77e7b 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -118,6 +118,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXFIGLOOP, .boxName = "FIGURE LOOP", .permanentId = 75 }, { .boxId = BOXFIGPOINTROLL, .boxName = "FIGURE 4PT ROLL", .permanentId = 76 }, { .boxId = BOXFIGSEQ, .boxName = "FIGURE SEQ", .permanentId = 77 }, + { .boxId = BOXATTLOCK, .boxName = "3D LOCK", .permanentId = 78 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -302,6 +303,7 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXFIGLOOP); ADD_ACTIVE_BOX(BOXFIGPOINTROLL); ADD_ACTIVE_BOX(BOXFIGSEQ); + ADD_ACTIVE_BOX(BOXATTLOCK); #endif } } @@ -479,6 +481,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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); #endif #ifdef USE_SERIAL_GIMBAL diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 1392e7ed786..87e3130ceab 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -94,6 +94,7 @@ typedef enum { BOXFIGLOOP = 66, BOXFIGPOINTROLL = 67, BOXFIGSEQ = 68, + BOXATTLOCK = 69, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 46526ffcbe3..3465eb3e56f 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -37,6 +37,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/settings.h" @@ -81,7 +82,15 @@ static const orientationHoldPreset_t * orientationHoldActivePreset(void) bool orientationHoldIsRequested(void) { - return figureSequencerRequested() || orientationHoldActivePreset() != NULL; + return figureSequencerRequested() || orientationHoldActivePreset() != NULL + || IS_RC_MODE_ACTIVE(BOXATTLOCK); +} + +static bool orientationHoldSticksDeflected(void) +{ + return ABS(rcCommand[ROLL]) > rcControlsConfig()->deadband + || ABS(rcCommand[PITCH]) > rcControlsConfig()->deadband + || ABS(rcCommand[YAW]) > rcControlsConfig()->yaw_deadband; } // Same Euler to quaternion convention as imuComputeQuaternionFromRPY (yaw = 0) @@ -178,6 +187,7 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion #define OHOLD_SOURCE_NONE (-1) #define OHOLD_SOURCE_FLOOR (-2) #define OHOLD_SOURCE_FIGURE (-3) +#define OHOLD_SOURCE_LOCK (-4) static int activeTargetSource = OHOLD_SOURCE_NONE; @@ -214,6 +224,16 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE); figureSequencerGetTarget(&figRoll, &figPitch); orientationHoldTargetFromRP(&qTarget, figRoll, figPitch); + } else if (orientationHoldActivePreset() == NULL && IS_RC_MODE_ACTIVE(BOXATTLOCK)) { + // 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 + static fpQuaternion_t lockTarget; + if (activeTargetSource != OHOLD_SOURCE_LOCK || orientationHoldSticksDeflected()) { + lockTarget = orientation; + } + orientationHoldCheckSourceSwitch(OHOLD_SOURCE_LOCK); + qTarget = lockTarget; } else { const orientationHoldPreset_t *preset = orientationHoldActivePreset(); if (!preset) { From ef5d3ddce88db6b13b0caac5bd2ff9dd15142854 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 6 Jul 2026 19:53:55 +0200 Subject: [PATCH 011/108] Add MSP2_INAV_ORIENTATION_HOLD_TEST level-1 injection command Doc section 7, Ebene 1: evaluate the orientation hold error function and level gain on injected quaternions (8x float32 in: q_est, q_target wxyz; 6x float32 out: err_deg xyz, rate_target_dps xyz). Pure computation on the target MCU's float32 - no controller, estimator or arming state is touched, deterministic single-step, safe in any build. Lets the singularity checklist run against the real F4/F7 numerics over MSP. SITL: 82 test vectors (signs, yaw invariance, pitch-90 sweep, antipode, exact 180, near-inverted degeneracy regression, denormalized input, random grid) pass with worst float32-vs-float64 deviation 0.0001 deg. --- src/main/fc/fc_msp.c | 37 +++++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 3 ++- 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0a32f92c366..6ede812094a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -77,6 +77,7 @@ #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" @@ -4489,6 +4490,42 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif +#ifdef USE_ORIENTATION_HOLD + 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/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 8247127395c..b9f56c36d1f 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -135,4 +135,5 @@ #define MSP2_INAV_SET_AUX_RC 0x2230 #define MSP2_INAV_FIGURE_SEQUENCE 0x2240 -#define MSP2_INAV_SET_FIGURE_SEQUENCE 0x2241 \ No newline at end of file +#define MSP2_INAV_SET_FIGURE_SEQUENCE 0x2241 +#define MSP2_INAV_ORIENTATION_HOLD_TEST 0x2242 //in/out: level-1 test injection, 8x float32 (q_est wxyz, q_target wxyz) -> 6x float32 (err_deg xyz, rate_target_dps xyz); pure computation \ No newline at end of file From 92d4e61d2d11b061bbb41ed016d37b5a3a2b0cb2 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 6 Jul 2026 20:32:28 +0200 Subject: [PATCH 012/108] Add hover throttle: altitude hold in the prop hang (doc section 5) While PROP HANG is the active hold target and the nose is near the zenith, the thrust carries the weight and a dedicated throttle PID owns the altitude axis: - I-term seeded from the pilot's throttle at engage: learns the model's hover throttle online, no setting needed - Altitude target latches only once the vertical motion has settled (engaging mid pull-up must not freeze a fly-through altitude) - Elevation hysteresis 60/45 deg: the attitude wobble around the hang must not flap the controller (every re-engage would re-capture the target - a ratcheting drift) - Tilt compensated output (vertical thrust component), throttle stick out of the mid deadband hands control back to the pilot - Hooked into the mixer throttle path before scaling, so battery compensation still applies Settings ohold_hover_thr_p/i/d (PG_HOVER_THROTTLE_CONFIG). SITL: hands-free prop hang holds +-2.3 m over 12 s in a thrust-borne plant with motor lag, pilot throttle override climbs away cleanly. --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/settings.yaml | 24 +++++ src/main/flight/hover_throttle.c | 149 ++++++++++++++++++++++++++ src/main/flight/hover_throttle.h | 48 +++++++++ src/main/flight/mixer.c | 6 ++ src/main/flight/orientation_hold.c | 5 + src/main/flight/orientation_hold.h | 4 + 8 files changed, 240 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/hover_throttle.c create mode 100644 src/main/flight/hover_throttle.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 433ce97e026..f584bdcc892 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -335,6 +335,8 @@ main_sources(COMMON_SRC flight/rate_dynamics.h flight/altitude_floor.c flight/altitude_floor.h + flight/hover_throttle.c + flight/hover_throttle.h flight/mixer.c flight/mixer.h flight/orientation_hold.c diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 66cdf92c29b..a5f29774ffa 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -137,7 +137,8 @@ #define PG_ORIENTATION_HOLD_CONFIG 1047 #define PG_FIGURE_SEQUENCER_CONFIG 1048 #define PG_FIGURE_SEQUENCE 1049 -#define PG_INAV_END PG_FIGURE_SEQUENCE +#define PG_HOVER_THROTTLE_CONFIG 1050 +#define PG_INAV_END PG_HOVER_THROTTLE_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2e070f9c1ee..bd1c8e2c76f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4610,3 +4610,27 @@ groups: field: assistMax min: 0 max: 30 + + - name: PG_HOVER_THROTTLE_CONFIG + type: hoverThrottleConfig_t + headers: ["flight/hover_throttle.h"] + condition: USE_ORIENTATION_HOLD + members: + - name: ohold_hover_thr_p + description: "Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage)" + default_value: 25 + field: pGain + min: 0 + max: 100 + - name: ohold_hover_thr_i + description: "Hover throttle I gain [throttle us per m per second]" + default_value: 10 + field: iGain + min: 0 + max: 100 + - name: ohold_hover_thr_d + description: "Hover throttle D gain [throttle us per m/s of climb rate]" + default_value: 30 + field: dGain + min: 0 + max: 100 diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c new file mode 100644 index 00000000000..cd0fa4f4695 --- /dev/null +++ b/src/main/flight/hover_throttle.c @@ -0,0 +1,149 @@ +/* + * 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_ORIENTATION_HOLD + +#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/hover_throttle.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/orientation_hold.h" + +#include "navigation/navigation.h" + +#include "rx/rx.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 0); + +PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, + .pGain = SETTING_OHOLD_HOVER_THR_P_DEFAULT, + .iGain = SETTING_OHOLD_HOVER_THR_I_DEFAULT, + .dGain = SETTING_OHOLD_HOVER_THR_D_DEFAULT, +); + +// 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 + +static bool hoverActive = false; +static bool hoverLatched = false; +static float targetAltCm; +static float iTermUs; +static timeUs_t lastUpdateUs; + +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; + + if (!ARMING_FLAG(ARMED) + || !orientationHoldIsPropHang() + || !navIsAltitudeEstimateTrusted() + || elevDeg < elevGate) { + hoverActive = false; + return pilotThrottle; + } + + const float z = getEstimatedActualPosition(Z); + + // pilot throttle outside the mid deadband: direct control, target follows + if (ABS(pilotThrottle - PWM_RANGE_MIDDLE) > rcControlsConfig()->mid_throttle_deadband) { + 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; + lastUpdateUs = micros(); + } + + 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; + } + } + + const float zErrM = (targetAltCm - z) / 100.0f; + const float climbMs = climbCms / 100.0f; + + iTermUs = constrainf(iTermUs + hoverThrottleConfig()->iGain * zErrM * dT, + getThrottleIdleValue(), getMaxThrottle()); + + // 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 = (hoverThrottleConfig()->pGain * zErrM + - hoverThrottleConfig()->dGain * climbMs) / vertical; + + return constrain(lrintf(iTermUs + correction), getThrottleIdleValue(), getMaxThrottle()); +} + +#endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h new file mode 100644 index 00000000000..fea92fb383d --- /dev/null +++ b/src/main/flight/hover_throttle.h @@ -0,0 +1,48 @@ +/* + * 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. + +typedef struct hoverThrottleConfig_s { + uint8_t pGain; // throttle us per m of altitude error + uint8_t iGain; // throttle us per m per second + uint8_t dGain; // throttle us per m/s of climb rate +} 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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..4a9a7558dbf 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -46,6 +46,7 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/hover_throttle.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -587,7 +588,12 @@ void FAST_CODE mixTable(void) } #endif } else { +#ifdef USE_ORIENTATION_HOLD + // hover throttle owns the altitude axis while PROP HANG is held + mixerThrottleCommand = hoverThrottleApply(rcCommand[THROTTLE]); +#else mixerThrottleCommand = rcCommand[THROTTLE]; +#endif throttleRangeMin = throttleIdleValue; throttleRangeMax = getMaxThrottle(); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 3465eb3e56f..7c87770eb71 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -201,6 +201,11 @@ static void orientationHoldCheckSourceSwitch(int source) } } +bool orientationHoldIsPropHang(void) +{ + return activeTargetSource == BOXPROPHANG; +} + void orientationHoldResetSourceTracking(void) { // mode left: also reset, the attitude's holding-load trim in the I-term diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 6e7fa95a5b4..642ea46c446 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -72,3 +72,7 @@ bool orientationHoldComputeError(fpVector3_t *errDeg); // 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); From b0a70318d028a7df2fe0194dfc35f5a24f1c6307 Mon Sep 17 00:00:00 2001 From: pdani Date: Tue, 7 Jul 2026 09:09:56 +0200 Subject: [PATCH 013/108] Add IMPULSE (post-stall entry) and WAIT_POS (containment) segments FIGSEG_IMPULSE: open-loop full-rate kick (p1 pitch %, p2 yaw %, p3 ms) for snap/spin entries; the rate loop saturates the surfaces, the next segment (or the level hold) catches the resulting attitude shortest path. SITL: 193 deg/s peak, caught wings-level 1.1 deg after. FIGSEG_WAIT_POS: airspace containment - bank toward HOME (course loop, 0.8 deg bank per deg of course error, capped at p2) until GPS_distanceToHome < p1. The coordinated turn rates are fed forward via the existing pidTurnAssistant (fw_reference_airspeed), otherwise the heading-free hold regulates the physical turn yaw rate to zero and the aircraft never turns. Holds level while no home fix exists. SITL: turn-in and approach verified (course 331->188 deg coordinated, distance 315->137 m closing); the full closed-loop containment test needs a consistent turn/heading plant model in the bench first (the bench plant's turn kinematics and the AHRS/COG heading chain disagree) - tracked as a bench issue, not firmware. --- src/main/flight/figure_sequencer.c | 69 ++++++++++++++++++++++++++++++ src/main/flight/figure_sequencer.h | 13 ++++++ src/main/flight/pid.c | 24 +++++++++++ 3 files changed, 106 insertions(+) diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index abd9bb1cd52..83378e0e748 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -42,6 +42,7 @@ #include "fc/settings.h" #include "flight/figure_sequencer.h" +#include "flight/imu.h" #include "navigation/navigation.h" @@ -85,6 +86,10 @@ 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 figureType_e requestedFigure(void) { @@ -154,6 +159,8 @@ void figureSequencerUpdate(void) float roll = 0.0f; float pitch = 0.0f; bool assist = false; + seqImpulseActive = false; // recomputed below while an IMPULSE runs + seqTurnCoordination = false; switch (activeFigure) { case FIGURE_ROLL: { @@ -251,6 +258,48 @@ void figureSequencerUpdate(void) 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_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; @@ -310,6 +359,26 @@ bool figureSequencerRequested(void) return activeFigure != FIGURE_NONE && state != FIG_STATE_IDLE; } +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; +} + void figureSequencerGetTarget(float *rollDeg, float *pitchDeg) { *rollDeg = targetRollDeg; diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h index dd4b73feafc..02c81657aa0 100644 --- a/src/main/flight/figure_sequencer.h +++ b/src/main/flight/figure_sequencer.h @@ -68,6 +68,11 @@ typedef enum { 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_TYPE_COUNT } figureSegmentType_e; @@ -91,3 +96,11 @@ bool figureSequencerRequested(void); // Current figure target, valid while requested void figureSequencerGetTarget(float *rollDeg, float *pitchDeg); + +// 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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a5c8365bcd2..e89daede2ac 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -43,6 +43,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/figure_sequencer.h" #include "flight/mixer_profile.h" #include "flight/orientation_hold.h" #include "flight/rpm_filter.h" @@ -735,6 +736,18 @@ static void pidOrientationHold(pidState_t *pidStates, 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++) { + pidStates[axis].rateTarget = constrainf( + impulseNorm[axis] * currentControlProfile->stabilized.rates[axis] * 10.0f, + -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + } + return; + } + if (!orientationHoldComputeError(&errDeg)) { return; } @@ -1344,6 +1357,17 @@ void FAST_CODE pidController(float dT) pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } +#ifdef USE_ORIENTATION_HOLD + else if (FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { + // WAIT_POS banks toward home: 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 + float bankDeg; + if (figureSequencerGetTurnBank(&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)) { From c859383daed6dc65f567e804c81b413ad25806b8 Mon Sep 17 00:00:00 2001 From: pdani Date: Tue, 7 Jul 2026 10:44:34 +0200 Subject: [PATCH 014/108] Fix MSP_BOXNAMES overflow: shorter box names + SITL MSP out-buffer Found by driving the Configurator against SITL: with the 10 new boxes (and all NAV boxes active once FEATURE_GPS is on) the active box-name list exceeds MSP_PORT_OUTBUF_SIZE, serializeBoxNamesReply() returns an MSP error and the Configurator aborts the connect ('No configuration received'). Real F4/F7 targets have the 4 KB FLASHFS buffer; only no-FLASHFS targets (SITL) sit at 512. - Shorten the new box names (INVERT, KNIFE L/R, P-HANG, FLOOR, F ROLL/LOOP/4PT/SEQ, 3DLOCK) - Guard MSP_PORT_OUTBUF_SIZE with #ifndef and override to 1024 on SITL --- src/main/fc/fc_msp_box.c | 20 ++++++++++---------- src/main/msp/msp_serial.h | 2 ++ src/main/target/SITL/target.h | 1 + 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 70d9cf77e7b..e86cff8ad36 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -109,16 +109,16 @@ 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 = "INVERTED", .permanentId = 69 }, - { .boxId = BOXKNIFELEFT, .boxName = "KNIFE EDGE LEFT", .permanentId = 70 }, - { .boxId = BOXKNIFERIGHT, .boxName = "KNIFE EDGE RIGHT", .permanentId = 71 }, - { .boxId = BOXPROPHANG, .boxName = "PROP HANG", .permanentId = 72 }, - { .boxId = BOXALTFLOOR, .boxName = "ALT FLOOR", .permanentId = 73 }, - { .boxId = BOXFIGROLL, .boxName = "FIGURE ROLL", .permanentId = 74 }, - { .boxId = BOXFIGLOOP, .boxName = "FIGURE LOOP", .permanentId = 75 }, - { .boxId = BOXFIGPOINTROLL, .boxName = "FIGURE 4PT ROLL", .permanentId = 76 }, - { .boxId = BOXFIGSEQ, .boxName = "FIGURE SEQ", .permanentId = 77 }, - { .boxId = BOXATTLOCK, .boxName = "3D LOCK", .permanentId = 78 }, + { .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 = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 67487606da0..5daa102d8ce 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -57,6 +57,7 @@ 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 @@ -64,6 +65,7 @@ typedef enum { #else #define MSP_PORT_OUTBUF_SIZE 512 #endif +#endif typedef struct __attribute__((packed)) { uint8_t size; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 46ad6d9dad4..eb4b97c7bfd 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -63,6 +63,7 @@ #define USE_MAG #define USE_BARO #define USE_PITOT_FAKE +#define MSP_PORT_OUTBUF_SIZE 1024 // no FLASHFS on SITL; the full box-name list exceeds 512 #define USE_IMU_FAKE #define USE_FAKE_BARO #define USE_FAKE_MAG From 0950436056716897ee0edcf81be530c3ed279627 Mon Sep 17 00:00:00 2001 From: pdani Date: Wed, 8 Jul 2026 08:04:47 +0200 Subject: [PATCH 015/108] Keep pidOrientationHold out of ITCM (NOINLINE) The static function has a single call site inside the FAST_CODE pidController and was inlined into .tcm_code, overflowing the 16 KB ITCM_RAM on OMNIBUSF7/V2 by 424 bytes. Same convention as pidApplyFixedWingRateController. --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e89daede2ac..0bd0307d9d9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -732,7 +732,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam // knife edge, prop hang). Works on all three body axes and stays defined at // pitch = +/-90 deg where the Euler based pidLevel() is singular. Sticks // remain live as rate commands on top of the stabilisation. -static void pidOrientationHold(pidState_t *pidStates, float dT) +static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) { fpVector3_t errDeg; From 5a43dcac0ce799b2e04a05c3fc82e692016ee9ee Mon Sep 17 00:00:00 2001 From: pdani Date: Wed, 8 Jul 2026 08:04:47 +0200 Subject: [PATCH 016/108] docs: regenerate Settings.md (update_cli_docs.py) --- docs/Settings.md | 170 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 170 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 3d7351dc371..a0dcdccfa11 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), a predicted floor breach engages an automatic upright + climb recovery. Switch the 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] to arm the floor after takeoff and to release the recovery + +| 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_hover_thr_d + +Hover throttle D gain [throttle us per m/s of climb rate] + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 100 | + +--- + +### ohold_hover_thr_i + +Hover throttle I gain [throttle us per m per second] + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 100 | + +--- + +### ohold_hover_thr_p + +Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage) + +| Default | Min | Max | +| --- | --- | --- | +| 25 | 0 | 100 | + +--- + +### ohold_inverted_pitch_trim + +Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -15 | 15 | + +--- + +### ohold_knife_left_pitch_trim + +Pitch trim [deg] on the KNIFE EDGE LEFT hold target, positive = nose above the horizon, held via the rudder. Separate per side: the body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other side, so left/right = shared fuselage-lift part +/- prop part. Reversed prop rotation swaps the sides + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -15 | 15 | + +--- + +### ohold_knife_right_pitch_trim + +Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the horizon, held via the rudder. See ohold_knife_left_pitch_trim for why the sides differ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -15 | 15 | + +--- + ### opflow_hardware Selection of OPFLOW hardware. @@ -6522,6 +6672,26 @@ Turtle mode power factor --- +### tvc_gain + +Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### 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`. From ffebe993e670efc741278a417123b6def70a8057 Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 10 Jul 2026 20:53:47 +0200 Subject: [PATCH 017/108] Orientation holds: active altitude assist (shared with figure sequencer) The plain attitude holds (INVERT, KNIFE L/R) only had static pitch trims; altitude was an unregulated aerodynamic equilibrium that happened to look stable on a well-trimmed symmetric airframe and drifted 15-40 m otherwise. Export the figure sequencer's altitude assist and apply it in the hold preset path, referenced to the altitude captured when the hold engages. The internal cos-blend fades it out toward nose-vertical, so the prop hang stays owned by the hover throttle controller. JSBSim closed loop, 22 s holds: inverted returns to entry altitude (-0.1 m end drift, was +15..40 m); knife edge holds within 0.7 m (was 5.8 m equilibrium). --- src/main/flight/figure_sequencer.c | 4 ++-- src/main/flight/figure_sequencer.h | 5 +++++ src/main/flight/orientation_hold.c | 14 +++++++++++++- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index 83378e0e748..a5bc754e399 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -111,7 +111,7 @@ static figureType_e requestedFigure(void) // 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) -static float altitudeAssistDeg(float nosePitchDeg, float refAltCm) +float figureAltitudeAssistDeg(float nosePitchDeg, float refAltCm) { if (!navIsAltitudeEstimateTrusted()) { return 0.0f; @@ -350,7 +350,7 @@ void figureSequencerUpdate(void) targetRollDeg = roll; targetPitchDeg = pitch + (assist - ? altitudeAssistDeg(pitch, activeFigure == FIGURE_SEQUENCE ? seqSegAltCm : startAltitudeCm) + ? figureAltitudeAssistDeg(pitch, activeFigure == FIGURE_SEQUENCE ? seqSegAltCm : startAltitudeCm) : 0.0f); } diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h index 02c81657aa0..22be828a5a8 100644 --- a/src/main/flight/figure_sequencer.h +++ b/src/main/flight/figure_sequencer.h @@ -97,6 +97,11 @@ bool figureSequencerRequested(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]); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 7c87770eb71..1730b646b94 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -43,6 +43,8 @@ #include "flight/altitude_floor.h" #include "flight/figure_sequencer.h" + +#include "navigation/navigation.h" #include "flight/imu.h" #include "flight/orientation_hold.h" #include "flight/pid.h" @@ -191,6 +193,8 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion static int activeTargetSource = OHOLD_SOURCE_NONE; + +static float holdRefAltCm = 0.0f; // altitude assist reference, captured at hold entry static void orientationHoldCheckSourceSwitch(int source) { if (source != activeTargetSource) { @@ -198,6 +202,9 @@ static void orientationHoldCheckSourceSwitch(int source) pidResetErrorAccumulators(); } activeTargetSource = source; + // capture the altitude reference for the hold altitude assist at the + // moment the target engages (same pattern as the figure sequencer) + holdRefAltCm = getEstimatedActualPosition(Z); } } @@ -255,7 +262,12 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) } else if (preset->box == BOXKNIFERIGHT) { pitchTrim = orientationHoldConfig()->knifeRightPitchTrim; } - orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim); + // 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. + const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); + orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); } orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget); From 90b49186c008244eb22f450019a10e04c0eb7e08 Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 10 Jul 2026 21:37:10 +0200 Subject: [PATCH 018/108] Orientation holds: engage the altitude assist only once the attitude is captured During the entry the transient altitude error deflected the hold target: a knife-edge entry could stall at half the bank (target pushed by up to assist_max while rolling in) and only creep to the preset over tens of seconds. Gate the assist on a small attitude error (<25 deg) and keep the altitude reference tracking while still capturing, so the hold locks the altitude where the attitude settles, not where the switch flipped. JSBSim closed loop: knife L now captures -90 within 3 s and holds 2.0 m altitude span; inverted through a 3 m/s gust: 2.3 m span, ~0 end drift. --- src/main/flight/orientation_hold.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 1730b646b94..51224aed815 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -266,8 +266,22 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) // 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. - const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); - orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); + // 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. + orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim); + fpVector3_t entryErr; + orientationHoldComputeAttitudeError(&entryErr, &orientation, &qTarget); + if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f) { + const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); + orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); + } else { + // still capturing: keep the altitude reference tracking so the + // assist later holds the altitude where the attitude settled, + // not where the switch was flipped + holdRefAltCm = getEstimatedActualPosition(Z); + } } orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget); From 44b7a4d43b1890ee01e3396b9f553c232eedc092 Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 10 Jul 2026 22:26:17 +0200 Subject: [PATCH 019/108] Orientation holds: roll like a pilot into near-antipodal targets Engaging inverted from level is a near-antipodal tilt error: the shortest-rotation cross product barely rises above numerical noise, so the rotation axis -- and with it the whole entry path -- was an arbitrary mix of roll and yaw. Seen as a reproducible ~65 deg heading swing while rolling in. For tilt errors beyond ~150 deg prefer the body X axis (projected orthogonal to the target up, sign kept continuous with the cross product), falling back to the shortest rotation when body X is parallel to the target up (prop-hang entry from a dive). JSBSim closed loop: inverted entry heading swing -1 deg (was +65), knife entries unchanged. --- src/main/flight/orientation_hold.c | 32 +++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 51224aed815..6fc200e4d9e 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -147,7 +147,37 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion const float angle = atan2_approx(crossNorm, dot); fpVector3_t axis; - if (crossNorm > 1e-6f) { + if (dot < -0.87f) { + // Near-antipodal (tilt error > ~150 deg, e.g. engaging inverted from + // level): the cross product barely rises above noise, so the picked + // axis -- and with it 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: roll about body X. Use the body X axis projected + // orthogonal to the target up; keep the sign continuous with the + // cross product once that becomes meaningful. + axis.x = 1.0f - upTarget.x * upTarget.x; + axis.y = -upTarget.x * upTarget.y; + axis.z = -upTarget.x * upTarget.z; + const float prefNorm = fast_fsqrtf(sq(axis.x) + sq(axis.y) + sq(axis.z)); + if (prefNorm > 1e-3f) { + float s = 1.0f; + if (crossNorm > 1e-3f + && (axis.x * cross.x + axis.y * cross.y + axis.z * cross.z) < 0.0f) { + s = -1.0f; + } + axis.x = s * axis.x / prefNorm; + axis.y = s * axis.y / prefNorm; + axis.z = s * axis.z / prefNorm; + } else if (crossNorm > 1e-6f) { + // body X parallel to the target up (entering prop hang from a + // dive): fall back to the shortest-rotation axis + axis.x = cross.x / crossNorm; + axis.y = cross.y / crossNorm; + axis.z = cross.z / crossNorm; + } else { + axis.x = 0.0f; axis.y = 1.0f; axis.z = 0.0f; + } + } else if (crossNorm > 1e-6f) { axis.x = cross.x / crossNorm; axis.y = cross.y / crossNorm; axis.z = cross.z / crossNorm; From 9317423506a23bcb1bfb3df0bf63572cb5e6b374 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 08:17:28 +0200 Subject: [PATCH 020/108] Orientation holds: continuous axis blend instead of hard antipodal threshold Replace the dot < -0.87 if/else with a linear ramp (tilt error 120..150 deg) blending from the shortest-rotation cross product to the body-X preference. Removes the chattering risk when the tilt error dwells at the former threshold; behaviour at the endpoints is unchanged. JSBSim: inverted entry swing +0 deg, spans inverted 1.7 m / knife 1.9 m / hang 4.3 m. --- src/main/flight/orientation_hold.c | 61 +++++++++++++++++------------- 1 file changed, 34 insertions(+), 27 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 6fc200e4d9e..b71667417e5 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -147,37 +147,44 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion const float angle = atan2_approx(crossNorm, dot); fpVector3_t axis; - if (dot < -0.87f) { - // Near-antipodal (tilt error > ~150 deg, e.g. engaging inverted from - // level): the cross product barely rises above noise, so the picked - // axis -- and with it 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: roll about body X. Use the body X axis projected - // orthogonal to the target up; keep the sign continuous with the - // cross product once that becomes meaningful. - axis.x = 1.0f - upTarget.x * upTarget.x; - axis.y = -upTarget.x * upTarget.y; - axis.z = -upTarget.x * upTarget.z; - const float prefNorm = fast_fsqrtf(sq(axis.x) + sq(axis.y) + sq(axis.z)); + // Toward the antipode (engaging inverted from level) the cross product + // barely rises above noise, so the shortest-rotation axis -- and with it + // the whole 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 axis CONTINUOUSLY from the cross product + // (tilt error <= 120 deg) to body X projected orthogonal to the target + // up (>= 150 deg). The ramp avoids chattering at a hard threshold. + const float wPref = constrainf((-dot - 0.5f) / 0.37f, 0.0f, 1.0f); + if (wPref > 0.0f) { + fpVector3_t pref = { .v = { + 1.0f - upTarget.x * upTarget.x, + -upTarget.x * upTarget.y, + -upTarget.x * upTarget.z, + }}; + const float prefNorm = fast_fsqrtf(sq(pref.x) + sq(pref.y) + sq(pref.z)); if (prefNorm > 1e-3f) { - float s = 1.0f; + float s = 1.0f / prefNorm; if (crossNorm > 1e-3f - && (axis.x * cross.x + axis.y * cross.y + axis.z * cross.z) < 0.0f) { - s = -1.0f; + && (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; + errDeg->x = RADIANS_TO_DEGREES(angle) * axis.x; + errDeg->y = RADIANS_TO_DEGREES(angle) * axis.y; + errDeg->z = RADIANS_TO_DEGREES(angle) * axis.z; + return; } - axis.x = s * axis.x / prefNorm; - axis.y = s * axis.y / prefNorm; - axis.z = s * axis.z / prefNorm; - } else if (crossNorm > 1e-6f) { - // body X parallel to the target up (entering prop hang from a - // dive): fall back to the shortest-rotation axis - axis.x = cross.x / crossNorm; - axis.y = cross.y / crossNorm; - axis.z = cross.z / crossNorm; - } else { - axis.x = 0.0f; axis.y = 1.0f; axis.z = 0.0f; } - } else if (crossNorm > 1e-6f) { + // body X parallel to the target up (prop hang entry from a dive): + // fall through to the shortest rotation / deterministic seed below + } + if (crossNorm > 1e-6f) { axis.x = cross.x / crossNorm; axis.y = cross.y / crossNorm; axis.z = cross.z / crossNorm; From 599575c2235e72be86c44d7aa9264fd5594fad1e Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 15:13:08 +0200 Subject: [PATCH 021/108] Orientation holds: persistent slewed target quaternion (v2 core) The hold target becomes a persistent attitude quaternion, seeded on the actual attitude when a source engages and slewed toward the requested attitude at fig_roll_rate instead of stepping there. The regulator error therefore stays small at all times and the entry path is an explicit target trajectory (rolling like a pilot into near-antipodal targets moves from the error computation into the slew axis preference). The error function returns to the pure shortest-tilt rotation; the target's twist follows the actual attitude every cycle (free yaw axis, compliance groundwork for held-twist sources). Figure IMPULSE segments re-seed the target so the catch slews from where the spin ends. Floor recovery keeps tracking directly, safety before entry aesthetics. Bench: level1 numerics 82/82 against the float64 mirror; JSBSim suite green (roll 1.5 m / knife L+R 2.1 m / inverted 2.6 m spans, hang holds 6.2 m after the now energy-conserving pull, floor + spin unchanged); inverted entry heading swing 1.7 deg. --- src/main/flight/orientation_hold.c | 213 ++++++++++++++++++++++------- src/main/flight/orientation_hold.h | 16 ++- src/main/flight/pid.c | 5 +- 3 files changed, 178 insertions(+), 56 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index b71667417e5..4e40ecd4bf8 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -129,7 +129,10 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion // 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. + // 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); @@ -147,43 +150,6 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion const float angle = atan2_approx(crossNorm, dot); fpVector3_t axis; - // Toward the antipode (engaging inverted from level) the cross product - // barely rises above noise, so the shortest-rotation axis -- and with it - // the whole 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 axis CONTINUOUSLY from the cross product - // (tilt error <= 120 deg) to body X projected orthogonal to the target - // up (>= 150 deg). The ramp avoids chattering at a hard threshold. - const float wPref = constrainf((-dot - 0.5f) / 0.37f, 0.0f, 1.0f); - if (wPref > 0.0f) { - fpVector3_t pref = { .v = { - 1.0f - upTarget.x * upTarget.x, - -upTarget.x * upTarget.y, - -upTarget.x * upTarget.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; - errDeg->x = RADIANS_TO_DEGREES(angle) * axis.x; - errDeg->y = RADIANS_TO_DEGREES(angle) * axis.y; - errDeg->z = RADIANS_TO_DEGREES(angle) * axis.z; - return; - } - } - // body X parallel to the target up (prop hang entry from a dive): - // fall through to the shortest rotation / deterministic seed below - } if (crossNorm > 1e-6f) { axis.x = cross.x / crossNorm; axis.y = cross.y / crossNorm; @@ -218,6 +184,124 @@ void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion 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; +} + +// 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. Held-twist sources (course hold bridging) will skip +// this re-anchoring and feed the full error instead. +static void orientationHoldRegulate(fpVector3_t *errDeg) +{ + orientationHoldComputeAttitudeError(errDeg, &orientation, &qSollState); + + fpQuaternion_t qErr; + quatFromRotVecDeg(&qErr, errDeg); + quaternionMultiply(&qSollState, &orientation, &qErr); + quaternionNormalize(&qSollState, &qSollState); +} + // 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 @@ -242,6 +326,9 @@ static void orientationHoldCheckSourceSwitch(int source) // 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; } } @@ -250,6 +337,14 @@ bool orientationHoldIsPropHang(void) return activeTargetSource == BOXPROPHANG; } +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 @@ -260,29 +355,37 @@ void orientationHoldResetSourceTracking(void) } } -bool orientationHoldComputeError(fpVector3_t *errDeg) +bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) { - fpQuaternion_t qTarget; + fpQuaternion_t qDesired; + // Preset entries slew the target at the figure roll rate: the entry is + // the same mechanism as a figure segment, just toward a constant + float slewRateDegS = figureSequencerConfig()->rollRate; - // Altitude floor recovery overrides any selected preset: upright + climb + // Altitude floor recovery overrides any selected preset: upright + climb. + // Safety recovery tracks the requested attitude directly, no entry slew. if (altitudeFloorRecoveryActive()) { orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FLOOR); - orientationHoldTargetFromRP(&qTarget, 0.0f, altitudeFloorRecoveryPitchDeg()); + orientationHoldTargetFromRP(&qDesired, 0.0f, altitudeFloorRecoveryPitchDeg()); + slewRateDegS = 0.0f; } else if (figureSequencerRequested()) { float figRoll, figPitch; orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE); figureSequencerGetTarget(&figRoll, &figPitch); - orientationHoldTargetFromRP(&qTarget, figRoll, figPitch); + orientationHoldTargetFromRP(&qDesired, figRoll, figPitch); + // the trajectory is already rate shaped; the slew only smooths the + // engage and absolute HOLD segment steps + slewRateDegS = MAX(figureSequencerConfig()->rollRate, figureSequencerConfig()->loopRate); } else if (orientationHoldActivePreset() == NULL && IS_RC_MODE_ACTIVE(BOXATTLOCK)) { // 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 - static fpQuaternion_t lockTarget; - if (activeTargetSource != OHOLD_SOURCE_LOCK || orientationHoldSticksDeflected()) { - lockTarget = orientation; - } orientationHoldCheckSourceSwitch(OHOLD_SOURCE_LOCK); - qTarget = lockTarget; + if (orientationHoldSticksDeflected()) { + qSollState = orientation; + } + qDesired = qSollState; + slewRateDegS = 0.0f; } else { const orientationHoldPreset_t *preset = orientationHoldActivePreset(); if (!preset) { @@ -307,12 +410,12 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) // 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. - orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim); + orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim); fpVector3_t entryErr; - orientationHoldComputeAttitudeError(&entryErr, &orientation, &qTarget); + orientationHoldComputeAttitudeError(&entryErr, &orientation, &qDesired); if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f) { const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); - orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); + orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); } else { // still capturing: keep the altitude reference tracking so the // assist later holds the altitude where the attitude settled, @@ -321,7 +424,13 @@ bool orientationHoldComputeError(fpVector3_t *errDeg) } } - orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget); + if (slewRateDegS > 0.0f) { + orientationHoldSlewTarget(&qSollState, &qDesired, slewRateDegS * dT); + } else { + qSollState = qDesired; + } + + orientationHoldRegulate(errDeg); return true; } diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 642ea46c446..bf022acfd7c 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -54,7 +54,9 @@ 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. +// 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 @@ -66,8 +68,16 @@ void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float p bool orientationHoldIsRequested(void); // Body frame attitude error (deg) for the currently selected target. -// Returns false when no orientation hold box is active. -bool orientationHoldComputeError(fpVector3_t *errDeg); +// The target is a persistent attitude quaternion seeded on the actual +// attitude at engage and slewed toward the requested attitude (fig_roll_rate +// for preset entries), 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) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0bd0307d9d9..9613c69e486 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -745,10 +745,13 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) 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; } - if (!orientationHoldComputeError(&errDeg)) { + if (!orientationHoldComputeError(&errDeg, dT)) { return; } From 3e0c2043d2edc230a3d25efbaf25c66319aef976 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 18:12:11 +0200 Subject: [PATCH 022/108] Prop hang: configurable hover throttle floor (ohold_hover_thr_min) An updraft unloads the thrust, the hover altitude PID cuts the throttle and with it the prop wash over the control surfaces - the attitude authority starves and the hang nods up to 75 deg around vertical until the gust ends (found by the automated gust battery; whether it breaks varies run to run, the authority is marginal). The floor keeps the throttle at a configurable minimum while hovering; excess lift is accepted as a climb instead. Found by experiment near the model's hover throttle. Default 1000 = no floor beyond motor idle, behaviour unchanged. --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/hover_throttle.c | 11 +++++++++-- src/main/flight/hover_throttle.h | 12 +++++++++--- 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bd1c8e2c76f..72ed59e5262 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4634,3 +4634,9 @@ groups: field: dGain min: 0 max: 100 + - name: ohold_hover_thr_min + description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, keeping the prop wash over the control surfaces alive (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle." + default_value: 1000 + field: minThrottle + min: 1000 + max: 1800 diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index cd0fa4f4695..999e8fab3a6 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -58,6 +58,7 @@ PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, .pGain = SETTING_OHOLD_HOVER_THR_P_DEFAULT, .iGain = SETTING_OHOLD_HOVER_THR_I_DEFAULT, .dGain = SETTING_OHOLD_HOVER_THR_D_DEFAULT, + .minThrottle = SETTING_OHOLD_HOVER_THR_MIN_DEFAULT, ); // Engage only when the nose is this close to the zenith; once engaged, @@ -133,8 +134,14 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) const float zErrM = (targetAltCm - z) / 100.0f; const float climbMs = climbCms / 100.0f; + // throttle floor: never cut the throttle below what 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 = MAX(getThrottleIdleValue(), + (int16_t)hoverThrottleConfig()->minThrottle); + iTermUs = constrainf(iTermUs + hoverThrottleConfig()->iGain * zErrM * dT, - getThrottleIdleValue(), getMaxThrottle()); + floorThrottle, getMaxThrottle()); // thrust supports the weight with its vertical component only: // compensate the tilt away from the zenith (capped, the elevation @@ -143,7 +150,7 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) const float correction = (hoverThrottleConfig()->pGain * zErrM - hoverThrottleConfig()->dGain * climbMs) / vertical; - return constrain(lrintf(iTermUs + correction), getThrottleIdleValue(), getMaxThrottle()); + return constrain(lrintf(iTermUs + correction), floorThrottle, getMaxThrottle()); } #endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h index fea92fb383d..d2693edfe58 100644 --- a/src/main/flight/hover_throttle.h +++ b/src/main/flight/hover_throttle.h @@ -36,9 +36,15 @@ // hands control back to the pilot and re-captures the target. typedef struct hoverThrottleConfig_s { - uint8_t pGain; // throttle us per m of altitude error - uint8_t iGain; // throttle us per m per second - uint8_t dGain; // throttle us per m/s of climb rate + uint8_t pGain; // throttle us per m of altitude error + uint8_t iGain; // throttle us per m per second + uint8_t dGain; // throttle us per m/s of climb rate + uint16_t minThrottle; // throttle floor [us] while hovering: keeps the + // prop wash over the control surfaces (an updraft + // otherwise makes the PID cut the throttle and + // with it the control authority). Found by + // experiment near the model's hover throttle; + // 1000 = no floor beyond the motor idle. } hoverThrottleConfig_t; PG_DECLARE(hoverThrottleConfig_t, hoverThrottleConfig); From 44e2bac48a74516b479266429cf06af7bd5be3d6 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 18:20:14 +0200 Subject: [PATCH 023/108] Prop hang: hover throttle floor covers thrust vectoring too (doc) One propeller, one floor: the authority that scales with thrust is the prop wash over the surfaces or the vectored nozzle - the same minimum applies to both steering paths. --- src/main/fc/settings.yaml | 2 +- src/main/flight/hover_throttle.h | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 72ed59e5262..1d5603ee891 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4635,7 +4635,7 @@ groups: min: 0 max: 100 - name: ohold_hover_thr_min - description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, keeping the prop wash over the control surfaces alive (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle." + description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle." default_value: 1000 field: minThrottle min: 1000 diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h index d2693edfe58..94d90a63b65 100644 --- a/src/main/flight/hover_throttle.h +++ b/src/main/flight/hover_throttle.h @@ -39,10 +39,13 @@ typedef struct hoverThrottleConfig_s { uint8_t pGain; // throttle us per m of altitude error uint8_t iGain; // throttle us per m per second uint8_t dGain; // throttle us per m/s of climb rate - uint16_t minThrottle; // throttle floor [us] while hovering: keeps the - // prop wash over the control surfaces (an updraft - // otherwise makes the PID cut the throttle and - // with it the control authority). Found by + uint16_t minThrottle; // throttle floor [us] while hovering: preserves + // the control authority that scales with thrust, + // prop wash over the surfaces as well as thrust + // vectoring (an updraft otherwise makes the PID + // cut the throttle and with it the authority). + // One propeller, one floor: the same value + // covers both steering paths. Found by // experiment near the model's hover throttle; // 1000 = no floor beyond the motor idle. } hoverThrottleConfig_t; From fb54d1842bfbafd490fc253c688c2ce4b41d778c Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 18:33:13 +0200 Subject: [PATCH 024/108] Prop hang: learn the damping reserve instead of hand-tuning a hover gain Hovering has almost no natural aerodynamic damping and the prop-wash moment responds with a lag, so angle-loop gains that are well damped in forward flight can limit cycle around the vertical: the gust battery showed a growing 1-2 Hz pitch/yaw oscillation with the surfaces at a quarter deflection and the throttle healthy - a phase margin problem, not an authority problem. Same philosophy as the hover throttle learning its hover point: a detector watches the tilt-error zero crossings (0.4..3 Hz band, amplitude above noise) and each detected half wave backs the angle gain off fast; quiet time recovers it slowly toward 1.0. The scale settles just below the stability boundary for the actual airframe, CG and battery state, re-learned on every hang. Active only while PROP HANG holds near vertical; no setting, no persistence. --- src/main/flight/orientation_hold.c | 88 ++++++++++++++++++++++++++++++ src/main/flight/orientation_hold.h | 6 ++ src/main/flight/pid.c | 6 +- 3 files changed, 99 insertions(+), 1 deletion(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 4e40ecd4bf8..3bf6abf3f1e 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -337,6 +337,93 @@ bool orientationHoldIsPropHang(void) return activeTargetSource == BOXPROPHANG; } +// ---- Learned damping reserve for the hover regime ------------------------- +// +// Hovering has almost no natural aerodynamic damping (no airflow from +// forward motion over the tail) and the prop-wash moment responds with a +// lag, so angle-loop gains that are well damped in forward flight can limit +// cycle around the vertical: a growing 1-2 Hz pitch/yaw oscillation with +// the surfaces far from saturation. Instead of a hand-tuned hover gain the +// controller LEARNS its own damping reserve, the same philosophy as the +// hover throttle learning its hover point: +// - detect the limit cycle per tilt axis: decisive zero crossings of the +// attitude error at 0.4..3 Hz with amplitude above a floor +// - each detected half wave backs the angle gain off fast (attack) +// - quiet time recovers it slowly toward 1.0 (release) +// The scale settles just below the stability boundary for the actual +// airframe, CG and battery state. Active only while the PROP HANG preset +// holds near vertical; it re-learns on every hang on purpose (no setting, +// no persistence). + +#define HOVER_OSC_MIN_HALFWAVE_S 0.15f // 0.4..3 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 +#define HOVER_GAIN_RESTORE_TAU_S 1.0f // outside the hover regime + +typedef struct { + float sign; // sign of the current half wave + float peakDeg; // amplitude seen since the last crossing + float sinceFlipS; +} hoverOscDetector_t; + +static float hoverGainScale = 1.0f; +static hoverOscDetector_t hoverOsc[2]; // body pitch, body yaw + +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; +} + +static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) +{ + bool active = activeTargetSource == BOXPROPHANG; + if (active) { + // nose elevation gate, same release threshold as the hover throttle + fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; + quaternionRotateVectorInv(&nose, &nose, &orientation); + active = RADIANS_TO_DEGREES(asin_approx(constrainf(-nose.z, -1.0f, 1.0f))) > 45.0f; + } + + if (!active) { + hoverGainScale += (1.0f - hoverGainScale) * MIN(dT / HOVER_GAIN_RESTORE_TAU_S, 1.0f); + hoverOsc[0] = hoverOsc[1] = (hoverOscDetector_t){ 0 }; + return; + } + + bool osc = hoverOscDetectAxis(&hoverOsc[0], errDeg->y, dT); + osc = hoverOscDetectAxis(&hoverOsc[1], errDeg->z, dT) || osc; + + if (osc) { + hoverGainScale = MAX(HOVER_GAIN_FLOOR, hoverGainScale * HOVER_GAIN_ATTACK); + } else { + hoverGainScale += (1.0f - hoverGainScale) * MIN(dT / HOVER_GAIN_RELEASE_TAU_S, 1.0f); + } +} + +float orientationHoldLevelGainScale(void) +{ + return hoverGainScale; +} + void orientationHoldSyncTargetToAttitude(void) { // open-loop flying (figure IMPULSE): the persistent target must not go @@ -431,6 +518,7 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } orientationHoldRegulate(errDeg); + hoverGainUpdate(errDeg, dT); return true; } diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index bf022acfd7c..21f5783f144 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -86,3 +86,9 @@ 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); + +// 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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9613c69e486..7c60a9e0f89 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -755,9 +755,13 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) return; } + // 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(); + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { // Same gain and rate limit handling as pidLevel() - float rateTarget = constrainf(errDeg.v[axis] * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + float rateTarget = constrainf(errDeg.v[axis] * levelGainScale * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlProfile->stabilized.rates[axis] * 10.0f, currentControlProfile->stabilized.rates[axis] * 10.0f); From 4b92db13b2e2220f8befff3cd793c411c9a9afe0 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 18:37:34 +0200 Subject: [PATCH 025/108] Prop hang: widen the limit-cycle detector band to 8 Hz Small airframes oscillate fast: a 0.7 m model with its small inertia limit cycles at 4-8 Hz where a 1.5 m aerobat sits at 1-2 Hz. Noise rejection is the job of the amplitude gates, not of the band. --- src/main/flight/orientation_hold.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 3bf6abf3f1e..5d4c14be9a7 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -355,7 +355,10 @@ bool orientationHoldIsPropHang(void) // holds near vertical; it re-learns on every hang on purpose (no setting, // no persistence). -#define HOVER_OSC_MIN_HALFWAVE_S 0.15f // 0.4..3 Hz band +// 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 From 3c5993eb1ce5cfd4fa1272e7b3e17f802263014f Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 18:51:58 +0200 Subject: [PATCH 026/108] Prop hang: persist the learned hover gain (ohold_hover_gain) Starting every hang at full gain is not conservative: the controller has to oscillate its way down for 1-3 seconds first (amplitude gate plus a few half waves at 0.85 each). The learned scale now freezes at hang exit, is written back to the config and saved to EEPROM on disarm - the next hang and the next flight start at the learned value. A value learned under worse conditions self-corrects upward through the release while hovering quietly. Never writes EEPROM while armed. --- src/main/fc/settings.yaml | 6 +++++ src/main/flight/orientation_hold.c | 41 +++++++++++++++++++++++++++--- src/main/flight/orientation_hold.h | 5 ++++ 3 files changed, 48 insertions(+), 4 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1d5603ee891..22098ac9046 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4562,6 +4562,12 @@ groups: field: knifeLeftPitchTrim min: -15 max: 15 + - name: ohold_hover_gain + description: "LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain." + default_value: 100 + field: hoverGainLearned + min: 30 + max: 100 - name: ohold_knife_right_pitch_trim description: "Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the horizon, held via the rudder. See ohold_knife_left_pitch_trim for why the sides differ" default_value: 0 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 5d4c14be9a7..bba16bf26a0 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -37,8 +37,10 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "fc/config.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" @@ -55,6 +57,7 @@ PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT, .knifeLeftPitchTrim = SETTING_OHOLD_KNIFE_LEFT_PITCH_TRIM_DEFAULT, .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT, + .hoverGainLearned = SETTING_OHOLD_HOVER_GAIN_DEFAULT, ); typedef struct { @@ -365,7 +368,6 @@ bool orientationHoldIsPropHang(void) #define HOVER_GAIN_ATTACK 0.85f // per detected half wave #define HOVER_GAIN_FLOOR 0.3f #define HOVER_GAIN_RELEASE_TAU_S 4.0f -#define HOVER_GAIN_RESTORE_TAU_S 1.0f // outside the hover regime typedef struct { float sign; // sign of the current half wave @@ -373,7 +375,15 @@ typedef struct { float sinceFlipS; } hoverOscDetector_t; -static float hoverGainScale = 1.0f; +// The scale PERSISTS: it freezes at hang exit (the next hang 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 +// hovering quietly. +static float hoverGainScale; +static bool hoverGainInitialized = false; +static bool hoverGainWasActive = false; +static bool hoverGainDirty = false; // learned value awaiting the disarm save static hoverOscDetector_t hoverOsc[2]; // body pitch, body yaw static bool hoverOscDetectAxis(hoverOscDetector_t *d, float sigDeg, float dT) @@ -398,6 +408,12 @@ static bool hoverOscDetectAxis(hoverOscDetector_t *d, float sigDeg, float dT) static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) { + if (!hoverGainInitialized) { + hoverGainScale = constrainf(orientationHoldConfig()->hoverGainLearned / 100.0f, + HOVER_GAIN_FLOOR, 1.0f); + hoverGainInitialized = true; + } + bool active = activeTargetSource == BOXPROPHANG; if (active) { // nose elevation gate, same release threshold as the hover throttle @@ -407,10 +423,20 @@ static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) } if (!active) { - hoverGainScale += (1.0f - hoverGainScale) * MIN(dT / HOVER_GAIN_RESTORE_TAU_S, 1.0f); + // freeze the learned value across hang exits; write it back once so + // the disarm save picks it up + if (hoverGainWasActive) { + const uint8_t learned = lrintf(hoverGainScale * 100.0f); + if (learned != orientationHoldConfig()->hoverGainLearned) { + orientationHoldConfigMutable()->hoverGainLearned = learned; + hoverGainDirty = true; + } + hoverGainWasActive = false; + } hoverOsc[0] = hoverOsc[1] = (hoverOscDetector_t){ 0 }; return; } + hoverGainWasActive = true; bool osc = hoverOscDetectAxis(&hoverOsc[0], errDeg->y, dT); osc = hoverOscDetectAxis(&hoverOsc[1], errDeg->z, dT) || osc; @@ -424,7 +450,7 @@ static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) float orientationHoldLevelGainScale(void) { - return hoverGainScale; + return (hoverGainInitialized) ? hoverGainScale : 1.0f; } void orientationHoldSyncTargetToAttitude(void) @@ -443,6 +469,13 @@ void orientationHoldResetSourceTracking(void) pidResetErrorAccumulators(); activeTargetSource = OHOLD_SOURCE_NONE; } + + // persist the learned hover gain once the aircraft is on the ground + // (never write EEPROM while armed, the flight loop would stall) + if (hoverGainDirty && !ARMING_FLAG(ARMED)) { + hoverGainDirty = false; + saveConfigAndNotify(); + } } bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 21f5783f144..29c949eaeb9 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -47,6 +47,11 @@ typedef struct orientationHoldConfig_s { 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 hover angle-gain scale: written + // by the limit-cycle detector at hang exit, + // saved on disarm. The next hang (and the + // next flight) starts at the learned value + // instead of oscillating its way down again. } orientationHoldConfig_t; PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig); From d71385644cd0b1ced0aa64b60ae453547b8671c1 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 18:53:48 +0200 Subject: [PATCH 027/108] docs: regenerate Settings.md (update_cli_docs.py) Adds ohold_hover_thr_min and ohold_hover_gain. --- docs/Settings.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index a0dcdccfa11..dc3446310de 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4552,6 +4552,16 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within --- +### ohold_hover_gain + +LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 30 | 100 | + +--- + ### ohold_hover_thr_d Hover throttle D gain [throttle us per m/s of climb rate] @@ -4572,6 +4582,16 @@ Hover throttle I gain [throttle us per m per second] --- +### ohold_hover_thr_min + +Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 1000 | 1800 | + +--- + ### ohold_hover_thr_p Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage) From f675a238e9bdca2e54aa66ff4420eb8278049712 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 19:19:46 +0200 Subject: [PATCH 028/108] TVC: raise the tvc_gain ceiling to 400 percent The gust battery on the 15 deg vectoring mount showed the hover using 1-2 deg of the available travel while coning: the controller is too tame, not too hot. At the old 200 percent ceiling the failure rate halved already (5/10 -> 9/10 at 3 m/s gusts); the output stays clamped at full mechanical deflection, so higher gains only use more of the travel that is actually there. --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index dc3446310de..de1ed2abf4d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -6694,11 +6694,11 @@ Turtle mode power factor ### tvc_gain -Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources +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 | 200 | +| 100 | 0 | 400 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 22098ac9046..4e93447dc8b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4533,11 +4533,11 @@ groups: 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" + 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: 200 + 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 From c9e40f9acddc9d45b7f72e742a9949f64c00530e Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 19:35:08 +0200 Subject: [PATCH 029/108] Orientation holds: error leash with target pullback (qacro pattern) The target never runs further ahead of the attitude than the slowest axis rate can catch up within 0.2 s; the excess pulls the target back through the existing re-anchor. Anti-windup at the target level: after a stall, saturated surfaces or a blocked entry the recovery starts from where the aircraft actually is instead of chasing an error that grew unbounded meanwhile. In normal operation the slewed target keeps the error far below the leash. --- src/main/flight/orientation_hold.c | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index bba16bf26a0..0970c6f7521 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -29,6 +29,7 @@ #ifdef USE_ORIENTATION_HOLD +#include "common/axis.h" #include "common/maths.h" #include "common/quaternion.h" #include "common/utils.h" @@ -38,6 +39,7 @@ #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" @@ -289,6 +291,14 @@ static float orientationHoldSlewTarget(fpQuaternion_t *qSoll, const fpQuaternion 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 @@ -299,6 +309,20 @@ static void orientationHoldRegulate(fpVector3_t *errDeg) { 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); From d5409f700958d3a5bfd726b1689a35f0669af330 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 20:35:49 +0200 Subject: [PATCH 030/108] Orientation holds: own slew rate for preset entries (ohold_entry_rate) The entry shared fig_roll_rate with the slow-roll figure, so engaging INVERTED took two full seconds to reach 180 deg. A deliberate slow roll and a snappy entry into a hold are different intents: presets now slew at their own rate, default 180 deg/s. --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 6 ++++++ src/main/flight/orientation_hold.c | 8 +++++--- src/main/flight/orientation_hold.h | 4 ++++ 4 files changed, 25 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index de1ed2abf4d..aabaa7b44bb 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4552,6 +4552,16 @@ 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_hover_gain LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4e93447dc8b..00b92cd994d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4574,6 +4574,12 @@ groups: field: knifeRightPitchTrim min: -15 max: 15 + - 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: PG_FIGURE_SEQUENCER_CONFIG type: figureSequencerConfig_t diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 0970c6f7521..86683c7f03c 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -60,6 +60,7 @@ PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .knifeLeftPitchTrim = SETTING_OHOLD_KNIFE_LEFT_PITCH_TRIM_DEFAULT, .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT, .hoverGainLearned = SETTING_OHOLD_HOVER_GAIN_DEFAULT, + .entryRateDps = SETTING_OHOLD_ENTRY_RATE_DEFAULT, ); typedef struct { @@ -505,9 +506,10 @@ void orientationHoldResetSourceTracking(void) bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) { fpQuaternion_t qDesired; - // Preset entries slew the target at the figure roll rate: the entry is - // the same mechanism as a figure segment, just toward a constant - float slewRateDegS = figureSequencerConfig()->rollRate; + // 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; // Altitude floor recovery overrides any selected preset: upright + climb. // Safety recovery tracks the requested attitude directly, no entry slew. diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 29c949eaeb9..30952e6ded1 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -52,6 +52,10 @@ typedef struct orientationHoldConfig_s { // saved on disarm. The next hang (and the // next flight) starts at the learned value // instead of oscillating its way down again. + 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. } orientationHoldConfig_t; PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig); From e7e863e37e6a216a65839edf8a781dd5cd0511b8 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 21:22:03 +0200 Subject: [PATCH 031/108] Orientation holds: stick deflection carves the target (ANGLE semantics) Roll/pitch sticks become body-frame angle offsets around the rotated reference: deflection is a held offset from the preset (carving around inverted, nose control in the knife), centered sticks return the target gently at ohold_stick_return_rate. Yaw stays a rate command, it is the free axis. The raw stick rate feed for roll/pitch is suppressed while this is active - one input, one meaning. Before, sticks fed rate commands the angle controller fought back: deflection produced an uncalibrated offset (stick rate over LEVEL P) with the I-term winding into the fight. Now the offset is explicit, calibrated (ohold_stick_angle at full deflection, default 30 deg) and flows through the same slewed-target path as everything else. ohold_stick_angle 0 restores the old raw-rate behaviour. --- docs/Settings.md | 20 ++++++++++++ src/main/fc/settings.yaml | 12 +++++++ src/main/flight/orientation_hold.c | 51 ++++++++++++++++++++++++++++-- src/main/flight/orientation_hold.h | 13 ++++++++ src/main/flight/pid.c | 7 +++- 5 files changed, 100 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index aabaa7b44bb..86c6c832b3e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4642,6 +4642,26 @@ Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the --- +### 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 | + +--- + ### opflow_hardware Selection of OPFLOW hardware. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 00b92cd994d..e448ff4519f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4580,6 +4580,18 @@ groups: 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: PG_FIGURE_SEQUENCER_CONFIG type: figureSequencerConfig_t diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 86683c7f03c..cbd5d2fc5c5 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -61,6 +61,8 @@ PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT, .hoverGainLearned = SETTING_OHOLD_HOVER_GAIN_DEFAULT, .entryRateDps = SETTING_OHOLD_ENTRY_RATE_DEFAULT, + .stickAngleMaxDeg = SETTING_OHOLD_STICK_ANGLE_DEFAULT, + .stickReturnRateDps = SETTING_OHOLD_STICK_RETURN_RATE_DEFAULT, ); typedef struct { @@ -101,6 +103,16 @@ static bool orientationHoldSticksDeflected(void) || 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) { @@ -342,8 +354,8 @@ static void orientationHoldRegulate(fpVector3_t *errDeg) static int activeTargetSource = OHOLD_SOURCE_NONE; - 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) { @@ -357,6 +369,7 @@ static void orientationHoldCheckSourceSwitch(int source) // 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; } } @@ -365,6 +378,14 @@ bool orientationHoldIsPropHang(void) return activeTargetSource == BOXPROPHANG; } +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 for the hover regime ------------------------- // // Hovering has almost no natural aerodynamic damping (no airflow from @@ -571,10 +592,36 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // not where the switch was flipped holdRefAltCm = getEstimatedActualPosition(Z); } + + // Pilot stick offsets, ANGLE semantics around the rotated reference: + // deflection = body-frame angle offset from the preset, held while + // deflected; centered sticks return the target slowly. Yaw stays a + // rate command (the free axis). While this is active the rate path + // must not also feed roll/pitch sticks as rates, see + // orientationHoldSticksAreTargetOffsets(). + if (orientationHoldConfig()->stickAngleMaxDeg > 0) { + const float rollOffDeg = orientationHoldStickNorm(rcCommand[ROLL], rcControlsConfig()->deadband) + * orientationHoldConfig()->stickAngleMaxDeg; + const float pitchOffDeg = orientationHoldStickNorm(rcCommand[PITCH], rcControlsConfig()->deadband) + * orientationHoldConfig()->stickAngleMaxDeg; + if (rollOffDeg != 0.0f || pitchOffDeg != 0.0f) { + const fpVector3_t offVec = { .v = { rollOffDeg, pitchOffDeg, 0.0f } }; + fpQuaternion_t qOff; + quatFromRotVecDeg(&qOff, &offVec); + quaternionMultiply(&qDesired, &qDesired, &qOff); + // carving: keep following at the entry rate + } else if (presetSlewCaptured) { + // sticks centered after capture: gentle return to the preset + slewRateDegS = orientationHoldConfig()->stickReturnRateDps; + } + } } if (slewRateDegS > 0.0f) { - orientationHoldSlewTarget(&qSollState, &qDesired, slewRateDegS * dT); + const float remainingDeg = orientationHoldSlewTarget(&qSollState, &qDesired, slewRateDegS * dT); + if (remainingDeg < 1.0f) { + presetSlewCaptured = true; + } } else { qSollState = qDesired; } diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 30952e6ded1..2a5fc89bd58 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -56,6 +56,13 @@ typedef struct orientationHoldConfig_s { // 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 } orientationHoldConfig_t; PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig); @@ -96,6 +103,12 @@ void orientationHoldResetSourceTracking(void); // hover throttle to own the altitude axis) bool orientationHoldIsPropHang(void); +// 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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7c60a9e0f89..b46b292805d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -758,6 +758,10 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) // 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 + const bool stickOffsets = orientationHoldSticksAreTargetOffsets(); for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { // Same gain and rate limit handling as pidLevel() @@ -770,7 +774,8 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) rateTarget = pt1FilterApply4(&pidStates[axis].angleFilterState, rateTarget, pidBank()->pid[PID_LEVEL].I, dT); } - pidStates[axis].rateTarget = constrainf(pidStates[axis].rateTarget + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + const float stickRate = (stickOffsets && axis != FD_YAW) ? 0.0f : pidStates[axis].rateTarget; + pidStates[axis].rateTarget = constrainf(stickRate + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); } } #endif From 211fe8c68c5191f788ff60ad68ced0ee10ec1793 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 22:00:42 +0200 Subject: [PATCH 032/108] Orientation holds: the altitude assist yields to a deliberate pitch input Same pattern as the hover throttle stick override: while the pitch stick is deflected the pilot owns the altitude - the assist would otherwise fight the commanded offset back to zero. The altitude reference keeps tracking, so releasing the stick locks the NEW altitude. --- src/main/flight/orientation_hold.c | 57 +++++++++++++++++------------- 1 file changed, 32 insertions(+), 25 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index cbd5d2fc5c5..0dc418429f4 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -580,40 +580,47 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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 stick offsets, ANGLE semantics around the rotated reference: + // deflection = body-frame angle offset from the preset, held while + // deflected; centered sticks return the target slowly. Yaw stays a + // rate command (the free axis). While this is active the rate path + // must not also feed roll/pitch sticks 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; + } + orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim); fpVector3_t entryErr; orientationHoldComputeAttitudeError(&entryErr, &orientation, &qDesired); - if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f) { + // 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. + if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f && pitchOffDeg == 0.0f) { const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); } else { - // still capturing: keep the altitude reference tracking so the - // assist later holds the altitude where the attitude settled, - // not where the switch was flipped + // 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); } - // Pilot stick offsets, ANGLE semantics around the rotated reference: - // deflection = body-frame angle offset from the preset, held while - // deflected; centered sticks return the target slowly. Yaw stays a - // rate command (the free axis). While this is active the rate path - // must not also feed roll/pitch sticks as rates, see - // orientationHoldSticksAreTargetOffsets(). - if (orientationHoldConfig()->stickAngleMaxDeg > 0) { - const float rollOffDeg = orientationHoldStickNorm(rcCommand[ROLL], rcControlsConfig()->deadband) - * orientationHoldConfig()->stickAngleMaxDeg; - const float pitchOffDeg = orientationHoldStickNorm(rcCommand[PITCH], rcControlsConfig()->deadband) - * orientationHoldConfig()->stickAngleMaxDeg; - if (rollOffDeg != 0.0f || pitchOffDeg != 0.0f) { - const fpVector3_t offVec = { .v = { rollOffDeg, pitchOffDeg, 0.0f } }; - fpQuaternion_t qOff; - quatFromRotVecDeg(&qOff, &offVec); - quaternionMultiply(&qDesired, &qDesired, &qOff); - // carving: keep following at the entry rate - } else if (presetSlewCaptured) { - // sticks centered after capture: gentle return to the preset - slewRateDegS = orientationHoldConfig()->stickReturnRateDps; - } + if (rollOffDeg != 0.0f || pitchOffDeg != 0.0f) { + const fpVector3_t offVec = { .v = { rollOffDeg, pitchOffDeg, 0.0f } }; + fpQuaternion_t qOff; + quatFromRotVecDeg(&qOff, &offVec); + quaternionMultiply(&qDesired, &qDesired, &qOff); + // carving: keep following at the entry rate + } else if (orientationHoldConfig()->stickAngleMaxDeg > 0 && presetSlewCaptured) { + // sticks centered after capture: gentle return to the preset + slewRateDegS = orientationHoldConfig()->stickReturnRateDps; } } From cab0e73a479748b11f2361c14be7e38ea34119de Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 22:48:56 +0200 Subject: [PATCH 033/108] SITL: --lockstep, simulated time advances 1 ms per MSP_SIMULATOR frame HITL benches couple a plant to the SITL in fixed sensor slots, but the SITL clock runs on host real time: every scheduling gap on the bench host makes the AHRS integrate the last injected gyro against a plant that is not being stepped, and during fast maneuvers the estimate runs tens of degrees ahead of the injected truth. With --lockstep the simulated clock is driven by the injection itself: each MSP_SIMULATOR frame advances it exactly one millisecond, equidistant and independent of host load, and the bench may run faster than real time. Between frames the clock creeps with real time but is capped just below the next tick, so the scheduler and the serial task that parses the next frame keep running while simulated time can never outrun the injected sensor data. Boot and gyro calibration run on real time until the first frame arrives. --- src/main/fc/fc_msp.c | 6 +++++ src/main/target/SITL/target.c | 47 ++++++++++++++++++++++++++++++++++- src/main/target/SITL/target.h | 4 +++ 3 files changed, 56 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6ede812094a..cdb2d486fd5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4407,6 +4407,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; diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 2542a352bc1..927c08c24ce 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,51 @@ 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; +static bool lockstepActive = false; +static volatile uint64_t lockstepTickTimeUs; // sim time of the last tick +static volatile uint64_t lockstepTickRealUs; // wall clock at the last tick + +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; } +void sitlLockstepTick(void) { + if (!sitlLockstepEnabled) { + return; + } + if (!lockstepActive) { + lockstepTickTimeUs = realMicros(); + lockstepActive = true; + } else { + lockstepTickTimeUs += 1000; + } + lockstepTickRealUs = realMicros(); +} + +timeUs_t micros(void) { + if (lockstepActive) { + uint64_t creepUs = realMicros() - lockstepTickRealUs; + if (creepUs > 999) { + creepUs = 999; + } + return lockstepTickTimeUs + creepUs; + } + return realMicros(); +} + uint64_t microsISR(void) { return micros(); diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index eb4b97c7bfd..8721b3c4865 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -201,6 +201,10 @@ 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 +extern bool sitlLockstepEnabled; +extern void sitlLockstepTick(void); extern char *strnstr(const char *s, const char *find, size_t slen); extern int lookupAddress (char *, int, int, struct sockaddr *, socklen_t*); From 73d68123d29036011b11f8e621b080835321f05d Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 22:55:54 +0200 Subject: [PATCH 034/108] SITL lockstep: widen the frozen-clock window while RX bytes wait First acceptance run deadlocked exactly as the design note warned: when a bench roundtrip takes longer than the sub-tick creep window, the serial task is no longer due on the frozen clock and the frame that would advance it never gets parsed. The TCP receive thread (always running) now flags arriving bytes; the flag widens the creep cap just enough for one more serial pass, the tick clears it, and a monotonicity high-water mark keeps time strictly increasing across the widened window. --- src/main/drivers/serial_tcp.c | 4 ++++ src/main/target/SITL/target.c | 27 ++++++++++++++++++++++++--- src/main/target/SITL/target.h | 4 +++- 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 7b1c462572e..e677f964289 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -121,6 +121,10 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) } void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) { + // lockstep: newly arrived bytes widen the frozen-clock creep window so + // the serial task gets scheduled to parse the frame that advances the + // simulated time (see target/SITL/target.c) + sitlLockstepRxPending = true; for (ssize_t i = 0; i < recvSize; i++) { if (port->serialPort.rxCallback) { port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 927c08c24ce..e7bf8810420 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -404,9 +404,11 @@ void unlockMainPID(void) // Until the first frame arrives (boot, gyro calibration) time runs on the // host clock as before. bool sitlLockstepEnabled = false; +volatile bool sitlLockstepRxPending = false; // set by the TCP receive thread static bool lockstepActive = false; static volatile uint64_t lockstepTickTimeUs; // sim time of the last tick static volatile uint64_t lockstepTickRealUs; // wall clock at the last tick +static volatile uint64_t lockstepLastUs; // monotonicity high-water mark static uint64_t realMicros(void) { struct timespec now; @@ -424,17 +426,36 @@ void sitlLockstepTick(void) { lockstepActive = true; } else { lockstepTickTimeUs += 1000; + // a stall may have crept past the next tick (see micros below): + // never step backwards + if (lockstepTickTimeUs < lockstepLastUs) { + lockstepTickTimeUs = lockstepLastUs; + } } lockstepTickRealUs = realMicros(); + sitlLockstepRxPending = false; } timeUs_t micros(void) { if (lockstepActive) { uint64_t creepUs = realMicros() - lockstepTickRealUs; - if (creepUs > 999) { - creepUs = 999; + // frozen-clock creep window: sub-millisecond real time keeps the + // scheduler alive between frames, but simulated time can never run + // a full tick ahead of the injected sensor data. If the NEXT frame + // arrives late (bench hiccup), the capped clock would starve the + // serial task that parses it - freshly received bytes widen the + // window just enough for one more serial pass. + const uint64_t capUs = sitlLockstepRxPending ? 2500 : 999; + if (creepUs > capUs) { + creepUs = capUs; + } + uint64_t t = lockstepTickTimeUs + creepUs; + if (t < lockstepLastUs) { + t = lockstepLastUs; // monotonic under the widened window + } else { + lockstepLastUs = t; } - return lockstepTickTimeUs + creepUs; + return t; } return realMicros(); } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 8721b3c4865..d027d37c90e 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -202,8 +202,10 @@ 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 +// MSP_SIMULATOR frame; sitlLockstepTick() is called from the frame handler, +// sitlLockstepRxPending by the TCP receive thread on arriving bytes extern bool sitlLockstepEnabled; +extern volatile bool sitlLockstepRxPending; extern void sitlLockstepTick(void); extern char *strnstr(const char *s, const char *find, size_t slen); extern int lookupAddress (char *, int, int, struct sockaddr *, socklen_t*); From 4bad11249622608c9368641918754280141ec7ec Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 23:08:22 +0200 Subject: [PATCH 035/108] SITL lockstep: anchor the creep window on every byte arrival The one-shot widened window deadlocked one layer deeper: any non-simulator request between frame bursts (an arming-flags poll, a setting read) consumed the widening, and since only a tick cleared the flag, the next simulator frame found the window already spent. Anchor semantics instead: every arrival restarts a fresh sub-tick creep window at the current simulated time, ticks re-anchor the 1 ms grid with a monotonicity high-water mark. Deadlock is now structurally impossible - simulated time can always creep just far enough to parse whatever arrived, and never runs more than about a millisecond past the last received data. --- src/main/drivers/serial_tcp.c | 8 ++--- src/main/target/SITL/target.c | 67 ++++++++++++++++++++--------------- src/main/target/SITL/target.h | 4 +-- 3 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index e677f964289..42fbd2a0275 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -121,10 +121,10 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) } void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) { - // lockstep: newly arrived bytes widen the frozen-clock creep window so - // the serial task gets scheduled to parse the frame that advances the - // simulated time (see target/SITL/target.c) - sitlLockstepRxPending = true; + // lockstep: newly arrived bytes restart the frozen-clock creep window so + // the serial task gets scheduled to parse what just arrived (see + // target/SITL/target.c) + sitlLockstepRxArrival(); for (ssize_t i = 0; i < recvSize; i++) { if (port->serialPort.rxCallback) { port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index e7bf8810420..7c72285499a 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -404,10 +404,10 @@ void unlockMainPID(void) // Until the first frame arrives (boot, gyro calibration) time runs on the // host clock as before. bool sitlLockstepEnabled = false; -volatile bool sitlLockstepRxPending = false; // set by the TCP receive thread static bool lockstepActive = false; -static volatile uint64_t lockstepTickTimeUs; // sim time of the last tick -static volatile uint64_t lockstepTickRealUs; // wall clock at the last tick +static volatile uint64_t lockstepTickTimeUs; // sim time of the last tick (base of the 1 ms grid) +static volatile uint64_t lockstepAnchorSimUs; // sim time the creep window starts from +static volatile uint64_t lockstepAnchorRealUs; // wall clock the creep window starts from static volatile uint64_t lockstepLastUs; // monotonicity high-water mark static uint64_t realMicros(void) { @@ -417,6 +417,30 @@ static uint64_t realMicros(void) { return (now.tv_sec - start_time.tv_sec) * 1000000 + (now.tv_nsec - start_time.tv_nsec) / 1000; } +timeUs_t micros(void) { + if (lockstepActive) { + // 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. + uint64_t creepUs = realMicros() - lockstepAnchorRealUs; + if (creepUs > 999) { + creepUs = 999; + } + uint64_t t = lockstepAnchorSimUs + creepUs; + if (t < lockstepLastUs) { + t = lockstepLastUs; // strictly monotonic across anchors + } else { + lockstepLastUs = t; + } + return t; + } + return realMicros(); +} + void sitlLockstepTick(void) { if (!sitlLockstepEnabled) { return; @@ -426,38 +450,25 @@ void sitlLockstepTick(void) { lockstepActive = true; } else { lockstepTickTimeUs += 1000; - // a stall may have crept past the next tick (see micros below): - // never step backwards + // request bursts between frames may have crept past the next grid + // step: never step backwards, re-anchor the grid instead if (lockstepTickTimeUs < lockstepLastUs) { lockstepTickTimeUs = lockstepLastUs; } } - lockstepTickRealUs = realMicros(); - sitlLockstepRxPending = false; + lockstepAnchorSimUs = lockstepTickTimeUs; + lockstepAnchorRealUs = realMicros(); } -timeUs_t micros(void) { - if (lockstepActive) { - uint64_t creepUs = realMicros() - lockstepTickRealUs; - // frozen-clock creep window: sub-millisecond real time keeps the - // scheduler alive between frames, but simulated time can never run - // a full tick ahead of the injected sensor data. If the NEXT frame - // arrives late (bench hiccup), the capped clock would starve the - // serial task that parses it - freshly received bytes widen the - // window just enough for one more serial pass. - const uint64_t capUs = sitlLockstepRxPending ? 2500 : 999; - if (creepUs > capUs) { - creepUs = capUs; - } - uint64_t t = lockstepTickTimeUs + creepUs; - if (t < lockstepLastUs) { - t = lockstepLastUs; // monotonic under the widened window - } else { - lockstepLastUs = t; - } - return t; +// 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; } - return realMicros(); + lockstepAnchorSimUs = micros(); + lockstepAnchorRealUs = realMicros(); } uint64_t microsISR(void) diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index d027d37c90e..0a4788eb30a 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -203,10 +203,10 @@ 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, -// sitlLockstepRxPending by the TCP receive thread on arriving bytes +// sitlLockstepRxArrival() by the TCP receive thread on arriving bytes extern bool sitlLockstepEnabled; -extern volatile bool sitlLockstepRxPending; extern void sitlLockstepTick(void); +extern void sitlLockstepRxArrival(void); extern char *strnstr(const char *s, const char *find, size_t slen); extern int lookupAddress (char *, int, int, struct sockaddr *, socklen_t*); From 5171d4a5e464c186226199d098e550fdd46d8eda Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 23:17:58 +0200 Subject: [PATCH 036/108] SITL lockstep: mutex the clock state, the anchors were torn The stall detector caught creep values around 2^64: the anchor written by the TCP receive thread was read torn by the main thread, an anchor in the future freezes the clock until the next arrival. A mutex around tick, arrival and the lockstep micros path (cheap at SITL call rates) plus an underflow guard fixes it. --- src/main/target/SITL/target.c | 61 ++++++++++++++++++++++------------- 1 file changed, 39 insertions(+), 22 deletions(-) diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 7c72285499a..97ba5769b55 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -405,10 +405,14 @@ void unlockMainPID(void) // host clock as before. bool sitlLockstepEnabled = false; static bool lockstepActive = false; -static volatile uint64_t lockstepTickTimeUs; // sim time of the last tick (base of the 1 ms grid) -static volatile uint64_t lockstepAnchorSimUs; // sim time the creep window starts from -static volatile uint64_t lockstepAnchorRealUs; // wall clock the creep window starts from -static volatile uint64_t lockstepLastUs; // monotonicity high-water mark +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; @@ -417,25 +421,34 @@ static uint64_t realMicros(void) { 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; + if (creepUs > 999) { + 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) { - // 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. - uint64_t creepUs = realMicros() - lockstepAnchorRealUs; - if (creepUs > 999) { - creepUs = 999; - } - uint64_t t = lockstepAnchorSimUs + creepUs; - if (t < lockstepLastUs) { - t = lockstepLastUs; // strictly monotonic across anchors - } else { - lockstepLastUs = t; - } + pthread_mutex_lock(&lockstepMutex); + const uint64_t t = lockstepMicrosLocked(); + pthread_mutex_unlock(&lockstepMutex); return t; } return realMicros(); @@ -445,6 +458,7 @@ void sitlLockstepTick(void) { if (!sitlLockstepEnabled) { return; } + pthread_mutex_lock(&lockstepMutex); if (!lockstepActive) { lockstepTickTimeUs = realMicros(); lockstepActive = true; @@ -458,6 +472,7 @@ void sitlLockstepTick(void) { } lockstepAnchorSimUs = lockstepTickTimeUs; lockstepAnchorRealUs = realMicros(); + pthread_mutex_unlock(&lockstepMutex); } // Called by the TCP receive thread on arriving bytes: restart the creep @@ -467,8 +482,10 @@ void sitlLockstepRxArrival(void) { if (!lockstepActive) { return; } - lockstepAnchorSimUs = micros(); + pthread_mutex_lock(&lockstepMutex); + lockstepAnchorSimUs = lockstepMicrosLocked(); lockstepAnchorRealUs = realMicros(); + pthread_mutex_unlock(&lockstepMutex); } uint64_t microsISR(void) From 1052ad4513a311c0f2653e96c94735a8dfd9e3cd Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 11 Jul 2026 23:29:48 +0200 Subject: [PATCH 037/108] SITL lockstep: keep the creep window open while RX bytes wait unparsed The stall reporter showed the last failure mode: the final in-window serial pass can land right at the creep cap, putting its next execution beyond the frozen ceiling while the arriving bytes were still being enqueued - the clock freezes with work queued. A pending-bytes counter (enqueue/read, maintained by the TCP driver) keeps the window open until the queued bytes are consumed; the arrival anchor is also set AFTER the enqueue so a woken pass cannot outrun the buffer fill. --- src/main/drivers/serial_tcp.c | 13 +++++++++---- src/main/target/SITL/target.c | 7 ++++++- src/main/target/SITL/target.h | 4 ++++ 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 42fbd2a0275..b0761f162ba 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -121,10 +121,6 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) } void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) { - // lockstep: newly arrived bytes restart the frozen-clock creep window so - // the serial task gets scheduled to parse what just arrived (see - // target/SITL/target.c) - sitlLockstepRxArrival(); for (ssize_t i = 0; i < recvSize; i++) { if (port->serialPort.rxCallback) { port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData); @@ -133,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 ) { @@ -238,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/target/SITL/target.c b/src/main/target/SITL/target.c index 97ba5769b55..1024e340093 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -404,6 +404,7 @@ void unlockMainPID(void) // 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 @@ -432,7 +433,11 @@ static uint64_t lockstepMicrosLocked(void) { // serial pass that would advance the clock. const uint64_t nowReal = realMicros(); uint64_t creepUs = (nowReal > lockstepAnchorRealUs) ? nowReal - lockstepAnchorRealUs : 0; - if (creepUs > 999) { + // 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; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 0a4788eb30a..91ef238c941 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -207,6 +207,10 @@ extern void parseArguments(int argc, char *argv[]); 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*); From c3a95944b0c16ca98de27e2e56d9e9193b0647c5 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 08:52:20 +0200 Subject: [PATCH 038/108] SITL lockstep: absolute tick grid, never inherit the high-water mark Simulated time after N frames must be exactly N milliseconds. Creep chains from request bursts between frames could run micros() past the next grid point, and adopting the high-water mark into the tick base stretched the grid permanently - measured as 14 percent clock inflation against the injected sensor stream on a faster-than-realtime bench (the AHRS integrated 205 deg/s from a 180 deg/s gyro). The grid is now strict; the monotonicity clamp lives only on the micros() output, a short self-correcting flat spot instead of accumulating drift. --- src/main/target/SITL/target.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 1024e340093..58a3577048f 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -468,12 +468,14 @@ void sitlLockstepTick(void) { 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; - // request bursts between frames may have crept past the next grid - // step: never step backwards, re-anchor the grid instead - if (lockstepTickTimeUs < lockstepLastUs) { - lockstepTickTimeUs = lockstepLastUs; - } } lockstepAnchorSimUs = lockstepTickTimeUs; lockstepAnchorRealUs = realMicros(); From 83714d2bf50627a21697247837d408d5a33d557f Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 09:14:00 +0200 Subject: [PATCH 039/108] Flat spin: FLAT SPIN flight mode plus a SPIN figure segment During the rotation roll and pitch are actively regulated FLAT - the controller keeps the plane level and damps the wobble - while only the yaw axis autorotates. FLAT SPIN mode (its own box, like INVERTED): the target is the level attitude with the usual free yaw; the pilot's rudder stick drives the autorotation (full stick saturates the yaw rate loop = full rudder, like a real spin), releasing the rudder stops the rotation with the attitude still held flat, releasing the box recovers normally. No altitude assist, a spin descends by design; the altitude floor preempts globally. FIGSEG_SPIN (p1 turns with sign, p2 rudder percent, p3 timeout): the programmed variant with a wrap-aware turn counter, rudder open loop while roll/pitch stay closed on the flat target. Enter stalled via a preceding IMPULSE segment. --- src/main/fc/fc_msp_box.c | 3 ++ src/main/fc/rc_modes.h | 1 + src/main/flight/figure_sequencer.c | 51 ++++++++++++++++++++++++++++++ src/main/flight/figure_sequencer.h | 11 +++++++ src/main/flight/orientation_hold.c | 14 ++++++-- src/main/flight/pid.c | 11 +++++++ 6 files changed, 89 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index e86cff8ad36..1343ac6ff46 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -119,6 +119,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .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 = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -304,6 +305,7 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXFIGPOINTROLL); ADD_ACTIVE_BOX(BOXFIGSEQ); ADD_ACTIVE_BOX(BOXATTLOCK); + ADD_ACTIVE_BOX(BOXFSPIN); #endif } } @@ -482,6 +484,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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 diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 87e3130ceab..58b296b4958 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -95,6 +95,7 @@ typedef enum { BOXFIGPOINTROLL = 67, BOXFIGSEQ = 68, BOXATTLOCK = 69, + BOXFSPIN = 70, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index a5bc754e399..a6c89dbdc0d 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -90,6 +90,11 @@ 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) { @@ -150,6 +155,7 @@ void figureSequencerUpdate(void) startAltitudeCm = getEstimatedActualPosition(Z); seqIndex = 0; seqSegStartMs = startTimeMs; + seqSpinTracking = false; seqBaseRoll = 0.0f; seqBasePitch = 0.0f; seqSegAltCm = startAltitudeCm; @@ -161,6 +167,7 @@ void figureSequencerUpdate(void) 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: { @@ -272,6 +279,41 @@ void figureSequencerUpdate(void) 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 @@ -379,6 +421,15 @@ bool figureSequencerGetRateCommand(float ratesNorm[3]) return true; } +bool figureSequencerGetSpinCommand(float *yawNorm) +{ + if (!seqSpinActive) { + return false; + } + *yawNorm = seqSpinYawNorm; + return true; +} + void figureSequencerGetTarget(float *rollDeg, float *pitchDeg) { *rollDeg = targetRollDeg; diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h index 22be828a5a8..23c361e86d5 100644 --- a/src/main/flight/figure_sequencer.h +++ b/src/main/flight/figure_sequencer.h @@ -73,6 +73,12 @@ typedef enum { // 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; @@ -109,3 +115,8 @@ 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/orientation_hold.c b/src/main/flight/orientation_hold.c index 0dc418429f4..c0c5cb051b7 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -78,6 +78,14 @@ static const orientationHoldPreset_t orientationHoldPresets[] = { { BOXKNIFELEFT, -90.0f, 0.0f }, { BOXKNIFERIGHT, 90.0f, 0.0f }, { BOXPROPHANG, 0.0f, 90.0f }, + // FLAT SPIN: its own flight mode like INVERTED. Roll and pitch are + // regulated FLAT while the pilot's rudder stick drives the + // autorotation (full stick saturates the yaw rate loop = full + // rudder, exactly like a real spin); releasing the rudder stops the + // rotation with the attitude still held flat, releasing the box + // recovers normally. No altitude assist: a spin descends by design + // (the altitude floor still preempts globally). + { BOXFSPIN, 0.0f, 0.0f }, }; static const orientationHoldPreset_t * orientationHoldActivePreset(void) @@ -601,8 +609,10 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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. - if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f && pitchOffDeg == 0.0f) { + // tracks so the release locks the NEW altitude. The FLAT SPIN mode + // never gets the assist: a spin descends by design. + if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f && pitchOffDeg == 0.0f + && preset->box != BOXFSPIN) { const float assistDeg = figureAltitudeAssistDeg(preset->pitchDeg + pitchTrim, holdRefAltCm); orientationHoldTargetFromRP(&qDesired, preset->rollDeg, preset->pitchDeg + pitchTrim + assistDeg); } else { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b46b292805d..cb0c9bf74c7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -762,8 +762,19 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) // must not also feed roll/pitch as rate commands -- yaw stays a rate, // it is the free axis const bool stickOffsets = orientationHoldSticksAreTargetOffsets(); + // controlled flat spin (figure SPIN segment): the rudder goes open loop + // for the autorotation while roll and pitch stay CLOSED loop on the + // flat target attitude + float spinYawNorm; + const bool spinYaw = figureSequencerGetSpinCommand(&spinYawNorm); for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + if (spinYaw && axis == FD_YAW) { + pidStates[FD_YAW].rateTarget = constrainf( + spinYawNorm * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f, + -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + continue; + } // 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, From 4a68cfe8dcbcd53e9bc656df1bb4ae5b734d1ab4 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 09:23:46 +0200 Subject: [PATCH 040/108] Prop hang: the learned gain applies only inside the hover regime The persistence change left the learned scale globally active after a hang exit: switching from the hover into another hold ran inverted or knife edge at the low learned factor. The stored value still persists for the next hang; outside the hover regime the angle gain is full. The freeze/write-back also runs on mode exit now - landing straight out of a hang and disarming must not lose the learned value. --- src/main/flight/orientation_hold.c | 34 ++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index c0c5cb051b7..6a766adeaf1 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -460,6 +460,20 @@ static bool hoverOscDetectAxis(hoverOscDetector_t *d, float sigDeg, float dT) return false; } +// freeze the learned value when the hover regime ends; write it back once +// so the disarm save picks it up +static void hoverGainFreeze(void) +{ + if (hoverGainWasActive) { + const uint8_t learned = lrintf(hoverGainScale * 100.0f); + if (learned != orientationHoldConfig()->hoverGainLearned) { + orientationHoldConfigMutable()->hoverGainLearned = learned; + hoverGainDirty = true; + } + hoverGainWasActive = false; + } +} + static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) { if (!hoverGainInitialized) { @@ -477,16 +491,7 @@ static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) } if (!active) { - // freeze the learned value across hang exits; write it back once so - // the disarm save picks it up - if (hoverGainWasActive) { - const uint8_t learned = lrintf(hoverGainScale * 100.0f); - if (learned != orientationHoldConfig()->hoverGainLearned) { - orientationHoldConfigMutable()->hoverGainLearned = learned; - hoverGainDirty = true; - } - hoverGainWasActive = false; - } + hoverGainFreeze(); hoverOsc[0] = hoverOsc[1] = (hoverOscDetector_t){ 0 }; return; } @@ -504,7 +509,10 @@ static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) float orientationHoldLevelGainScale(void) { - return (hoverGainInitialized) ? hoverGainScale : 1.0f; + // the learned damping reserve applies ONLY in the hover regime: the + // stored value persists for the next hang, but leaving the hover for + // another hold (or re-entering later at speed) must run at full gain + return (hoverGainInitialized && hoverGainWasActive) ? hoverGainScale : 1.0f; } void orientationHoldSyncTargetToAttitude(void) @@ -524,6 +532,10 @@ void orientationHoldResetSourceTracking(void) activeTargetSource = OHOLD_SOURCE_NONE; } + // leaving the mode ends the hover regime too: freeze the learned gain + // (landing straight out of a hang and disarming must not lose it) + hoverGainFreeze(); + // persist the learned hover gain once the aircraft is on the ground // (never write EEPROM while armed, the flight loop would stall) if (hoverGainDirty && !ARMING_FLAG(ARMED)) { From e9d46ea205b24aa19af959c49a4136da2fd68b15 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 10:06:29 +0200 Subject: [PATCH 041/108] Prop hang: hover throttle defaults from the rig sweep (P 85, D 100) Three sweep rounds under lockstep against the aerobat3d plant: the hold span shrinks monotonically from 28.7 m at the old 25/30 defaults to a 5.0 m plateau from P 55 upward with D at 100 (including a 3 m/s downdraft gust), with the QUIETEST throttle of all configs - higher damping ends the chasing. The plateau itself is the altitude estimator, not the PID: the estimate wanders 3.5 m and sits 2.5 m above the truth while hovering. --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 86c6c832b3e..7ef60c17b79 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4578,7 +4578,7 @@ Hover throttle D gain [throttle us per m/s of climb rate] | Default | Min | Max | | --- | --- | --- | -| 30 | 0 | 100 | +| 100 | 0 | 100 | --- @@ -4608,7 +4608,7 @@ Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is h | Default | Min | Max | | --- | --- | --- | -| 25 | 0 | 100 | +| 85 | 0 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e448ff4519f..2aab19b8155 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4642,7 +4642,7 @@ groups: members: - name: ohold_hover_thr_p description: "Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage)" - default_value: 25 + default_value: 85 field: pGain min: 0 max: 100 @@ -4654,7 +4654,7 @@ groups: max: 100 - name: ohold_hover_thr_d description: "Hover throttle D gain [throttle us per m/s of climb rate]" - default_value: 30 + default_value: 100 field: dGain min: 0 max: 100 From 3b6974a5396701ee306e6c1bd158256af1c13b57 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 10:48:58 +0200 Subject: [PATCH 042/108] Orientation hold: exit handover slew toward ANGLE Releasing a hold far from level dropped the full attitude error onto the Euler level controller as one step. A prop hang exit whipped: ~90 deg of error at near zero airspeed commands full rates, the airframe has no authority to arrest the resulting pitch rate at the horizon and slices through to nose down (SITL lockstep: +90 to -90 deg pitch in 0.4 s with a 180 deg roll-over, settled only after 1.2 s, 9 m altitude loss). The hold now keeps the aircraft for one handover transition: on release toward ANGLE with more than 30 deg of tilt it slews its target to level at ohold_entry_rate and hands over once the attitude is within 10 deg. A deflected stick aborts to the pilot instantly; a 3 s timeout and any re-engaged hold source also end the handover. Exits to ACRO stay raw, floor recovery ends level by construction. SITL lockstep hang exit after: level in 0.4 s, 8 deg undershoot, no roll excursion, 4.8 m altitude loss. flat_spin/loop_fig exits near level are untouched (handover correctly does not engage). --- src/main/fc/fc_core.c | 12 +++++- src/main/flight/orientation_hold.c | 64 ++++++++++++++++++++++++++++++ src/main/flight/orientation_hold.h | 8 ++++ 3 files changed, 83 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 2bb000dbcdd..cd87b014c50 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -708,7 +708,17 @@ void processRx(timeUs_t currentTimeUs) ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); #endif } else if (IS_RC_MODE_ACTIVE(BOXANGLE)) { - ENABLE_FLIGHT_MODE(ANGLE_MODE); +#ifdef USE_ORIENTATION_HOLD + // 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_ORIENTATION_HOLD diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 6a766adeaf1..92ac51668ab 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -359,8 +359,18 @@ static void orientationHoldRegulate(fpVector3_t *errDeg) #define OHOLD_SOURCE_FLOOR (-2) #define OHOLD_SOURCE_FIGURE (-3) #define OHOLD_SOURCE_LOCK (-4) +#define OHOLD_SOURCE_EXIT (-5) + +// 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 @@ -371,6 +381,10 @@ static void orientationHoldCheckSourceSwitch(int source) pidResetErrorAccumulators(); } activeTargetSource = source; + // any real hold taking over cancels a pending exit handover + if (source != OHOLD_SOURCE_EXIT) { + exitSlewActive = 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); @@ -531,6 +545,7 @@ void orientationHoldResetSourceTracking(void) pidResetErrorAccumulators(); activeTargetSource = OHOLD_SOURCE_NONE; } + exitSlewActive = false; // leaving the mode ends the hover regime too: freeze the learned gain // (landing straight out of a hang and disarming must not lose it) @@ -544,6 +559,39 @@ void orientationHoldResetSourceTracking(void) } } +// 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; @@ -576,6 +624,22 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } qDesired = qSollState; slewRateDegS = 0.0f; + } 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) { diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 2a5fc89bd58..b22dd6f7c93 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -83,6 +83,14 @@ void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float p // 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); + // 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 (fig_roll_rate From d2d2947324cf4d9e4be2de20e7f51404cbcc82f3 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 13:41:04 +0200 Subject: [PATCH 043/108] Knife/inverted throttle assist: criterion vz -> 0 (ohold_assist_thr_p/i) In a knife edge or inverted hold the attitude controller owns the surfaces and the pitch assist owns the flight path, but the throttle is a frozen pilot stick: if the speed is too low for the attitude's lift (fuselage lift at knife edge, inverted wing lift) the hold can only sink. The throttle criterion of these holds is vz -> 0: a slow integrating trim (capped +-150 us) around the pilot's stick adds throttle while the hold sinks and takes it back while it climbs, plus a small vz damping term. The pilot stays the base: moving the stick moves the operating point, a deliberate throttle cut stays a cut, 0 disables. This is the second instance of the declared-per-hold throttle criterion (prop hang: altitude PID owns the throttle; knife/inverted: vz trim around the pilot). A NAV ALTHOLD combination was considered and rejected: its fixed wing controller assumes upright forward flight and owns pitch, which conflicts with every attitude this feature exists for. If the pilot engages a NAV mode it outranks the hold as before. SITL A/B (lockstep, JSBSim): spans improve modestly (knife 1.8 -> 1.6 m, inverted 1.6 -> 1.4 m; low-throttle knife 2.4 -> 2.1 m) because the bench airframe's pitch assist already carries most of it; the trim stays well inside its cap. The headroom matters on airframes with less fuselage lift. --- src/main/fc/settings.yaml | 12 ++++++ src/main/flight/hover_throttle.c | 61 +++++++++++++++++++++++++++++- src/main/flight/hover_throttle.h | 4 ++ src/main/flight/orientation_hold.c | 7 ++++ src/main/flight/orientation_hold.h | 4 ++ 5 files changed, 87 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2aab19b8155..6b776e1626a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4664,3 +4664,15 @@ groups: field: minThrottle min: 1000 max: 1800 + - name: ohold_assist_thr_p + description: "Knife edge / inverted throttle assist: damping term, throttle us per m/s of climb rate" + default_value: 40 + field: assistVzP + min: 0 + max: 255 + - name: ohold_assist_thr_i + description: "Knife edge / inverted throttle assist: trim rate, throttle us per m/s of climb per second. The assist slowly trims the throttle around the pilot's stick until the hold stops sinking (or climbing). 0 disables the assist." + default_value: 20 + field: assistVzI + min: 0 + max: 255 diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 999e8fab3a6..b8641d78eb8 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -52,13 +52,15 @@ #include "rx/rx.h" -PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 1); PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, .pGain = SETTING_OHOLD_HOVER_THR_P_DEFAULT, .iGain = SETTING_OHOLD_HOVER_THR_I_DEFAULT, .dGain = SETTING_OHOLD_HOVER_THR_D_DEFAULT, .minThrottle = SETTING_OHOLD_HOVER_THR_MIN_DEFAULT, + .assistVzP = SETTING_OHOLD_ASSIST_THR_P_DEFAULT, + .assistVzI = SETTING_OHOLD_ASSIST_THR_I_DEFAULT, ); // Engage only when the nose is this close to the zenith; once engaged, @@ -80,6 +82,58 @@ static float targetAltCm; static float iTermUs; static timeUs_t lastUpdateUs; +// ---- 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 + +static bool assistActive = false; +static float assistTrimUs; +static timeUs_t assistLastUs; + +static int16_t knifeInvertedAssistApply(int16_t pilotThrottle) +{ + // a deliberate throttle cut stays a throttle cut + if (!navIsAltitudeEstimateTrusted() + || hoverThrottleConfig()->assistVzI == 0 + || pilotThrottle < getThrottleIdleValue() + 50) { + assistActive = false; + return pilotThrottle; + } + + const timeUs_t nowUs = micros(); + if (!assistActive) { + assistActive = true; + assistTrimUs = 0.0f; + assistLastUs = nowUs; + } + const float dT = constrainf((nowUs - assistLastUs) * 1e-6f, 0.0f, 0.1f); + assistLastUs = nowUs; + + const float climbMs = getEstimatedActualVelocity(Z) / 100.0f; + if (fabsf(climbMs) < ASSIST_VZ_CLAMP_MS) { + assistTrimUs = constrainf(assistTrimUs - hoverThrottleConfig()->assistVzI * climbMs * dT, + -ASSIST_TRIM_MAX_US, ASSIST_TRIM_MAX_US); + } + const float damping = -hoverThrottleConfig()->assistVzP + * constrainf(climbMs, -ASSIST_VZ_CLAMP_MS, ASSIST_VZ_CLAMP_MS); + + return constrain(lrintf(pilotThrottle + assistTrimUs + damping), + getThrottleIdleValue(), getMaxThrottle()); +} + static float noseElevationDeg(void) { fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; @@ -98,8 +152,13 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) || !navIsAltitudeEstimateTrusted() || elevDeg < elevGate) { hoverActive = false; + if (ARMING_FLAG(ARMED) && orientationHoldIsKnifeOrInverted()) { + return knifeInvertedAssistApply(pilotThrottle); + } + assistActive = false; return pilotThrottle; } + assistActive = false; const float z = getEstimatedActualPosition(Z); diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h index 94d90a63b65..82c4e5f95d9 100644 --- a/src/main/flight/hover_throttle.h +++ b/src/main/flight/hover_throttle.h @@ -48,6 +48,10 @@ typedef struct hoverThrottleConfig_s { // covers both steering paths. Found by // experiment near the model's hover throttle; // 1000 = no floor beyond the motor idle. + uint8_t assistVzP; // knife/inverted throttle assist: us per m/s of + // climb rate (damping) + uint8_t assistVzI; // knife/inverted throttle assist: trim rate, + // us per m/s per second; 0 disables the assist } hoverThrottleConfig_t; PG_DECLARE(hoverThrottleConfig_t, hoverThrottleConfig); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 92ac51668ab..1f9bd9258fe 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -400,6 +400,13 @@ bool orientationHoldIsPropHang(void) return activeTargetSource == BOXPROPHANG; } +bool orientationHoldIsKnifeOrInverted(void) +{ + return activeTargetSource == BOXINVERTED + || activeTargetSource == BOXKNIFELEFT + || activeTargetSource == BOXKNIFERIGHT; +} + bool orientationHoldSticksAreTargetOffsets(void) { // preset sources carry the box id (positive); the special sources diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index b22dd6f7c93..b6210c9ae17 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -111,6 +111,10 @@ void orientationHoldResetSourceTracking(void); // 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 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, From ca092773c3d88389f796a800413badcd960d850b Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 14:28:45 +0200 Subject: [PATCH 044/108] Figure line-hold: earth-anchored trajectories, full attitude error A slow roll lost ~15 deg of course per roll (SITL truth; the FC estimate walked the other way): the figure target rotated about the body axis, so the roll axis followed wherever the nose drifted, and the reduced attitude error is heading-free by construction - nothing pulled the line back. Figures now anchor their trajectory to the heading captured at figure start (q_yaw(psi0) composed with the RP target) and regulate the FULL attitude error. Verified identity (bench math_verify section G): the full error differs from the reduced one exactly by the twist about body-up, so tilt regulation is unchanged and the line-hold is purely additive. Segments that change the heading on purpose (WAIT_POS banks toward home) or fly open loop (impulse, spin) release the anchor and it re-captures on completion; pilot holds, 3D LOCK and the flat spin stay heading-free as designed. Two coupled fixes this needed: - the target slew must OUTRUN the figure rate (rate + 90 dps): at exactly the figure rate it chases the rotating target saturated and the heading correction never closes - anchored figures slew the full rotation (rate-limited slerp): the reduced slew works on the up vectors only and simply inherits the drifted heading into the target SITL lockstep: slow roll course loss 17.4 -> 0.8 deg (FC frame 0.0), loop course 0.4 deg, altitude spans unchanged. --- src/main/flight/figure_sequencer.c | 10 +++ src/main/flight/figure_sequencer.h | 5 ++ src/main/flight/orientation_hold.c | 104 +++++++++++++++++++++++++++-- 3 files changed, 115 insertions(+), 4 deletions(-) diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index a6c89dbdc0d..90f970a7e85 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -401,6 +401,16 @@ 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) { diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h index 23c361e86d5..7bc995948e6 100644 --- a/src/main/flight/figure_sequencer.h +++ b/src/main/flight/figure_sequencer.h @@ -100,6 +100,11 @@ 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); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 1f9bd9258fe..37315a6f2d9 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -312,6 +312,32 @@ static float orientationHoldSlewTarget(fpQuaternion_t *qSoll, const fpQuaternion 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 fortgeschrieben, 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 @@ -326,9 +352,19 @@ static float orientationHoldSlewTarget(fpQuaternion_t *qSoll, const fpQuaternion // the target thereby follows the actual attitude every cycle -- axis // compliance w_yaw = 0. Held-twist sources (course hold bridging) will skip // this re-anchoring and feed the full error instead. +static bool figureLineAnchored = false; +static fpQuaternion_t qFigureYawAnchor; +static void orientationHoldComputeFullAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget); + static void orientationHoldRegulate(fpVector3_t *errDeg) { - orientationHoldComputeAttitudeError(errDeg, &orientation, &qSollState); + if (figureLineAnchored) { + // figure on a line: the full error adds the heading (twist) + // component the reduced error deliberately drops + 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) @@ -385,6 +421,7 @@ static void orientationHoldCheckSourceSwitch(int source) 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); @@ -407,6 +444,44 @@ bool orientationHoldIsKnifeOrInverted(void) || activeTargetSource == BOXKNIFERIGHT; } +// ---- 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) +{ + 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 @@ -618,9 +693,28 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) 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 - slewRateDegS = MAX(figureSequencerConfig()->rollRate, figureSequencerConfig()->loopRate); + // 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)) { // 3D LOCK: sticks centered = hold the attitude captured at release; // sticks deflected = pure rate flying, the lock target follows the @@ -718,7 +812,9 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } if (slewRateDegS > 0.0f) { - const float remainingDeg = orientationHoldSlewTarget(&qSollState, &qDesired, slewRateDegS * dT); + const float remainingDeg = figureLineAnchored + ? orientationHoldSlewTargetFull(&qSollState, &qDesired, slewRateDegS * dT) + : orientationHoldSlewTarget(&qSollState, &qDesired, slewRateDegS * dT); if (remainingDeg < 1.0f) { presetSlewCaptured = true; } From 3712ea15616bf6502e4867415f5089c79a25a364 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 14:39:02 +0200 Subject: [PATCH 045/108] Hover throttle: pilot corrects via climb-rate command, not override The throttle stick outside the mid deadband no longer hands the whole throttle back to the pilot: it commands a climb rate (full deflection = 2 m/s) by RAMPING the altitude reference, and the unchanged altitude loop tracks the moving target. Releasing the stick latches wherever the ramp stopped. Direct pilot throttle on top of the altitude PID would be two controllers fighting over one actuator - the classic oscillation; commanding the target keeps a single loop, so the pilot can correct without exciting one. The reference clamps to the reachable neighbourhood (windup guard at the throttle floor / saturation) and a stick slammed to the bottom stays a hard throttle cut (bailout). SITL lockstep: 55% stick = +1.12 m/s measured (+1.1 commanded), release holds the new altitude, sink command respects the throttle floor, zero throttle reversals in any phase. --- src/main/flight/hover_throttle.c | 36 ++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index b8641d78eb8..f0b19eddb19 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -76,6 +76,14 @@ PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, // 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; @@ -162,12 +170,27 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) const float z = getEstimatedActualPosition(Z); - // pilot throttle outside the mid deadband: direct control, target follows - if (ABS(pilotThrottle - PWM_RANGE_MIDDLE) > rcControlsConfig()->mid_throttle_deadband) { + // 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; } + // pilot throttle outside the mid deadband: a climb-rate command, the + // controller keeps the throttle (see HOVER_STICK_CLIMB_MS). The stick + // maps linearly beyond the deadband, full deflection = full rate. + float stickClimbMs = 0.0f; + const int16_t stickOff = pilotThrottle - PWM_RANGE_MIDDLE; + 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; + } + } + if (!hoverActive) { hoverActive = true; hoverLatched = false; @@ -189,6 +212,15 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) 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; From 032649fa493a8b3a66554ef689659e31d4563fcf Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 15:05:49 +0200 Subject: [PATCH 046/108] docs: regenerate Settings.md (ohold_assist_thr_p/i) --- docs/Settings.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 7ef60c17b79..aede0bb08ab 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4552,6 +4552,26 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within --- +### ohold_assist_thr_i + +Knife edge / inverted throttle assist: trim rate, throttle us per m/s of climb per second. The assist slowly trims the throttle around the pilot's stick until the hold stops sinking (or climbing). 0 disables the assist. + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 255 | + +--- + +### ohold_assist_thr_p + +Knife edge / inverted throttle assist: damping term, throttle us per m/s of climb rate + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 255 | + +--- + ### 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 From edf484406a1d5915b437eb9559f1bb587e276328 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 16:30:07 +0200 Subject: [PATCH 047/108] Hover regime: raise the baro position weight (ohold_hover_baro_weight) Hovering on the prop pollutes the accelerometer Z (the specific force never matches the kinematics the way it does in forward flight) and the inertial altitude estimate wanders meters around the truth while carrying a bias - the estimator, not the throttle PID, limits how well the hover holds altitude. While the hover throttle owns the altitude the baro deserves more trust: apply ohold_hover_baro_weight (x100, default 1.0, the sweep winner) as a floor over inav_w_z_baro_p; 0 keeps the global weight. Forward flight is untouched. SITL lockstep A/B including the 3 m/s gust: hang truth span 5.2 -> 4.4 m, TVC hover 6.3 -> 5.0 m, estimate bias down ~0.8 m on both. --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 6 ++++++ src/main/flight/hover_throttle.c | 8 +++++++- src/main/flight/hover_throttle.h | 10 ++++++++++ src/main/navigation/navigation_pos_estimator.c | 12 +++++++++++- 5 files changed, 44 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index aede0bb08ab..0038d704612 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4582,6 +4582,16 @@ Target slew rate [deg/s] for entering an orientation hold preset (INVERTED, KNIF --- +### ohold_hover_baro_weight + +Baro position weight (x100) while the hover throttle owns the altitude, applied as a floor over inav_w_z_baro_p. Hovering thrust pollutes the accelerometer, the baro deserves more trust than in forward flight. 0 keeps the global weight. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 150 | + +--- + ### ohold_hover_gain LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6b776e1626a..ab86bb210c0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4676,3 +4676,9 @@ groups: field: assistVzI min: 0 max: 255 + - name: ohold_hover_baro_weight + description: "Baro position weight (x100) while the hover throttle owns the altitude, applied as a floor over inav_w_z_baro_p. Hovering thrust pollutes the accelerometer, the baro deserves more trust than in forward flight. 0 keeps the global weight." + default_value: 100 + field: hoverBaroWeight + min: 0 + max: 150 diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index f0b19eddb19..cd6dd07b0d8 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -52,7 +52,7 @@ #include "rx/rx.h" -PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 2); PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, .pGain = SETTING_OHOLD_HOVER_THR_P_DEFAULT, @@ -61,6 +61,7 @@ PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, .minThrottle = SETTING_OHOLD_HOVER_THR_MIN_DEFAULT, .assistVzP = SETTING_OHOLD_ASSIST_THR_P_DEFAULT, .assistVzI = SETTING_OHOLD_ASSIST_THR_I_DEFAULT, + .hoverBaroWeight = SETTING_OHOLD_HOVER_BARO_WEIGHT_DEFAULT, ); // Engage only when the nose is this close to the zenith; once engaged, @@ -142,6 +143,11 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle) getThrottleIdleValue(), getMaxThrottle()); } +bool hoverThrottleIsEngaged(void) +{ + return hoverActive; +} + static float noseElevationDeg(void) { fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h index 82c4e5f95d9..7db0c29d07c 100644 --- a/src/main/flight/hover_throttle.h +++ b/src/main/flight/hover_throttle.h @@ -52,6 +52,12 @@ typedef struct hoverThrottleConfig_s { // climb rate (damping) uint8_t assistVzI; // knife/inverted throttle assist: trim rate, // us per m/s per second; 0 disables the assist + 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); @@ -59,3 +65,7 @@ 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/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 188ab7d25e3..de54fac43cc 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" @@ -627,8 +628,17 @@ 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; + float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; +#ifdef USE_ORIENTATION_HOLD + // 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; From b079fbc2a9abfaea515d799fe746351122058369 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 19:43:44 +0200 Subject: [PATCH 048/108] FLAT SPIN generalized: spin about the earth vertical in any held attitude The spin axis was an implementation choice, not physics: the reduced attitude error leaves rotation about the EARTH VERTICAL free in every attitude, so the pilot's rudder command is now distributed onto the body axes via the earth-up direction in the body frame instead of being wired to body yaw. At flat/inverted that lands on the yaw axis (unchanged behavior), at knife edge on the pitch axis, at the hang on the roll axis. Body rates along this axis provably leave the tilt untouched (bench math mirror section H), so holding and spinning never fight. The attitude selector now picks the HELD attitude while the FLAT SPIN box picks the behavior: SEL off = flat spin, INVERTED = inverted flat spin, KNIFE L/R = knife edge spin, PROP HANG = torque roll. No assist and no trims in any spin variant; positive rudder = the same rotation seen from above regardless of attitude. The figure SPIN segment uses the same distribution. SITL lockstep: flat 12.5 turns/10 s (regression clean), inverted 12.3 turns/10 s ending still inverted on release, knife 6.4 turns/10 s holding the knife within 11 deg mean; all three stop the rotation within half a degree of residual turn when the rudder centers. --- src/main/flight/orientation_hold.c | 43 ++++++++++++++++++++++++------ src/main/flight/orientation_hold.h | 9 +++++++ src/main/flight/pid.c | 39 +++++++++++++++++++-------- 3 files changed, 72 insertions(+), 19 deletions(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 37315a6f2d9..00d9f414b86 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -78,18 +78,34 @@ static const orientationHoldPreset_t orientationHoldPresets[] = { { BOXKNIFELEFT, -90.0f, 0.0f }, { BOXKNIFERIGHT, 90.0f, 0.0f }, { BOXPROPHANG, 0.0f, 90.0f }, - // FLAT SPIN: its own flight mode like INVERTED. Roll and pitch are - // regulated FLAT while the pilot's rudder stick drives the - // autorotation (full stick saturates the yaw rate loop = full - // rudder, exactly like a real spin); releasing the rudder stops the - // rotation with the attitude still held flat, releasing the box - // recovers normally. No altitude assist: a spin descends by design - // (the altitude floor still preempts globally). - { BOXFSPIN, 0.0f, 0.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, 90.0f }, // + PROP HANG (torque roll) + { BOXFSPIN, 0.0f, 0.0f }, // alone: flat spin }; static const orientationHoldPreset_t * orientationHoldActivePreset(void) { + if (IS_RC_MODE_ACTIVE(BOXFSPIN)) { + if (IS_RC_MODE_ACTIVE(BOXINVERTED)) return &orientationHoldSpinPresets[0]; + if (IS_RC_MODE_ACTIVE(BOXKNIFELEFT)) return &orientationHoldSpinPresets[1]; + if (IS_RC_MODE_ACTIVE(BOXKNIFERIGHT)) return &orientationHoldSpinPresets[2]; + if (IS_RC_MODE_ACTIVE(BOXPROPHANG)) return &orientationHoldSpinPresets[3]; + return &orientationHoldSpinPresets[4]; + } for (unsigned i = 0; i < ARRAYLEN(orientationHoldPresets); i++) { if (IS_RC_MODE_ACTIVE(orientationHoldPresets[i].box)) { return &orientationHoldPresets[i]; @@ -444,6 +460,17 @@ bool orientationHoldIsKnifeOrInverted(void) || 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 diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index b6210c9ae17..03dd2f505c5 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -115,6 +115,15 @@ bool orientationHoldIsPropHang(void); // (used by the knife/inverted throttle assist, criterion vz -> 0) bool orientationHoldIsKnifeOrInverted(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, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cb0c9bf74c7..cb693c5ba43 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -762,19 +762,30 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) // must not also feed roll/pitch as rate commands -- yaw stays a rate, // it is the free axis const bool stickOffsets = orientationHoldSticksAreTargetOffsets(); - // controlled flat spin (figure SPIN segment): the rudder goes open loop - // for the autorotation while roll and pitch stay CLOSED loop on the - // flat target attitude + // controlled spin (FLAT SPIN family or figure SPIN segment): the spin + // command is a rotation about the EARTH VERTICAL - exactly the axis the + // reduced attitude error leaves free - distributed onto the body axes + // via the earth-up direction in the body frame. At flat/inverted that + // is the yaw axis, at knife edge the pitch axis, at the hang the roll + // axis (torque roll). Rates along this axis leave the tilt untouched, + // so holding and spinning never fight (bench math mirror, section H). float spinYawNorm; - const bool spinYaw = figureSequencerGetSpinCommand(&spinYawNorm); + const bool spinSegment = figureSequencerGetSpinCommand(&spinYawNorm); + const bool spinPreset = orientationHoldIsSpinAboutVertical(); + float spinRateDps = 0.0f; + fpVector3_t upBody; + if (spinSegment) { + spinRateDps = spinYawNorm * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f; + } else if (spinPreset) { + // the pilot's rudder rate command becomes the spin rate: positive + // rudder = the same rotation seen from above in every attitude + spinRateDps = pidStates[FD_YAW].rateTarget; + } + if (spinSegment || spinPreset) { + orientationHoldUpInBody(&upBody); + } for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { - if (spinYaw && axis == FD_YAW) { - pidStates[FD_YAW].rateTarget = constrainf( - spinYawNorm * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f, - -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); - continue; - } // 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, @@ -785,7 +796,13 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) rateTarget = pt1FilterApply4(&pidStates[axis].angleFilterState, rateTarget, pidBank()->pid[PID_LEVEL].I, dT); } - const float stickRate = (stickOffsets && axis != FD_YAW) ? 0.0f : pidStates[axis].rateTarget; + float stickRate = (stickOffsets && axis != FD_YAW) ? 0.0f : pidStates[axis].rateTarget; + if (spinSegment || spinPreset) { + if (axis == FD_YAW) { + stickRate = 0.0f; // the rudder is consumed by the spin command + } + rateTarget += spinRateDps * upBody.v[axis]; + } pidStates[axis].rateTarget = constrainf(stickRate + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); } } From d4a7f4f13ade60b58fd3034e7b91296cb1a705da Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 19:52:52 +0200 Subject: [PATCH 049/108] Spin stick sense: aircraft-referenced, not earth-referenced The body axis nearest the vertical receives the rudder stick with its own positive sign: right rudder yaws the airframe right at flat AND inverted, so the rotation seen from above reverses in the inverted flat spin - exactly like a real aircraft. At the knife edge the stick maps to positive body pitch. SITL: flat -12.5 turns/10 s, inverted +9.0 (earth sense reversed as expected), knife +3.8 holding the edge. --- src/main/flight/pid.c | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cb693c5ba43..a56b202f8d4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -777,12 +777,28 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) if (spinSegment) { spinRateDps = spinYawNorm * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f; } else if (spinPreset) { - // the pilot's rudder rate command becomes the spin rate: positive - // rudder = the same rotation seen from above in every attitude + // the pilot's rudder rate command becomes the spin rate spinRateDps = pidStates[FD_YAW].rateTarget; } if (spinSegment || spinPreset) { orientationHoldUpInBody(&upBody); + // AIRCRAFT-referenced stick sense: the body axis nearest the + // vertical receives the stick with its own positive sign - right + // rudder yaws the airframe right at flat AND inverted (so the + // rotation seen from above reverses when inverted, exactly like a + // real aircraft), and maps to positive pitch at the knife edge. + // The sign flip does not disturb the tilt (the distribution stays + // along the free axis either way). + float dominant = upBody.z; + if (fabsf(upBody.y) > fabsf(dominant)) { + dominant = upBody.y; + } + if (fabsf(upBody.x) > fabsf(dominant)) { + dominant = upBody.x; + } + if (dominant < 0.0f) { + vectorScale(&upBody, &upBody, -1.0f); + } } for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { From f2298565c9946a637a23b589577e4935b4ef3f7a Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 20:35:13 +0200 Subject: [PATCH 050/108] Learned damping reserve generalized: one scale per regime Daniel's direction: the normal-flight gains are the REFERENCE, every hold regime runs on a single learned scale of them instead of its own gain set - transitions overshoot exactly where a regime is untrained. The hover limit-cycle learner (zero crossings with amplitude gates, fast attack, slow release, freeze at exit, EEPROM save on disarm) becomes a per-regime table: hover (unchanged, body pitch/yaw axes), inverted, knife and figure (tilt axes roll/pitch). Flying the same figures repeatedly converges their scale - they get better with every flight. The target regime's scale applies from the moment the source switches, so the entry slew already flies with it. Spins and the special sources (lock, floor, exit handover) learn nothing; normal flight always runs the reference gains. New settings ohold_inverted_gain / ohold_knife_gain / ohold_figure_gain (firmware-maintained, editable), PG bump resets the stored group once. SITL lockstep: hover regression behaves identically to the pre-refactor build under equal starting values (the PG bump wipes the learned hover gain once - first battery relearns, the disarm save then persists it: measured 100 -> 64 across one battery). A deliberately hot LEVEL P (160) produces a single damped overshoot on the bench airframe, not a limit cycle - the learner correctly stays passive there (no false positive); real limit-cycle material exists only in the hover regime in this plant. --- docs/Settings.md | 30 +++++ src/main/fc/settings.yaml | 18 +++ src/main/flight/orientation_hold.c | 195 +++++++++++++++++++---------- src/main/flight/orientation_hold.h | 14 ++- 4 files changed, 190 insertions(+), 67 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0038d704612..1ceeea2b0af 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4582,6 +4582,16 @@ Target slew rate [deg/s] for entering an orientation hold preset (INVERTED, KNIF --- +### ohold_figure_gain + +Learned angle-gain scale [%] while a figure flies, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable - flying the same figures repeatedly converges it. 100 = reference gains. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 30 | 100 | + +--- + ### ohold_hover_baro_weight Baro position weight (x100) while the hover throttle owns the altitude, applied as a floor over inav_w_z_baro_p. Hovering thrust pollutes the accelerometer, the baro deserves more trust than in forward flight. 0 keeps the global weight. @@ -4642,6 +4652,16 @@ Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is h --- +### ohold_inverted_gain + +Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 30 | 100 | + +--- + ### ohold_inverted_pitch_trim Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias) @@ -4652,6 +4672,16 @@ Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. --- +### ohold_knife_gain + +Learned angle-gain scale [%] for the knife edge holds, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 30 | 100 | + +--- + ### ohold_knife_left_pitch_trim Pitch trim [deg] on the KNIFE EDGE LEFT hold target, positive = nose above the horizon, held via the rudder. Separate per side: the body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other side, so left/right = shared fuselage-lift part +/- prop part. Reversed prop rotation swaps the sides diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ab86bb210c0..d3c395e624a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4562,6 +4562,24 @@ groups: field: knifeLeftPitchTrim min: -15 max: 15 + - name: ohold_inverted_gain + description: "Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains." + default_value: 100 + field: invertedGainLearned + min: 30 + max: 100 + - name: ohold_knife_gain + description: "Learned angle-gain scale [%] for the knife edge holds, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains." + default_value: 100 + field: knifeGainLearned + min: 30 + max: 100 + - name: ohold_figure_gain + description: "Learned angle-gain scale [%] while a figure flies, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable - flying the same figures repeatedly converges it. 100 = reference gains." + default_value: 100 + field: figureGainLearned + min: 30 + max: 100 - name: ohold_hover_gain description: "LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain." default_value: 100 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 00d9f414b86..b5a978ec102 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -53,13 +53,16 @@ #include "flight/orientation_hold.h" #include "flight/pid.h" -PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 1); PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT, .knifeLeftPitchTrim = SETTING_OHOLD_KNIFE_LEFT_PITCH_TRIM_DEFAULT, .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT, .hoverGainLearned = SETTING_OHOLD_HOVER_GAIN_DEFAULT, + .invertedGainLearned = SETTING_OHOLD_INVERTED_GAIN_DEFAULT, + .knifeGainLearned = SETTING_OHOLD_KNIFE_GAIN_DEFAULT, + .figureGainLearned = SETTING_OHOLD_FIGURE_GAIN_DEFAULT, .entryRateDps = SETTING_OHOLD_ENTRY_RATE_DEFAULT, .stickAngleMaxDeg = SETTING_OHOLD_STICK_ANGLE_DEFAULT, .stickReturnRateDps = SETTING_OHOLD_STICK_RETURN_RATE_DEFAULT, @@ -517,23 +520,24 @@ bool orientationHoldSticksAreTargetOffsets(void) && orientationHoldConfig()->stickAngleMaxDeg > 0; } -// ---- Learned damping reserve for the hover regime ------------------------- +// ---- Learned damping reserve per regime ------------------------------------ // -// Hovering has almost no natural aerodynamic damping (no airflow from -// forward motion over the tail) and the prop-wash moment responds with a -// lag, so angle-loop gains that are well damped in forward flight can limit -// cycle around the vertical: a growing 1-2 Hz pitch/yaw oscillation with -// the surfaces far from saturation. Instead of a hand-tuned hover gain the -// controller LEARNS its own damping reserve, the same philosophy as the -// hover throttle learning its hover point: +// 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..3 Hz with amplitude above a floor -// - each detected half wave backs the angle gain off fast (attack) +// 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) -// The scale settles just below the stability boundary for the actual -// airframe, CG and battery state. Active only while the PROP HANG preset -// holds near vertical; it re-learns on every hang on purpose (no setting, -// no persistence). +// 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 @@ -552,16 +556,39 @@ typedef struct { float sinceFlipS; } hoverOscDetector_t; -// The scale PERSISTS: it freezes at hang exit (the next hang 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 +// 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 -// hovering quietly. -static float hoverGainScale; -static bool hoverGainInitialized = false; -static bool hoverGainWasActive = false; -static bool hoverGainDirty = false; // learned value awaiting the disarm save -static hoverOscDetector_t hoverOsc[2]; // body pitch, body yaw +// 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) { @@ -583,59 +610,101 @@ static bool hoverOscDetectAxis(hoverOscDetector_t *d, float sigDeg, float dT) return false; } -// freeze the learned value when the hover regime ends; write it back once -// so the disarm save picks it up -static void hoverGainFreeze(void) +// freeze the learned value when a regime ends; write it back once so the +// disarm save picks it up +static void regimeGainFreeze(oholdRegime_e regime) { - if (hoverGainWasActive) { - const uint8_t learned = lrintf(hoverGainScale * 100.0f); - if (learned != orientationHoldConfig()->hoverGainLearned) { - orientationHoldConfigMutable()->hoverGainLearned = learned; - hoverGainDirty = true; + 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; } - hoverGainWasActive = false; + g->wasActive = false; + } +} + +static void regimeGainFreezeAll(void) +{ + for (int r = 0; r < OHOLD_REGIME_COUNT; r++) { + regimeGainFreeze((oholdRegime_e)r); } } -static void hoverGainUpdate(const fpVector3_t *errDeg, float dT) +// 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) { - if (!hoverGainInitialized) { - hoverGainScale = constrainf(orientationHoldConfig()->hoverGainLearned / 100.0f, - HOVER_GAIN_FLOOR, 1.0f); - hoverGainInitialized = true; + 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))) > 45.0f + ? 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; } +} - bool active = activeTargetSource == BOXPROPHANG; - if (active) { - // nose elevation gate, same release threshold as the hover throttle - fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } }; - quaternionRotateVectorInv(&nose, &nose, &orientation); - active = RADIANS_TO_DEGREES(asin_approx(constrainf(-nose.z, -1.0f, 1.0f))) > 45.0f; +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; } - if (!active) { - hoverGainFreeze(); - hoverOsc[0] = hoverOsc[1] = (hoverOscDetector_t){ 0 }; + const oholdRegime_e active = regimeGainActiveRegime(); + for (int r = 0; r < OHOLD_REGIME_COUNT; r++) { + if (r != active && regimeGain[r].wasActive) { + regimeGainFreeze((oholdRegime_e)r); + regimeGain[r].osc[0] = regimeGain[r].osc[1] = (hoverOscDetector_t){ 0 }; + } + } + if (active == OHOLD_REGIME_NONE) { return; } - hoverGainWasActive = true; + regimeGainState_t *g = ®imeGain[active]; + g->wasActive = true; - bool osc = hoverOscDetectAxis(&hoverOsc[0], errDeg->y, dT); - osc = hoverOscDetectAxis(&hoverOsc[1], errDeg->z, dT) || osc; + // 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) { - hoverGainScale = MAX(HOVER_GAIN_FLOOR, hoverGainScale * HOVER_GAIN_ATTACK); + g->scale = MAX(HOVER_GAIN_FLOOR, g->scale * HOVER_GAIN_ATTACK); } else { - hoverGainScale += (1.0f - hoverGainScale) * MIN(dT / HOVER_GAIN_RELEASE_TAU_S, 1.0f); + g->scale += (1.0f - g->scale) * MIN(dT / HOVER_GAIN_RELEASE_TAU_S, 1.0f); } } float orientationHoldLevelGainScale(void) { - // the learned damping reserve applies ONLY in the hover regime: the - // stored value persists for the next hang, but leaving the hover for - // another hold (or re-entering later at speed) must run at full gain - return (hoverGainInitialized && hoverGainWasActive) ? hoverGainScale : 1.0f; + // 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) @@ -656,14 +725,14 @@ void orientationHoldResetSourceTracking(void) } exitSlewActive = false; - // leaving the mode ends the hover regime too: freeze the learned gain - // (landing straight out of a hang and disarming must not lose it) - hoverGainFreeze(); + // 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 hover gain once the aircraft is on the ground + // persist the learned regime gains once the aircraft is on the ground // (never write EEPROM while armed, the flight loop would stall) - if (hoverGainDirty && !ARMING_FLAG(ARMED)) { - hoverGainDirty = false; + if (regimeGainDirty && !ARMING_FLAG(ARMED)) { + regimeGainDirty = false; saveConfigAndNotify(); } } @@ -850,7 +919,7 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } orientationHoldRegulate(errDeg); - hoverGainUpdate(errDeg, dT); + regimeGainUpdate(errDeg, dT); return true; } diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 03dd2f505c5..1c36c65d34d 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -47,11 +47,17 @@ typedef struct orientationHoldConfig_s { 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 hover angle-gain scale: written - // by the limit-cycle detector at hang exit, - // saved on disarm. The next hang (and the - // next flight) starts at the learned value + 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 From 28f75ffa108f32946ab332e3606f8a323c31238e Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 21:26:23 +0200 Subject: [PATCH 051/108] Altitude ownership follows the attitude, not the selected box Above the nose-elevation gate (60/45 deg hysteresis) the thrust carries the weight and the hover altitude controller owns the throttle - now also when a knife edge or inverted hold is pulled up into a harrier, not only under the PROP HANG box. Below the gate the vz trim remains the indirect energy path: the alpha continuum (knife -> harrier -> hover) becomes one mechanism whose direct-thrust share is the existing tilt compensation. The climb-rate stick references the throttle position captured at engage, so entering the hover regime out of a pull-up at cruise throttle does not read as a climb command (the hang entered at mid stick behaves exactly as before; SITL span 4.4 m, regression clean). --- src/main/flight/hover_throttle.c | 45 +++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index cd6dd07b0d8..d1bdf740a0e 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -89,6 +89,7 @@ static bool hoverActive = false; static bool hoverLatched = false; static float targetAltCm; static float iTermUs; +static int16_t stickRefUs; static timeUs_t lastUpdateUs; // ---- Knife/inverted throttle assist ---------------------------------------- @@ -161,8 +162,18 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) 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) - || !orientationHoldIsPropHang() + || !thrustAxisHold || !navIsAltitudeEstimateTrusted() || elevDeg < elevGate) { hoverActive = false; @@ -183,20 +194,6 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) return pilotThrottle; } - // pilot throttle outside the mid deadband: a climb-rate command, the - // controller keeps the throttle (see HOVER_STICK_CLIMB_MS). The stick - // maps linearly beyond the deadband, full deflection = full rate. - float stickClimbMs = 0.0f; - const int16_t stickOff = pilotThrottle - PWM_RANGE_MIDDLE; - 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; - } - } - if (!hoverActive) { hoverActive = true; hoverLatched = false; @@ -204,9 +201,27 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) // seed the I-term with the last pilot throttle: learns the model's // hover throttle online instead of requiring a setting iTermUs = 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; From 9c483b3da78c29267f4a39a2cb54874dc894e9af Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 21:47:42 +0200 Subject: [PATCH 052/108] Knife edge speed feedforward (ohold_knife_speed_ff) The fuselage side force carries the weight at the knife edge and scales with v^2: flying slower needs MORE nose-above-horizon angle immediately, not only after an altitude error has built up for the reactive assist. Without an airspeed sensor the own throttle is the v^2 proxy (T ~ v^2 in steady flight); the prop wash over the tail linearizes the theoretical throttle-to-angle hyperbola, so a linear term around the mid-throttle trim point is the honest model (same conclusion as the classic throttle-to-rudder mixers and ArduPilot's airspeed-based knife-edge feedforward, which trade under the same physics). 0 disables (default). SITL lockstep A/B, knife edge through throttle steps 1650/1400/1900: altitude band 2.9 -> 1.7 m with ff=12. --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 6 ++++++ src/main/flight/orientation_hold.c | 21 +++++++++++++++++---- src/main/flight/orientation_hold.h | 4 ++++ 4 files changed, 37 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 1ceeea2b0af..c3830976ba2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4702,6 +4702,16 @@ Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the --- +### ohold_knife_speed_ff + +Knife edge speed feedforward: extra nose-above-horizon angle [deg] per half throttle of speed deficit. The fuselage side force carries the weight and scales with speed squared, so flying slower needs more nose angle immediately - this feeds it forward from the throttle (the speed proxy) instead of waiting for an altitude error. 0 = off. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 30 | + +--- + ### 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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3c395e624a..7c17b13d307 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4562,6 +4562,12 @@ groups: field: knifeLeftPitchTrim min: -15 max: 15 + - name: ohold_knife_speed_ff + description: "Knife edge speed feedforward: extra nose-above-horizon angle [deg] per half throttle of speed deficit. The fuselage side force carries the weight and scales with speed squared, so flying slower needs more nose angle immediately - this feeds it forward from the throttle (the speed proxy) instead of waiting for an altitude error. 0 = off." + default_value: 0 + field: knifeSpeedFF + min: 0 + max: 30 - name: ohold_inverted_gain description: "Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains." default_value: 100 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index b5a978ec102..ca226ce18f0 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -66,6 +66,7 @@ PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .entryRateDps = SETTING_OHOLD_ENTRY_RATE_DEFAULT, .stickAngleMaxDeg = SETTING_OHOLD_STICK_ANGLE_DEFAULT, .stickReturnRateDps = SETTING_OHOLD_STICK_RETURN_RATE_DEFAULT, + .knifeSpeedFF = SETTING_OHOLD_KNIFE_SPEED_FF_DEFAULT, ); typedef struct { @@ -848,10 +849,22 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) float pitchTrim = 0.0f; if (preset->box == BOXINVERTED) { pitchTrim = orientationHoldConfig()->invertedPitchTrim; - } else if (preset->box == BOXKNIFELEFT) { - pitchTrim = orientationHoldConfig()->knifeLeftPitchTrim; - } else if (preset->box == BOXKNIFERIGHT) { - pitchTrim = orientationHoldConfig()->knifeRightPitchTrim; + } 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 diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 1c36c65d34d..839534a0a81 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -69,6 +69,10 @@ typedef struct orientationHoldConfig_s { // 0 = sticks stay raw rate commands. uint8_t stickReturnRateDps; // deg/s the target returns to the preset // after the sticks center + 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. } orientationHoldConfig_t; PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig); From 322338b738f8f7124d0f4b976b29506a222f3c69 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 22:07:47 +0200 Subject: [PATCH 053/108] Knife/inverted assist: keep the chosen speed as the forward component Daniel's spec for the energy side: the speed the pilot entered with is KEPT as the forward component - the assist base scales the pilot's throttle by cosRef/cos(theta), so a rising nose (assist, speed feedforward, harrier transition) no longer bleeds speed through the shrinking horizontal thrust share (the forward complement of the hover PID's 1/sin compensation; thrust-based, no sensor). When the model sinks the vz trim raises the operating point as before - and when the HOLD OSCILLATES it now does too: an oscillating knife edge usually means the surfaces are starving, more airflow is the physical cure while the gain learner only treats the symptom (signal comes from the regime limit-cycle detector). SITL lockstep regression with ff=12: knife span 1.5 m, inverted 1.6 m, zero drift, hold errors unchanged. --- src/main/flight/hover_throttle.c | 28 +++++++++++++++++++++++++--- src/main/flight/orientation_hold.c | 16 ++++++++++++++++ src/main/flight/orientation_hold.h | 5 +++++ 3 files changed, 46 insertions(+), 3 deletions(-) diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index d1bdf740a0e..117d0f1fb00 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -108,12 +108,18 @@ static timeUs_t lastUpdateUs; #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) static bool assistActive = false; static float assistTrimUs; +static float assistCosRef; static timeUs_t assistLastUs; -static int16_t knifeInvertedAssistApply(int16_t pilotThrottle) +static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) { // a deliberate throttle cut stays a throttle cut if (!navIsAltitudeEstimateTrusted() @@ -124,23 +130,39 @@ static int16_t knifeInvertedAssistApply(int16_t 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; 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); + const float climbMs = getEstimatedActualVelocity(Z) / 100.0f; if (fabsf(climbMs) < ASSIST_VZ_CLAMP_MS) { assistTrimUs = constrainf(assistTrimUs - hoverThrottleConfig()->assistVzI * 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); + } const float damping = -hoverThrottleConfig()->assistVzP * constrainf(climbMs, -ASSIST_VZ_CLAMP_MS, ASSIST_VZ_CLAMP_MS); - return constrain(lrintf(pilotThrottle + assistTrimUs + damping), + return constrain(lrintf(baseUs + assistTrimUs + damping), getThrottleIdleValue(), getMaxThrottle()); } @@ -178,7 +200,7 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) || elevDeg < elevGate) { hoverActive = false; if (ARMING_FLAG(ARMED) && orientationHoldIsKnifeOrInverted()) { - return knifeInvertedAssistApply(pilotThrottle); + return knifeInvertedAssistApply(pilotThrottle, elevDeg); } assistActive = false; return pilotThrottle; diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index ca226ce18f0..acd162b17d5 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -694,6 +694,22 @@ static void regimeGainUpdate(const fpVector3_t *errDeg, float dT) } } +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 diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 839534a0a81..394fec18562 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -125,6 +125,11 @@ bool orientationHoldIsPropHang(void); // (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 From 431f5b1ca63ad19b6e3284a396f0b571e0d1de07 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 22:16:57 +0200 Subject: [PATCH 054/108] Stall reserve monitor: control effort is the EARLY speed-raise criterion Field observation: a good regulator masks the approach to the envelope edge - the attitude stays clean while the surfaces silently work toward saturation, then everything lets go at once (the ramp becomes a cliff; the pilot noticed the stall only when the controller could no longer compensate). The mean control effort is therefore the early escalation criterion, ahead of sinking and far ahead of oscillation: the low-passed maximum of |axisPID|/pidSumLimit above 70% raises the knife/inverted assist speed proportionally while reserve is still left. Escalation chain now: effort trend (early) -> sinking (vz trim) -> oscillation (regime detector). SITL regression: clean holds stay below the threshold (no false trigger), spans unchanged (knife 1.5 m, inverted 1.5 m). The positive path needs a stall-capable plant or the real airframe - the bench model has no honest stall hysteresis. --- src/main/flight/hover_throttle.c | 36 ++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 117d0f1fb00..53ffc1848d7 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -47,6 +47,7 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/orientation_hold.h" +#include "flight/pid.h" #include "navigation/navigation.h" @@ -114,11 +115,35 @@ static timeUs_t lastUpdateUs; // (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 @@ -135,6 +160,7 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) assistActive = true; assistTrimUs = 0.0f; assistCosRef = cosNow; + assistEffortFilt = 0.0f; assistLastUs = nowUs; } const float dT = constrainf((nowUs - assistLastUs) * 1e-6f, 0.0f, 0.1f); @@ -159,6 +185,16 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) 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 = -hoverThrottleConfig()->assistVzP * constrainf(climbMs, -ASSIST_VZ_CLAMP_MS, ASSIST_VZ_CLAMP_MS); From a4613c587da72f82b1d54d7572e2cf11d801fb26 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 23:11:04 +0200 Subject: [PATCH 055/108] Crash detection: impact followed by stillness stops the motor After a crash the motor otherwise keeps running on the pilot's throttle. An acceleration spike above crash_g_threshold (default 6 g) followed by the aircraft lying still within 2 s - rotation below 25 deg/s and the accelerometer resting near 1 g for half a second - disarms with the new reason CRASH. The stillness confirmation is the false-positive filter: a flying aircraft is never still, so snaps, spins and hard gusts cannot trigger it. Hand launch rule: the detector arms only once the aircraft is clearly in the air - nav launch reports flying, or the throttle was held above cruise level for a second - so a hand-launched (or carried) armed aircraft does not disarm from handling bumps. SITL lockstep scenarios: flight + impact + stillness disarms; flight + same impact + continued 200 deg/s rotation stays armed; armed on the ground + bump stays armed. --- docs/Settings.md | 10 ++ src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_core.c | 6 ++ src/main/fc/fc_core.h | 1 + src/main/fc/settings.yaml | 12 +++ src/main/flight/crash_detection.c | 138 ++++++++++++++++++++++++++ src/main/flight/crash_detection.h | 46 +++++++++ src/main/io/osd.c | 2 +- 9 files changed, 218 insertions(+), 2 deletions(-) create mode 100644 src/main/flight/crash_detection.c create mode 100644 src/main/flight/crash_detection.h diff --git a/docs/Settings.md b/docs/Settings.md index c3830976ba2..8f5900841f7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -622,6 +622,16 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho --- +### crash_g_threshold + +Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, stops the motor (disarm, reason CRASH). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off. + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 160 | + +--- + ### cruise_power Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f584bdcc892..163ddb11e89 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -335,6 +335,8 @@ main_sources(COMMON_SRC flight/rate_dynamics.h flight/altitude_floor.c flight/altitude_floor.h + flight/crash_detection.c + flight/crash_detection.h flight/hover_throttle.c flight/hover_throttle.h flight/mixer.c diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index a5f29774ffa..d0062f26573 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -138,7 +138,8 @@ #define PG_FIGURE_SEQUENCER_CONFIG 1048 #define PG_FIGURE_SEQUENCE 1049 #define PG_HOVER_THROTTLE_CONFIG 1050 -#define PG_INAV_END PG_HOVER_THROTTLE_CONFIG +#define PG_CRASH_DETECTION_CONFIG 1051 +#define PG_INAV_END PG_CRASH_DETECTION_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index cd87b014c50..3fca72819ac 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -89,6 +89,7 @@ #include "flight/imu.h" #include "flight/altitude_floor.h" #include "flight/figure_sequencer.h" +#include "flight/crash_detection.h" #include "flight/orientation_hold.h" #include "flight/rate_dynamics.h" @@ -1000,6 +1001,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processPilotAndFailSafeActions(dT); +#ifdef USE_ORIENTATION_HOLD + // impact followed by stillness stops the motor (hand-launch aware) + crashDetectionUpdate(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_core.h b/src/main/fc/fc_core.h index 02a5c889a65..f1ecce751a9 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -30,6 +30,7 @@ typedef enum disarmReason_e { DISARM_FAILSAFE = 6, DISARM_NAVIGATION = 7, DISARM_LANDING = 8, + DISARM_CRASH = 9, DISARM_REASON_COUNT } disarmReason_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7c17b13d307..d01bb3b023b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4706,3 +4706,15 @@ groups: field: hoverBaroWeight min: 0 max: 150 + + - name: PG_CRASH_DETECTION_CONFIG + type: crashDetectionConfig_t + headers: ["flight/crash_detection.h"] + condition: USE_ORIENTATION_HOLD + members: + - name: crash_g_threshold + description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, stops the motor (disarm, reason CRASH). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off." + default_value: 60 + field: crashGThreshold + min: 0 + max: 160 diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c new file mode 100644 index 00000000000..71c29e29107 --- /dev/null +++ b/src/main/flight/crash_detection.c @@ -0,0 +1,138 @@ +/* + * 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_ORIENTATION_HOLD + +#include "common/maths.h" +#include "common/vector.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/config.h" +#include "fc/fc_core.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/crash_detection.h" + +#include "navigation/navigation.h" + +#include "rx/rx.h" + +#include "sensors/acceleration.h" +#include "sensors/gyro.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, PG_CRASH_DETECTION_CONFIG, 0); + +PG_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, + .crashGThreshold = SETTING_CRASH_G_THRESHOLD_DEFAULT, +); + +// 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. +#define CRASH_WINDOW_S 2.0f +#define CRASH_STILL_RATE_DPS 25.0f +#define CRASH_STILL_ACC_G_LO 0.7f +#define CRASH_STILL_ACC_G_HI 1.3f +#define CRASH_STILL_CONFIRM_S 0.5f + +static bool inFlight = false; +static float inFlightTimerS; +static float impactWindowS; +static float stillTimerS; + +void crashDetectionUpdate(float dT) +{ + if (crashDetectionConfig()->crashGThreshold == 0 + || !STATE(AIRPLANE) + || !ARMING_FLAG(ARMED)) { + inFlight = false; + inFlightTimerS = 0.0f; + impactWindowS = 0.0f; + stillTimerS = 0.0f; + return; + } + + // in-flight latch (hand launch rule) + 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; + } + } + + 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 latches the confirmation window + if (accMagG > crashDetectionConfig()->crashGThreshold / 10.0f) { + impactWindowS = CRASH_WINDOW_S; + stillTimerS = 0.0f; + } + if (impactWindowS <= 0.0f) { + return; + } + impactWindowS -= dT; + + const float rateMagDps = fast_fsqrtf(sq((float)gyroRateDps(FD_ROLL)) + + sq((float)gyroRateDps(FD_PITCH)) + + sq((float)gyroRateDps(FD_YAW))); + const bool still = rateMagDps < CRASH_STILL_RATE_DPS + && accMagG > CRASH_STILL_ACC_G_LO + && accMagG < CRASH_STILL_ACC_G_HI; + + if (still) { + stillTimerS += dT; + if (stillTimerS > CRASH_STILL_CONFIRM_S) { + // impact followed by stillness: the flight is over, stop the motor + disarm(DISARM_CRASH); + } + } else { + stillTimerS = 0.0f; + } +} + +#endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/crash_detection.h b/src/main/flight/crash_detection.h new file mode 100644 index 00000000000..0b01481dde5 --- /dev/null +++ b/src/main/flight/crash_detection.h @@ -0,0 +1,46 @@ +/* + * 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 fixed wing: 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 disarms. +// A flying aircraft is never still, so aggressive maneuvers (snaps, spins, +// hard gusts) cannot trigger it - the stillness confirmation is the filter. +// Only armed AFTER the aircraft is clearly in the air (hand launch rule): +// nav launch completed, or throttle held above cruise level for a moment. + +typedef struct crashDetectionConfig_s { + uint8_t crashGThreshold; // impact threshold [g * 10]; 0 disables +} crashDetectionConfig_t; + +PG_DECLARE(crashDetectionConfig_t, crashDetectionConfig); + +// Call once per main PID loop iteration (after the IMU update) +void crashDetectionUpdate(float dT); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0bdb9e3375f..73e79da89ac 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5395,7 +5395,7 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) uint8_t drawStat_DisarmMethod(uint8_t col, uint8_t row, uint8_t statValX) { // We keep "" for backward compatibility with the Blackbox explorer and other potential usages - const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "", "FAILSAFE", "NAV SYS", "LANDING"}; + const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "", "FAILSAFE", "NAV SYS", "LANDING", "CRASH"}; displayWrite(osdDisplayPort, col, row, "DISARMED BY"); displayWrite(osdDisplayPort, statValX, row, ": "); From d27c673a0a933c46d9c376e8e4d71c4c68d6632e Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 23:20:55 +0200 Subject: [PATCH 056/108] GPS altitude requires a solid constellation (lock-quality Z gate) Aerobatic attitudes shade the GPS antenna and the reported epv lags the real degradation - a decaying fix kept pulling the altitude estimate while the baro knew better (the reason the bench flew BARO_ONLY as a stand-in). The vertical solution degrades first on a thin constellation, so GPS-Z now requires a margin of two satellites over the gps_min_sats fix threshold; below it the altitude stays baro-first while GPS XY keeps working as before. SITL: with a valid fix at 7 sats a +50 m GPS altitude lie leaves the estimate baro-anchored; at 12 sats the estimate follows GPS as intended. --- src/main/navigation/navigation_pos_estimator.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index de54fac43cc..3f917c8009c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -500,7 +500,17 @@ 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_ORIENTATION_HOLD + // 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 + ) { newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID; } else { From 5169bbbd029532c7c0e6babfa87b469ac52241f5 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 12 Jul 2026 23:31:04 +0200 Subject: [PATCH 057/108] Crash detection: motor cut with re-allow gesture instead of disarm Field experience: after a crash into high grass or corn a SHORT motor burst helps locating the aircraft - a hard disarm would require the arm switch and lose that. The detector now CUTS the motor (mixer forces idle) while staying armed; moving the throttle to zero and up again re-allows it deliberately. The original problem (motor keeps running on the pilot's throttle after an impact) stays solved: holding the stick up changes nothing until the acknowledge gesture. SITL: cruise 0.64 -> crash cuts to idle while armed -> stick held high stays cut -> zero-then-up restores 0.64. The snap-rotation and ground-handling negative scenarios are unchanged (detector untouched). --- docs/Settings.md | 2 +- src/main/fc/fc_core.h | 1 - src/main/fc/settings.yaml | 2 +- src/main/flight/crash_detection.c | 32 ++++++++++++++++++++++++++++--- src/main/flight/crash_detection.h | 16 +++++++++++----- src/main/flight/mixer.c | 9 +++++++-- src/main/io/osd.c | 2 +- 7 files changed, 50 insertions(+), 14 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8f5900841f7..45c951d0cc3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -624,7 +624,7 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho ### crash_g_threshold -Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, stops the motor (disarm, reason CRASH). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off. +Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index f1ecce751a9..02a5c889a65 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -30,7 +30,6 @@ typedef enum disarmReason_e { DISARM_FAILSAFE = 6, DISARM_NAVIGATION = 7, DISARM_LANDING = 8, - DISARM_CRASH = 9, DISARM_REASON_COUNT } disarmReason_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d01bb3b023b..35e7bffb7ea 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4713,7 +4713,7 @@ groups: condition: USE_ORIENTATION_HOLD members: - name: crash_g_threshold - description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, stops the motor (disarm, reason CRASH). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off." + description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off." default_value: 60 field: crashGThreshold min: 0 diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 71c29e29107..560c9584408 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -36,12 +36,12 @@ #include "config/parameter_group_ids.h" #include "fc/config.h" -#include "fc/fc_core.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 "navigation/navigation.h" @@ -75,6 +75,8 @@ static bool inFlight = false; static float inFlightTimerS; static float impactWindowS; static float stillTimerS; +static bool motorCut = false; +static bool cutAckLow = false; void crashDetectionUpdate(float dT) { @@ -85,6 +87,24 @@ void crashDetectionUpdate(float dT) inFlightTimerS = 0.0f; impactWindowS = 0.0f; stillTimerS = 0.0f; + motorCut = false; + cutAckLow = false; + 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; } @@ -127,12 +147,18 @@ void crashDetectionUpdate(float dT) if (still) { stillTimerS += dT; if (stillTimerS > CRASH_STILL_CONFIRM_S) { - // impact followed by stillness: the flight is over, stop the motor - disarm(DISARM_CRASH); + // 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_ORIENTATION_HOLD diff --git a/src/main/flight/crash_detection.h b/src/main/flight/crash_detection.h index 0b01481dde5..b8c894b4dc7 100644 --- a/src/main/flight/crash_detection.h +++ b/src/main/flight/crash_detection.h @@ -30,11 +30,14 @@ // Crash detection for fixed wing: 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 disarms. -// A flying aircraft is never still, so aggressive maneuvers (snaps, spins, -// hard gusts) cannot trigger it - the stillness confirmation is the filter. -// Only armed AFTER the aircraft is clearly in the air (hand launch rule): -// nav launch completed, or throttle held above cruise level for a moment. +// 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) cannot trigger it - the stillness confirmation +// is the filter. Only armed AFTER the aircraft is clearly in the air (hand +// launch rule): nav launch completed, or throttle held above cruise level +// for a moment. typedef struct crashDetectionConfig_s { uint8_t crashGThreshold; // impact threshold [g * 10]; 0 disables @@ -44,3 +47,6 @@ PG_DECLARE(crashDetectionConfig_t, crashDetectionConfig); // 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/mixer.c b/src/main/flight/mixer.c index 4a9a7558dbf..5e67d9136fb 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -46,6 +46,7 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/crash_detection.h" #include "flight/hover_throttle.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -589,8 +590,12 @@ void FAST_CODE mixTable(void) #endif } else { #ifdef USE_ORIENTATION_HOLD - // hover throttle owns the altitude axis while PROP HANG is held - mixerThrottleCommand = hoverThrottleApply(rcCommand[THROTTLE]); + // hover throttle owns the altitude axis while PROP HANG is held; + // after a detected crash the motor stays cut until the pilot + // re-allows it (throttle to zero, then up again) + mixerThrottleCommand = crashDetectionMotorCut() + ? throttleIdleValue + : hoverThrottleApply(rcCommand[THROTTLE]); #else mixerThrottleCommand = rcCommand[THROTTLE]; #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 73e79da89ac..0bdb9e3375f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5395,7 +5395,7 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) uint8_t drawStat_DisarmMethod(uint8_t col, uint8_t row, uint8_t statValX) { // We keep "" for backward compatibility with the Blackbox explorer and other potential usages - const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "", "FAILSAFE", "NAV SYS", "LANDING", "CRASH"}; + const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "", "FAILSAFE", "NAV SYS", "LANDING"}; displayWrite(osdDisplayPort, col, row, "DISARMED BY"); displayWrite(osdDisplayPort, statValX, row, ": "); From d3defc1a5ad4586fc2971a1f5a359518e1a09662 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 07:42:16 +0200 Subject: [PATCH 058/108] crash detection: baro-referenced stillness + GPS ground-speed gate Bench measurements drove three changes to the stillness confirmation: - The fused vertical speed is unusable right after an impact: a 12 g / 0.3 s pulse drives the INS estimate tens of m/s off and the baro pulls it back only after ~4.5 s, far beyond any safe confirmation window. Stillness now checks the RAW baro rate (PT1, 0.5 s), which is honest half a second after the airframe stops. The window stays at 3 s and the cut fires ~1 s after a real crash instead of ~6 s. - IMU + baro cannot tell a crashed airframe from a coordinated level line or shallow turn (both are rate-still, 1 g, baro-flat - measured as a false cut 1.2 s after a hard pull). With a 3D GPS fix the ground speed (< 3 m/s) provides that discrimination; without GPS the setting description now tells the pilot to keep the threshold above the figure g load. - Stillness tightened to 15 dps / 0.9-1.1 g / 1.0 s confirm, default threshold raised to 8 g. SITL: crash + still (GPS 0 and GPS-less) cuts at still+1.0 s, gesture restores the motor; 11 s of post-spike maneuvering and level lines with a moving GPS fix never cut; the panic-dive floor test with hard pulls stays clean. --- docs/Settings.md | 4 +- src/main/fc/settings.yaml | 4 +- src/main/flight/crash_detection.c | 68 +++++++++++++++++++++++++++---- 3 files changed, 63 insertions(+), 13 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 45c951d0cc3..372f185e950 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -624,11 +624,11 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho ### crash_g_threshold -Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off. +Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed), CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Without GPS a smooth level line flown within 3 s of a hard pull can read as still - raise the threshold above the figure g load on GPS-less models. 0 = off. | Default | Min | Max | | --- | --- | --- | -| 60 | 0 | 160 | +| 80 | 0 | 160 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 35e7bffb7ea..b2a208a6aef 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4713,8 +4713,8 @@ groups: condition: USE_ORIENTATION_HOLD members: - name: crash_g_threshold - description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 2 s, CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Aggressive maneuvers do not trigger it - a flying aircraft is never still. 0 = off." - default_value: 60 + description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed), CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Without GPS a smooth level line flown within 3 s of a hard pull can read as still - raise the threshold above the figure g load on GPS-less models. 0 = off." + default_value: 80 field: crashGThreshold min: 0 max: 160 diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 560c9584408..b9797097d70 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -43,11 +43,14 @@ #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" PG_REGISTER_WITH_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, PG_CRASH_DETECTION_CONFIG, 0); @@ -64,12 +67,26 @@ PG_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, #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. -#define CRASH_WINDOW_S 2.0f -#define CRASH_STILL_RATE_DPS 25.0f -#define CRASH_STILL_ACC_G_LO 0.7f -#define CRASH_STILL_ACC_G_HI 1.3f -#define CRASH_STILL_CONFIRM_S 0.5f +// 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; @@ -77,6 +94,22 @@ static float impactWindowS; static float stillTimerS; static bool motorCut = false; static bool cutAckLow = false; +static float baroRateCms; +static float lastBaroAltCm; + +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; + } +#endif + return getEstimatedActualVelocity(Z); +} void crashDetectionUpdate(float dT) { @@ -89,6 +122,10 @@ void crashDetectionUpdate(float dT) stillTimerS = 0.0f; motorCut = false; cutAckLow = false; + baroRateCms = 0.0f; +#ifdef USE_BARO + lastBaroAltCm = baro.BaroAlt; +#endif return; } @@ -123,6 +160,8 @@ void crashDetectionUpdate(float dT) } } + 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; @@ -140,9 +179,20 @@ void crashDetectionUpdate(float dT) const float rateMagDps = fast_fsqrtf(sq((float)gyroRateDps(FD_ROLL)) + sq((float)gyroRateDps(FD_PITCH)) + sq((float)gyroRateDps(FD_YAW))); - const bool still = rateMagDps < CRASH_STILL_RATE_DPS - && accMagG > CRASH_STILL_ACC_G_LO - && accMagG < CRASH_STILL_ACC_G_HI; + bool still = rateMagDps < 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; From bfaeda93cbd4a915e324f6264952e89f2db141dd Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 07:42:16 +0200 Subject: [PATCH 059/108] altitude floor: the recovery climb gets its own energy The floor recovery flew upright + climb pitch but left the throttle wherever the pilot froze it - a panic chop meant an idle-power climb command mushing at 16 kts into the floor plane, and the stick-low motor stop even turned the motor fully off. Three chokepoints, measured on the panic-dive bench case (throttle chopped, down-elevator held): - hover_throttle: while the recovery is active the mixer throttle gets a floor of cruise throttle + pitch-to-throttle compensation for the recovery climb angle; more pilot throttle always wins. - getMotorStatus: the recovery keeps the motor RUNNING through a held low stick - the same override navigation gets via nav_overrides_motor_stop; the pilot override is the floor switch. - pidOrientationHold: roll/pitch stick rates are suppressed during the recovery (the panic-held elevator fought the recovery target down to -13 deg and flew it under power through the floor to 34 m); yaw stays live for steering. Panic dive from 250 m with the stick held down: catch at the floor plane (min 132 m vs 34 m before), airspeed recovers to 65 kts on the raised throttle, release/re-catch cycles around floor + margin as designed. --- src/main/flight/hover_throttle.c | 14 ++++++++++++++ src/main/flight/mixer.c | 9 +++++++++ src/main/flight/pid.c | 8 ++++++-- 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 53ffc1848d7..1f359cce5db 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -43,6 +43,7 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "flight/altitude_floor.h" #include "flight/hover_throttle.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -53,6 +54,8 @@ #include "rx/rx.h" +#include "sensors/battery.h" + PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 2); PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, @@ -239,6 +242,17 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) return knifeInvertedAssistApply(pilotThrottle, elevDeg); } assistActive = false; + // the altitude floor recovery must not climb on whatever throttle + // the pilot froze in the dive (a panic chop leaves idle): the climb + // gets at least the airframe's cruise throttle plus the standard + // pitch-to-throttle compensation for the recovery climb angle - + // more pilot throttle always wins + if (ARMING_FLAG(ARMED) && altitudeFloorRecoveryActive()) { + const int16_t climbThrottle = currentBatteryProfile->nav.fw.cruise_throttle + + lrintf(altitudeFloorRecoveryPitchDeg() * currentBatteryProfile->nav.fw.pitch_to_throttle); + return constrain(MAX(pilotThrottle, climbThrottle), + getThrottleIdleValue(), getMaxThrottle()); + } return pilotThrottle; } assistActive = false; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5e67d9136fb..b9b09b19ce3 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -46,6 +46,7 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/altitude_floor.h" #include "flight/crash_detection.h" #include "flight/hover_throttle.h" #include "flight/imu.h" @@ -688,6 +689,14 @@ motorStatus_e getMotorStatus(void) const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) { +#ifdef USE_ORIENTATION_HOLD + // the altitude floor recovery climbs on its own throttle floor - a + // panic-chopped stick must not stop the motor that climb needs (the + // same override navigation gets via nav_overrides_motor_stop) + if (STATE(AIRPLANE) && altitudeFloorRecoveryActive()) { + return MOTOR_RUNNING; + } +#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/pid.c b/src/main/flight/pid.c index a56b202f8d4..9435ad36ac5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -43,6 +43,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/altitude_floor.h" #include "flight/figure_sequencer.h" #include "flight/mixer_profile.h" #include "flight/orientation_hold.h" @@ -760,8 +761,11 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) 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 - const bool stickOffsets = orientationHoldSticksAreTargetOffsets(); + // 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(); // controlled spin (FLAT SPIN family or figure SPIN segment): the spin // command is a rotation about the EARTH VERTICAL - exactly the axis the // reduced attitude error leaves free - distributed onto the body axes From d962e18ed6b1bd299bcb069034b7cf9126f96747 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 07:53:19 +0200 Subject: [PATCH 060/108] altitude floor: pilot takeover releases the recovery The recovery climb already ends at floor + margin (the margin IS the configurable delta above the floor) - now the pilot can also take over early: once the sticks have returned to center after the catch, a fresh roll/pitch deflection releases the recovery immediately. The panic-held down-elevator from the dive does not count (it never centered), and yaw stays a steering input, not a release. Settings docs spell out where the climb stops. --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/altitude_floor.c | 17 ++++++++++++++++- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 372f185e950..04c1d3ba256 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -414,7 +414,7 @@ Nose up pitch target [deg] flown during altitude floor recovery ### alt_floor_margin -Margin above the altitude floor [m] to arm the floor after takeoff and to release the recovery +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 | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b2a208a6aef..acc33c82414 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4515,7 +4515,7 @@ groups: min: 5 max: 500 - name: alt_floor_margin - description: "Margin above the altitude floor [m] to arm the floor after takeoff and to release the recovery" + 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 diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 6f8838cbd9d..b7867850f68 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -34,6 +34,7 @@ #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" @@ -58,6 +59,7 @@ PG_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, static bool floorArmed = false; // climbed above floor + margin once static bool floorRecovery = false; +static bool sticksSeenCentered = false; void altitudeFloorUpdate(void) { @@ -85,12 +87,25 @@ void altitudeFloorUpdate(void) // Predictive engage: catch before the floor, not at it if (vz < 0.0f && (z + vz * ALT_FLOOR_LOOKAHEAD_S) < floorCm) { floorRecovery = true; + sticksSeenCentered = false; } } else { - // Release when back above floor + margin and climbing + // Release when back above floor + margin and climbing - the climb + // ends at the margin, it does not run away upward if (z > (floorCm + marginCm) && vz > 0.0f) { floorRecovery = false; } + // ... or when the pilot takes over after the catch: the sticks must + // return to center ONCE first (the panic-held down-elevator from + // the dive is not a takeover), a fresh roll/pitch deflection then + // hands control back immediately. Yaw stays steering, not release. + const bool deflected = ABS(rcCommand[ROLL]) > rcControlsConfig()->deadband + || ABS(rcCommand[PITCH]) > rcControlsConfig()->deadband; + if (!sticksSeenCentered) { + sticksSeenCentered = !deflected; + } else if (deflected) { + floorRecovery = false; + } } } From 9078bc3f91c748e14aa1d9f2640c22bd77057230 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 08:54:31 +0200 Subject: [PATCH 061/108] sitl: no CPU-load arming gate in a SITL build The scheduler-backlog heuristic reads HOST load, not flight-controller load: a SITL loop is paced by the simulator frame stream (in lockstep exactly one 1 kHz tick per injected frame), so a busy host blocks arming with a false SYSTEM_OVERLOADED. Compile-gated on SITL_BUILD - hardware targets keep the check unchanged. --- src/main/fc/fc_core.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3fca72819ac..a282c5570cf 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -258,12 +258,20 @@ static void updateArmingStatus(void) } /* CHECK: CPU load */ +#if defined(SITL_BUILD) + /* A SITL loop is paced by the simulator frame stream (lockstep: + * 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. */ + DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); +#else if (isSystemOverloaded()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); } +#endif /* CHECK: Navigation safety */ if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) { From cf607ace929f24a6699949849e7f8dd6bd623c30 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 09:10:21 +0200 Subject: [PATCH 062/108] sitl: skip the CPU-load arming gate only under --lockstep Follow-up to the SITL_BUILD compile gate: a SITL run without --lockstep now keeps the stock check (SITL should stay as close to original behaviour as possible). Only the lockstep mode - where the loop is paced by the simulator frame stream and host load reads as a false scheduler backlog - disables the gate, at runtime via the existing sitlLockstepEnabled option flag. --- src/main/fc/fc_core.c | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a282c5570cf..bce98173b6f 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -258,20 +258,21 @@ static void updateArmingStatus(void) } /* CHECK: CPU load */ + if (isSystemOverloaded() #if defined(SITL_BUILD) - /* A SITL loop is paced by the simulator frame stream (lockstep: - * 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. */ - DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); -#else - if (isSystemOverloaded()) { + /* 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 { DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); } -#endif /* CHECK: Navigation safety */ if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) { From a8696c1dbf26f067c6c79248182f4785ae76d1f8 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 16:49:11 +0200 Subject: [PATCH 063/108] docs: OrientationHold feature guide (wiki-ready) User-facing documentation for the new mode family: the ten boxes and what they do, throttle semantics per hold, the FLAT SPIN combinations with aircraft-referenced rudder sense, the altitude floor's arming / catch / takeover rules, crash detection incl. the no-GPS caveat, learned gains, the physical-trim-first setup order, and the SITL lockstep note. Written so the page can be pasted into the INAV wiki unchanged after merge; settings details stay in Settings.md. --- docs/OrientationHold.md | 189 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 189 insertions(+) create mode 100644 docs/OrientationHold.md diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md new file mode 100644 index 00000000000..86255d804b3 --- /dev/null +++ b/docs/OrientationHold.md @@ -0,0 +1,189 @@ +# 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: bench-validated against a closed-loop JSBSim simulation +(deterministic lockstep, 50-case gust matrix, replay videos); first +hardware flights are upcoming. Treat everything here as experimental. + +## Requirements + +- Fixed wing (`platform_type = AIRPLANE`). Multirotors are untouched. +- A target built with `USE_ORIENTATION_HOLD` (F7/H7 class; F411 fits). +- Barometer required (altitude assist, floor, hover throttle). +- GPS optional but recommended: it gates crash detection in flight and + hardens the altitude estimate during aerobatics. + +## 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. + +### 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). + +### 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 (`ohold_assist_thr_p/i`), 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. + +### 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: practicing low 3D means a mistake reaches the +ground before you do. + +- 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. +- A predicted breach (sink rate looked ahead a few seconds) engages an + automatic upright + climb recovery that OVERRIDES the selected 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 climb ends at floor + `alt_floor_margin`. To take over earlier: + center the sticks once, then any fresh roll/pitch input hands control + back immediately. Switching the box off always ends it. + +### 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 is done - switch back to your normal mode; there is no +automatic level-off yet. + +### Crash detection (`crash_g_threshold`) + +Problem being solved: after an unscheduled arrival the prop keeps +churning until you walk over and disarm. + +An impact spike above the threshold, 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). Opt-in, 0 = off. +Without GPS, keep the threshold above your figures' g load - a smooth +level line right after a hard pull is indistinguishable from lying +still on IMU + baro alone. + +## Learned gains + +Normal-flight gains are the reference. Each hold regime (hover; +inverted/knife/figures) learns its own damping scale from its own limit +cycles and persists it on disarm - fly a figure repeatedly and it gets +better. No per-regime hand tuning. + +## Setup order + +1. Trim the airframe physically first (level trim, CG via the 45-degree + inverted test, per-side knife-edge coupling, thrust line, aileron + differential). Never paper over a bad CG with software trim. +2. Set the per-attitude pitch trims from those flights. +3. Let the hover gain learner converge (a few prop hangs). +4. Then figure rates and the altitude assist. + +All `ohold_*`, `alt_floor_*`, `fig_*` and `crash_g_threshold` settings +are documented 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. From b94b5f7e1db410b568f0786118cd61c6b7783295 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 21:11:27 +0200 Subject: [PATCH 064/108] crash detection: standalone USE_CRASH_DETECTION feature Impact-plus-stillness motor cut is useful on any airframe, not only with the orientation-hold modes it happened to ship inside. Give it its own feature define and decouple it: - common.h defines USE_CRASH_DETECTION unconditionally - crash_detection.c guards USE_CRASH_DETECTION (was USE_ORIENTATION_HOLD) - mixer.c: the crash motor cut now applies independently of the hover throttle - a plane without the aerobatics modes still gets it - fc_core.c update hook and the settings.yaml PG condition move to the new define SITL-verified: crash detection compiles and links with the orientation- hold modes disabled. First step toward extracting crash detection into its own PR (it fits with the landing detector and should serve multirotors too). --- src/main/fc/fc_core.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/crash_detection.c | 4 ++-- src/main/flight/mixer.c | 14 ++++++++------ src/main/target/common.h | 4 ++++ 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index bce98173b6f..2ccd54fc602 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -1010,7 +1010,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processPilotAndFailSafeActions(dT); -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_CRASH_DETECTION // impact followed by stillness stops the motor (hand-launch aware) crashDetectionUpdate(dT); #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index acc33c82414..3584f9b0c02 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4710,7 +4710,7 @@ groups: - name: PG_CRASH_DETECTION_CONFIG type: crashDetectionConfig_t headers: ["flight/crash_detection.h"] - condition: USE_ORIENTATION_HOLD + condition: USE_CRASH_DETECTION members: - name: crash_g_threshold description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed), CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Without GPS a smooth level line flown within 3 s of a hard pull can read as still - raise the threshold above the figure g load on GPS-less models. 0 = off." diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index b9797097d70..0d6eaed2c19 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -27,7 +27,7 @@ #include -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_CRASH_DETECTION #include "common/maths.h" #include "common/vector.h" @@ -211,4 +211,4 @@ bool crashDetectionMotorCut(void) return motorCut; } -#endif // USE_ORIENTATION_HOLD +#endif // USE_CRASH_DETECTION diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b9b09b19ce3..f8f4d58a3f4 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -590,15 +590,17 @@ void FAST_CODE mixTable(void) } #endif } else { + mixerThrottleCommand = rcCommand[THROTTLE]; #ifdef USE_ORIENTATION_HOLD - // hover throttle owns the altitude axis while PROP HANG is held; + // hover throttle owns the altitude axis while PROP HANG is held + mixerThrottleCommand = hoverThrottleApply(mixerThrottleCommand); +#endif +#ifdef USE_CRASH_DETECTION // after a detected crash the motor stays cut until the pilot // re-allows it (throttle to zero, then up again) - mixerThrottleCommand = crashDetectionMotorCut() - ? throttleIdleValue - : hoverThrottleApply(rcCommand[THROTTLE]); -#else - mixerThrottleCommand = rcCommand[THROTTLE]; + if (crashDetectionMotorCut()) { + mixerThrottleCommand = throttleIdleValue; + } #endif throttleRangeMin = throttleIdleValue; throttleRangeMax = getMaxThrottle(); diff --git a/src/main/target/common.h b/src/main/target/common.h index 1e2e72f0f15..13609d45a68 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -76,6 +76,10 @@ #define USE_EXTENDED_CMS_MENUS #define USE_ORIENTATION_HOLD #define USE_THRUST_VECTORING +// Crash detection (impact + stillness -> motor cut) is small and useful on +// any platform, not only with the orientation-hold modes, so it is its own +// feature. +#define USE_CRASH_DETECTION // Allow default rangefinders #define USE_RANGEFINDER From 1fed1dc4e400ca5bba9fb4cfe4062ea70997f564 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 21:12:01 +0200 Subject: [PATCH 065/108] target: gate the orientation-hold suite to > 512 KB flash The aerobatics suite is ~12 KB and experimental; on a 512 KB board it leaves almost no headroom (measured: F722 at 98% flash, ~9 KB free). Restrict it to targets with room to spare - MCU_FLASH_SIZE > 512 - so F722, F411 and other 512 KB boards are exempt and keep their flash. F405, F765, H743 and the rest keep the feature. SITL has no MCU_FLASH_SIZE (native build), so it enables the suite explicitly in its own target.h, exactly as it does for USE_GEOZONE - the bench needs it. SITL-verified both ways (feature on; and off, which is the 512 KB path). --- src/main/target/SITL/target.h | 6 ++++++ src/main/target/common.h | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 91ef238c941..bc5206058e8 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -83,6 +83,12 @@ #define MAX_GEOZONES_IN_CONFIG 63 #define MAX_VERTICES_IN_CONFIG 126 +// The orientation-hold 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_ORIENTATION_HOLD +#define USE_THRUST_VECTORING + #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? #undef USE_VCP #undef USE_PPM diff --git a/src/main/target/common.h b/src/main/target/common.h index 13609d45a68..fe9192eba91 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -74,8 +74,14 @@ #define USE_SMITH_PREDICTOR #define USE_RATE_DYNAMICS #define USE_EXTENDED_CMS_MENUS +// The orientation-hold aerobatics suite (holds, figures, thrust vectoring) +// is large (~12 KB) and experimental. Restrict it to targets with room to +// spare: 512 KB boards (F722, F411, ...) are exempt and keep their flash. +// SITL has no MCU_FLASH_SIZE and enables it in its own target.h. +#if (MCU_FLASH_SIZE > 512) #define USE_ORIENTATION_HOLD #define USE_THRUST_VECTORING +#endif // Crash detection (impact + stillness -> motor cut) is small and useful on // any platform, not only with the orientation-hold modes, so it is its own // feature. From 4a04a1b7306b90e69417c12ba45847489d5394c5 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 21:12:16 +0200 Subject: [PATCH 066/108] docs: OrientationHold operate-and-tune guide For a feature this size a reference is not enough - a user needs a walkthrough. Expand the guide with: - a 'learned gains - do not hand-tune these' warning (the four *_gain settings are firmware-maintained; hand-tuning fights the learner) - a Step 0-7 first-flights sequence, physical trim first, each step naming the symptom of a wrong value - a symptom-to-setting troubleshooting table - a note that crash detection is a standalone feature being split out Wiki-ready. --- docs/OrientationHold.md | 114 +++++++++++++++++++++++++++++++++------- 1 file changed, 96 insertions(+), 18 deletions(-) diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md index 86255d804b3..3cb364bc2a8 100644 --- a/docs/OrientationHold.md +++ b/docs/OrientationHold.md @@ -146,6 +146,11 @@ automatic level-off yet. ### Crash detection (`crash_g_threshold`) +*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. @@ -159,24 +164,97 @@ Without GPS, keep the threshold above your figures' g load - a smooth level line right after a hard pull is indistinguishable from lying still on IMU + baro alone. -## Learned gains - -Normal-flight gains are the reference. Each hold regime (hover; -inverted/knife/figures) learns its own damping scale from its own limit -cycles and persists it on disarm - fly a figure repeatedly and it gets -better. No per-regime hand tuning. - -## Setup order - -1. Trim the airframe physically first (level trim, CG via the 45-degree - inverted test, per-side knife-edge coupling, thrust line, aileron - differential). Never paper over a bad CG with software trim. -2. Set the per-attitude pitch trims from those flights. -3. Let the hover gain learner converge (a few prop hangs). -4. Then figure rates and the altitude assist. - -All `ohold_*`, `alt_floor_*`, `fig_*` and `crash_g_threshold` settings -are documented in [Settings.md](Settings.md). +## 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. The hover altitude itself is a learned gain +(step above) - give it a few hangs to settle. 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. + +**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 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 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 From bc4dc2444c707d69dd4f42b68ec59ddb7ca9acd9 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 13 Jul 2026 21:38:29 +0200 Subject: [PATCH 067/108] crash detection: works on any flying platform, motor cut holds a copter too Two changes so a crashed multirotor is covered, not just a fixed wing: - platform gate STATE(AIRPLANE) -> STATE(AIRPLANE) || STATE(MULTIROTOR) (rovers/boats excluded - an impact there is no reason to cut). The in-flight latch was already platform-general: the throttle-held rule covers both, the fixed-wing hand launch just arms it earlier; no GPS needed either way. - the cut now goes through getMotorStatus() -> MOTOR_STOPPED_USER, not by lowering mixerThrottleCommand. On a multirotor the throttle command is added to the per-motor PID mix, so lowering it alone would let the attitude loops keep spinning a crashed copter's motors; the stopped status forces every motor to idle directly. Fixed wing is unchanged in behaviour (SITL crash_test: cut at still+1.0 s, throttle gesture restores). The stillness detector (rates/1g/frozen baro/GPS ground speed) is already platform-neutral. This is the form crash detection should take in its own PR - independent of the orientation-hold modes and useful to every craft that flies and can crash. --- src/main/flight/crash_detection.c | 13 +++++++++++-- src/main/flight/crash_detection.h | 20 ++++++++++---------- src/main/flight/mixer.c | 21 ++++++++++++++------- 3 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 0d6eaed2c19..8a7a113fd89 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -113,8 +113,12 @@ static float crashVerticalRateCms(float dT) 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. if (crashDetectionConfig()->crashGThreshold == 0 - || !STATE(AIRPLANE) + || !(STATE(AIRPLANE) || STATE(MULTIROTOR)) || !ARMING_FLAG(ARMED)) { inFlight = false; inFlightTimerS = 0.0f; @@ -145,7 +149,12 @@ void crashDetectionUpdate(float dT) return; } - // in-flight latch (hand launch rule) + // 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; diff --git a/src/main/flight/crash_detection.h b/src/main/flight/crash_detection.h index b8c894b4dc7..79b68acbf35 100644 --- a/src/main/flight/crash_detection.h +++ b/src/main/flight/crash_detection.h @@ -28,16 +28,16 @@ #include "config/parameter_group.h" -// Crash detection for fixed wing: 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) cannot trigger it - the stillness confirmation -// is the filter. Only armed AFTER the aircraft is clearly in the air (hand -// launch rule): nav launch completed, or throttle held above cruise level -// for a moment. +// 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). typedef struct crashDetectionConfig_s { uint8_t crashGThreshold; // impact threshold [g * 10]; 0 disables diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f8f4d58a3f4..15b6caddae4 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -595,13 +595,8 @@ void FAST_CODE mixTable(void) // hover throttle owns the altitude axis while PROP HANG is held mixerThrottleCommand = hoverThrottleApply(mixerThrottleCommand); #endif -#ifdef USE_CRASH_DETECTION - // after a detected crash the motor stays cut until the pilot - // re-allows it (throttle to zero, then up again) - if (crashDetectionMotorCut()) { - mixerThrottleCommand = throttleIdleValue; - } -#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(); @@ -684,6 +679,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; } From d253f2a66e7133512e05109fc819bacb888e0c2f Mon Sep 17 00:00:00 2001 From: pdani Date: Wed, 15 Jul 2026 20:28:22 +0200 Subject: [PATCH 068/108] Derive instead of ask: load governor, derived gains, platform gating Review feedback round (Jetrell, sensei-hacker): too many settings that ask the pilot what the firmware can know, and modes offered on airframes that cannot fly them. Every item below removes a question: - Load governor (ohold_load_limit, g x 10 - a fact about the airframe: what it may pull; CLI only). Figures and spins fly AT the load budget: the commanded rotation and the target slew trim down with the measured overload (integral, no droop), and DURING a maneuver the throttle bleeds too - a governed rotation at full power just converts into speed and keeps the load (a = v * omega). Plain holds fighting a gust keep full authority (a governed recovery let the disturbance win). SITL: loop exit pull 13 g -> 7 g peak, zero time above 8 g. - Spin rate cap: full rudder commands at most half a turn per second about the earth vertical, independent of the ACRO yaw rate (an uncapped hot tune commanded 1260 deg/s; a real flat spin is ~330). - Crash detection asks nothing anymore: the impact threshold derives from the detected accelerometer (15% below full-scale - a spike that near saturation is an impact on any airframe, and the stillness that must follow is the discriminator, not the g number). crash_g_threshold (uint8, would not even hold a 32 g IMU's range) is replaced by a boolean crash_detection, ON by default. The SITL fake accel now reports a realistic 16 g scale so the derivation holds on the bench. - Hover / knife / inverted throttle gains derived, five settings gone (ohold_hover_thr_p/i/d, ohold_assist_thr_p/i): every one of them was throttle-us per unit of motion, and they all share one airframe fact, the throttle-to-thrust slope - (hover throttle - idle) per 1 g. The hover point is learned online; the gains anchor on the slow-filtered APPLIED throttle, not the engage seed (a 200 us low seed made the altitude loop 36% too soft in the gust matrix). - Thrust-first-guess authority scaling: above the airframe's cruise throttle the commanded hold authority backs off with thrust (surface moment ~ airflow^2 ~ thrust; no pitot carried, and GPS is gone in aerobatic attitudes). The hover regime always keeps the full throw - its airflow IS the wash. Resolves the 45-deg-3D-throw vs smooth cruise tune dilemma without a setting. - Platform gating: a knife edge is held on the yaw effector, so a mixer without one (flying wing: no rudder, no TVC yaw vane) does not offer the KNIFE boxes, and a stale configuration that still maps them is ignored in flight (servoMixerHasYawControl). - docs/OrientationHold.md verified claim-by-claim against the code (one stale statement fixed: after the last segment the sequencer holds LEVEL with assist until the box is released), Settings.md regenerated. Validated: 50-case gust matrix 49 PASS (the one fail is a pre-existing orientation-sensitive funjet TVC hover case, analysed and documented), full maneuver sweep, multirotor crash test (16 g / 3 ms pulse, cut and gesture-restore), all against a bench plant with servo-honest actuator slew (0.06 s/60 aileron-elevator, 0.10 s/60 rudder, +-15 deg nozzle). --- docs/OrientationHold.md | 121 ++++++++++++++----- docs/Settings.md | 68 +++-------- src/main/drivers/accgyro/accgyro_fake.c | 4 +- src/main/fc/fc_msp_box.c | 10 +- src/main/fc/settings.yaml | 49 ++------ src/main/flight/crash_detection.c | 14 ++- src/main/flight/crash_detection.h | 3 +- src/main/flight/figure_sequencer.c | 44 +++++-- src/main/flight/hover_throttle.c | 64 ++++++++-- src/main/flight/hover_throttle.h | 14 +-- src/main/flight/mixer.c | 4 + src/main/flight/orientation_hold.c | 151 +++++++++++++++++++++++- src/main/flight/orientation_hold.h | 23 ++++ src/main/flight/pid.c | 23 +++- src/main/flight/servos.c | 23 ++++ src/main/flight/servos.h | 4 + 16 files changed, 454 insertions(+), 165 deletions(-) diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md index 3cb364bc2a8..0326257f189 100644 --- a/docs/OrientationHold.md +++ b/docs/OrientationHold.md @@ -17,10 +17,15 @@ hardware flights are upcoming. Treat everything here as experimental. ## Requirements - Fixed wing (`platform_type = AIRPLANE`). Multirotors are untouched. -- A target built with `USE_ORIENTATION_HOLD` (F7/H7 class; F411 fits). +- 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 @@ -39,6 +44,15 @@ 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 @@ -58,6 +72,12 @@ the maneuver. The hold takes over the attitude; you keep flying. - 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 @@ -73,10 +93,10 @@ throttle means: 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 (`ohold_assist_thr_p/i`), 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`). + 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 @@ -102,6 +122,13 @@ 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 @@ -141,10 +168,27 @@ 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 is done - switch back to your normal mode; there is no -automatic level-off yet. +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`) -### Crash detection (`crash_g_threshold`) +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 @@ -154,17 +198,21 @@ moved to its own pull request.* Problem being solved: after an unscheduled arrival the prop keeps churning until you walk over and disarm. -An impact spike above the threshold, 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). Opt-in, 0 = off. -Without GPS, keep the threshold above your figures' g load - a smooth -level line right after a hard pull is indistinguishable from lying -still on IMU + baro alone. +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 +## 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`, @@ -183,14 +231,14 @@ Everything else below is yours to set. 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) +**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 +**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); @@ -198,48 +246,58 @@ 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 +**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 +**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 +**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. The hover altitude itself is a learned gain -(step above) - give it a few hangs to settle. The throttle stick is a +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 +**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. +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`, +**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 altitude assist (`fig_assist_z_gain`, +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 +**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 +## Troubleshooting - symptom to setting | Symptom | Look at | | --- | --- | @@ -250,6 +308,7 @@ the floor must catch and level against the held stick. | 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) | diff --git a/docs/Settings.md b/docs/Settings.md index 04c1d3ba256..ea8f0e8cc62 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -622,13 +622,13 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho --- -### crash_g_threshold +### crash_detection -Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed), CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Without GPS a smooth level line flown within 3 s of a hard pull can read as still - raise the threshold above the figure g load on GPS-less models. 0 = off. +Cut the motor after a crash while staying armed: a sharp acceleration spike near the accelerometer's full-scale, followed by the airframe lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed). Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. The impact threshold is DERIVED from the detected accelerometer (15% below full-scale), not set here - a spike that near saturation is an impact on any airframe, and the stillness that must follow is what tells a crash from a hard 3D figure. ON by default. | Default | Min | Max | | --- | --- | --- | -| 80 | 0 | 160 | +| ON | OFF | ON | --- @@ -4562,26 +4562,6 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within --- -### ohold_assist_thr_i - -Knife edge / inverted throttle assist: trim rate, throttle us per m/s of climb per second. The assist slowly trims the throttle around the pilot's stick until the hold stops sinking (or climbing). 0 disables the assist. - -| Default | Min | Max | -| --- | --- | --- | -| 20 | 0 | 255 | - ---- - -### ohold_assist_thr_p - -Knife edge / inverted throttle assist: damping term, throttle us per m/s of climb rate - -| Default | Min | Max | -| --- | --- | --- | -| 40 | 0 | 255 | - ---- - ### 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 @@ -4622,29 +4602,9 @@ LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes th --- -### ohold_hover_thr_d - -Hover throttle D gain [throttle us per m/s of climb rate] - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 0 | 100 | - ---- - -### ohold_hover_thr_i - -Hover throttle I gain [throttle us per m per second] - -| Default | Min | Max | -| --- | --- | --- | -| 10 | 0 | 100 | - ---- - ### ohold_hover_thr_min -Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. +Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. The altitude/vz loop gains themselves are not settings: they derive at runtime from the learned hover point (throttle-to-thrust slope), see hover_throttle.c. | Default | Min | Max | | --- | --- | --- | @@ -4652,16 +4612,6 @@ Hover throttle floor [us]. The hover altitude controller never cuts the throttle --- -### ohold_hover_thr_p - -Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage) - -| Default | Min | Max | -| --- | --- | --- | -| 85 | 0 | 100 | - ---- - ### ohold_inverted_gain Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains. @@ -4722,6 +4672,16 @@ Knife edge speed feedforward: extra nose-above-horizon angle [deg] per half thro --- +### 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 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/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 1343ac6ff46..318f065dd29 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" @@ -296,8 +297,13 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXANGLEHOLD); #ifdef USE_ORIENTATION_HOLD ADD_ACTIVE_BOX(BOXINVERTED); - ADD_ACTIVE_BOX(BOXKNIFELEFT); - ADD_ACTIVE_BOX(BOXKNIFERIGHT); + // 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(BOXFIGROLL); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3584f9b0c02..9146dd8fbf2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4568,6 +4568,12 @@ groups: field: knifeSpeedFF min: 0 max: 30 + - 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_inverted_gain description: "Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains." default_value: 100 @@ -4664,42 +4670,12 @@ groups: headers: ["flight/hover_throttle.h"] condition: USE_ORIENTATION_HOLD members: - - name: ohold_hover_thr_p - description: "Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage)" - default_value: 85 - field: pGain - min: 0 - max: 100 - - name: ohold_hover_thr_i - description: "Hover throttle I gain [throttle us per m per second]" - default_value: 10 - field: iGain - min: 0 - max: 100 - - name: ohold_hover_thr_d - description: "Hover throttle D gain [throttle us per m/s of climb rate]" - default_value: 100 - field: dGain - min: 0 - max: 100 - name: ohold_hover_thr_min - description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle." + description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. The altitude/vz loop gains themselves are not settings: they derive at runtime from the learned hover point (throttle-to-thrust slope), see hover_throttle.c." default_value: 1000 field: minThrottle min: 1000 max: 1800 - - name: ohold_assist_thr_p - description: "Knife edge / inverted throttle assist: damping term, throttle us per m/s of climb rate" - default_value: 40 - field: assistVzP - min: 0 - max: 255 - - name: ohold_assist_thr_i - description: "Knife edge / inverted throttle assist: trim rate, throttle us per m/s of climb per second. The assist slowly trims the throttle around the pilot's stick until the hold stops sinking (or climbing). 0 disables the assist." - default_value: 20 - field: assistVzI - min: 0 - max: 255 - name: ohold_hover_baro_weight description: "Baro position weight (x100) while the hover throttle owns the altitude, applied as a floor over inav_w_z_baro_p. Hovering thrust pollutes the accelerometer, the baro deserves more trust than in forward flight. 0 keeps the global weight." default_value: 100 @@ -4712,9 +4688,8 @@ groups: headers: ["flight/crash_detection.h"] condition: USE_CRASH_DETECTION members: - - name: crash_g_threshold - description: "Crash detection impact threshold [g x 10]: an acceleration spike above this, followed by the aircraft lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed), CUTS the motor while staying armed. Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. Without GPS a smooth level line flown within 3 s of a hard pull can read as still - raise the threshold above the figure g load on GPS-less models. 0 = off." - default_value: 80 - field: crashGThreshold - min: 0 - max: 160 + - name: crash_detection + description: "Cut the motor after a crash while staying armed: a sharp acceleration spike near the accelerometer's full-scale, followed by the airframe lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed). Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. The impact threshold is DERIVED from the detected accelerometer (15% below full-scale), not set here - a spike that near saturation is an impact on any airframe, and the stillness that must follow is what tells a crash from a hard 3D figure. ON by default." + default_value: ON + field: crashDetection + type: bool diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 8a7a113fd89..beb506fe7aa 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -56,7 +56,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, PG_CRASH_DETECTION_CONFIG, 0); PG_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, - .crashGThreshold = SETTING_CRASH_G_THRESHOLD_DEFAULT, + .crashDetection = SETTING_CRASH_DETECTION_DEFAULT, ); // In-flight latch: the detector must never fire while the armed aircraft is @@ -117,7 +117,7 @@ void crashDetectionUpdate(float dT) // 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. - if (crashDetectionConfig()->crashGThreshold == 0 + if (!crashDetectionConfig()->crashDetection || !(STATE(AIRPLANE) || STATE(MULTIROTOR)) || !ARMING_FLAG(ARMED)) { inFlight = false; @@ -175,8 +175,16 @@ void crashDetectionUpdate(float dT) 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 > crashDetectionConfig()->crashGThreshold / 10.0f) { + if (accMagG > thresholdG) { impactWindowS = CRASH_WINDOW_S; stillTimerS = 0.0f; } diff --git a/src/main/flight/crash_detection.h b/src/main/flight/crash_detection.h index 79b68acbf35..1b9d7098bf1 100644 --- a/src/main/flight/crash_detection.h +++ b/src/main/flight/crash_detection.h @@ -40,7 +40,8 @@ // hand launch, or throttle held above cruise for a moment (both platforms). typedef struct crashDetectionConfig_s { - uint8_t crashGThreshold; // impact threshold [g * 10]; 0 disables + uint8_t crashDetection; // master enable; the impact threshold itself + // is derived from the accel full-scale, not set } crashDetectionConfig_t; PG_DECLARE(crashDetectionConfig_t, crashDetectionConfig); diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index 90f970a7e85..b5bc86ccb2a 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -43,6 +43,7 @@ #include "flight/figure_sequencer.h" #include "flight/imu.h" +#include "flight/orientation_hold.h" #include "navigation/navigation.h" @@ -79,6 +80,10 @@ 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; @@ -140,6 +145,10 @@ float figureAltitudeAssistDeg(float nosePitchDeg, float refAltCm) 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)) { @@ -159,8 +168,20 @@ void figureSequencerUpdate(void) 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; @@ -171,10 +192,10 @@ void figureSequencerUpdate(void) switch (activeFigure) { case FIGURE_ROLL: { - const float theta = figureSequencerConfig()->rollRate * tS; - roll = MIN(theta, 360.0f); + figPhaseDeg += figureSequencerConfig()->rollRate * govScale * phaseDtS; + roll = MIN(figPhaseDeg, 360.0f); assist = true; - if (theta >= 360.0f) { + if (figPhaseDeg >= 360.0f) { state = FIG_STATE_DONE; roll = 0.0f; // 360 == 0, hold level } @@ -182,9 +203,9 @@ void figureSequencerUpdate(void) } case FIGURE_LOOP: { - const float theta = figureSequencerConfig()->loopRate * tS; - pitch = MIN(theta, 360.0f); - if (theta >= 360.0f) { + 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 @@ -209,7 +230,8 @@ void figureSequencerUpdate(void) switch (seg->type) { case FIGSEG_ROLL: { const float span = ABS((float)seg->p1); - const float theta = MIN(figureSequencerConfig()->rollRate * tSeg, span); + 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; @@ -222,7 +244,8 @@ void figureSequencerUpdate(void) case FIGSEG_PITCH: { const float span = ABS((float)seg->p1); - const float theta = MIN(figureSequencerConfig()->loopRate * tSeg, span); + figPhaseDeg += figureSequencerConfig()->loopRate * govScale * phaseDtS; + const float theta = MIN(figPhaseDeg, span); roll = seqBaseRoll; pitch = seqBasePitch + (seg->p1 < 0 ? -theta : theta); assist = false; @@ -352,6 +375,7 @@ void figureSequencerUpdate(void) } 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 } @@ -365,7 +389,9 @@ void figureSequencerUpdate(void) } case FIGURE_POINT_ROLL: { - // 4 points: rotate 90 deg at roll rate, dwell, repeat + // 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; diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 1f359cce5db..1eee342af55 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -47,6 +47,8 @@ #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" @@ -56,18 +58,37 @@ #include "sensors/battery.h" -PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 3); PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, - .pGain = SETTING_OHOLD_HOVER_THR_P_DEFAULT, - .iGain = SETTING_OHOLD_HOVER_THR_I_DEFAULT, - .dGain = SETTING_OHOLD_HOVER_THR_D_DEFAULT, .minThrottle = SETTING_OHOLD_HOVER_THR_MIN_DEFAULT, - .assistVzP = SETTING_OHOLD_ASSIST_THR_P_DEFAULT, - .assistVzI = SETTING_OHOLD_ASSIST_THR_I_DEFAULT, .hoverBaroWeight = SETTING_OHOLD_HOVER_BARO_WEIGHT_DEFAULT, ); +// 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 @@ -95,6 +116,12 @@ 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 ---------------------------------------- // @@ -151,7 +178,6 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) { // a deliberate throttle cut stays a throttle cut if (!navIsAltitudeEstimateTrusted() - || hoverThrottleConfig()->assistVzI == 0 || pilotThrottle < getThrottleIdleValue() + 50) { assistActive = false; return pilotThrottle; @@ -176,9 +202,13 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) 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 - hoverThrottleConfig()->assistVzI * climbMs * dT, + 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 @@ -198,7 +228,7 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) assistTrimUs = constrainf(assistTrimUs + ASSIST_EFFORT_RAISE_US_S * urgency * dT, -ASSIST_TRIM_MAX_US, ASSIST_TRIM_MAX_US); } - const float damping = -hoverThrottleConfig()->assistVzP + const float damping = -ASSIST_DAMPING_S * assistUsPerG / GRAVITY_MSS * constrainf(climbMs, -ASSIST_VZ_CLAMP_MS, ASSIST_VZ_CLAMP_MS); return constrain(lrintf(baseUs + assistTrimUs + damping), @@ -273,6 +303,7 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) // 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 @@ -324,17 +355,24 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) const int16_t floorThrottle = MAX(getThrottleIdleValue(), (int16_t)hoverThrottleConfig()->minThrottle); - iTermUs = constrainf(iTermUs + hoverThrottleConfig()->iGain * zErrM * dT, + // 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); + iTermUs = constrainf(iTermUs + HOVER_THR_TRIM_S2 * spanUsPerG / GRAVITY_MSS * zErrM * dT, floorThrottle, getMaxThrottle()); // 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 = (hoverThrottleConfig()->pGain * zErrM - - hoverThrottleConfig()->dGain * climbMs) / vertical; + const float correction = (HOVER_THR_STIFFNESS_S2 * spanUsPerG / GRAVITY_MSS * zErrM + - HOVER_THR_DAMPING_S * spanUsPerG / GRAVITY_MSS * climbMs) / vertical; - return constrain(lrintf(iTermUs + correction), floorThrottle, getMaxThrottle()); + const int16_t outUs = constrain(lrintf(iTermUs + correction), floorThrottle, getMaxThrottle()); + hoverThrAnchorUs += (outUs - hoverThrAnchorUs) * MIN(dT / HOVER_THR_ANCHOR_TAU_S, 1.0f); + return outUs; } #endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h index 7db0c29d07c..ca0f57d6e04 100644 --- a/src/main/flight/hover_throttle.h +++ b/src/main/flight/hover_throttle.h @@ -35,10 +35,14 @@ // 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 pGain; // throttle us per m of altitude error - uint8_t iGain; // throttle us per m per second - uint8_t dGain; // throttle us per m/s of climb rate uint16_t minThrottle; // throttle floor [us] while hovering: preserves // the control authority that scales with thrust, // prop wash over the surfaces as well as thrust @@ -48,10 +52,6 @@ typedef struct hoverThrottleConfig_s { // covers both steering paths. Found by // experiment near the model's hover throttle; // 1000 = no floor beyond the motor idle. - uint8_t assistVzP; // knife/inverted throttle assist: us per m/s of - // climb rate (damping) - uint8_t assistVzI; // knife/inverted throttle assist: trim rate, - // us per m/s per second; 0 disables the assist uint8_t hoverBaroWeight; // baro position weight (x100) while the hover // throttle owns the altitude: hovering thrust // pollutes the accelerometer Z, the baro diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 15b6caddae4..b5cf31c9044 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -49,6 +49,7 @@ #include "flight/altitude_floor.h" #include "flight/crash_detection.h" #include "flight/hover_throttle.h" +#include "flight/orientation_hold.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -594,6 +595,9 @@ void FAST_CODE mixTable(void) #ifdef USE_ORIENTATION_HOLD // 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) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index acd162b17d5..a6cd1de5fc5 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -45,15 +45,23 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "drivers/time.h" + #include "flight/altitude_floor.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" -PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 1); +#include "flight/servos.h" + +#include "sensors/acceleration.h" +#include "sensors/battery.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 2); PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT, @@ -67,6 +75,7 @@ PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .stickAngleMaxDeg = SETTING_OHOLD_STICK_ANGLE_DEFAULT, .stickReturnRateDps = SETTING_OHOLD_STICK_RETURN_RATE_DEFAULT, .knifeSpeedFF = SETTING_OHOLD_KNIFE_SPEED_FF_DEFAULT, + .loadLimitG = SETTING_OHOLD_LOAD_LIMIT_DEFAULT, ); typedef struct { @@ -103,14 +112,23 @@ static const orientationHoldPreset_t orientationHoldSpinPresets[] = { 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)) return &orientationHoldSpinPresets[1]; - if (IS_RC_MODE_ACTIVE(BOXKNIFERIGHT)) return &orientationHoldSpinPresets[2]; + if (IS_RC_MODE_ACTIVE(BOXKNIFELEFT) && knifePossible) return &orientationHoldSpinPresets[1]; + if (IS_RC_MODE_ACTIVE(BOXKNIFERIGHT) && knifePossible) return &orientationHoldSpinPresets[2]; if (IS_RC_MODE_ACTIVE(BOXPROPHANG)) return &orientationHoldSpinPresets[3]; return &orientationHoldSpinPresets[4]; } 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]; } @@ -937,9 +955,14 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } 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 - ? orientationHoldSlewTargetFull(&qSollState, &qDesired, slewRateDegS * dT) - : orientationHoldSlewTarget(&qSollState, &qDesired, slewRateDegS * dT); + ? orientationHoldSlewTargetFull(&qSollState, &qDesired, governedStepDeg) + : orientationHoldSlewTarget(&qSollState, &qDesired, governedStepDeg); if (remainingDeg < 1.0f) { presetSlewCaptured = true; } @@ -952,4 +975,122 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float 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 +#define AUTHORITY_HOVER_ELEVATION_DEG 45.0f + +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); +} + #endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 394fec18562..45b808136cd 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -73,6 +73,9 @@ typedef struct orientationHoldConfig_s { // 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); @@ -150,3 +153,23 @@ bool orientationHoldSticksAreTargetOffsets(void); // 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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9435ad36ac5..74176b247da 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -784,6 +784,16 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) // the pilot's rudder rate command becomes the spin rate spinRateDps = pidStates[FD_YAW].rateTarget; } + // 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(); if (spinSegment || spinPreset) { orientationHoldUpInBody(&upBody); // AIRCRAFT-referenced stick sense: the body axis nearest the @@ -805,11 +815,20 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) } } + // 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, - currentControlProfile->stabilized.rates[axis] * 10.0f); + -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() diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 046c454b144..a5f11a3d040 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -205,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) { diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 6b2872ceb08..cc2ee80c29b 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -204,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); From f7646cda2d455c6655d32b7d9a9dfe9e08673f54 Mon Sep 17 00:00:00 2001 From: pdani Date: Thu, 16 Jul 2026 00:29:03 +0200 Subject: [PATCH 069/108] crash detection: fix no-baro builds (-Werror unused) The fresh CI run on GEPRC_F722_AIO_UART3 (no USE_BARO) caught it: crashVerticalRateCms' dT parameter and the baro statics are unused when the baro path compiles out. Statics and their reset now live under USE_BARO, the no-baro path marks dT unused and falls through to the fused estimate as before. Verified: the failing CI target builds clean. --- src/main/flight/crash_detection.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index beb506fe7aa..4eebdb604ac 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -30,6 +30,7 @@ #ifdef USE_CRASH_DETECTION #include "common/maths.h" +#include "common/utils.h" #include "common/vector.h" #include "config/parameter_group.h" @@ -94,8 +95,10 @@ 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) { @@ -107,6 +110,8 @@ static float crashVerticalRateCms(float dT) 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); } @@ -126,8 +131,8 @@ void crashDetectionUpdate(float dT) stillTimerS = 0.0f; motorCut = false; cutAckLow = false; - baroRateCms = 0.0f; #ifdef USE_BARO + baroRateCms = 0.0f; lastBaroAltCm = baro.BaroAlt; #endif return; From 59638db1a7e38df51a9ede0df167edb02e527e6f Mon Sep 17 00:00:00 2001 From: pdani Date: Thu, 16 Jul 2026 00:29:31 +0200 Subject: [PATCH 070/108] smix: widen the speed limit to uint16 - declare a real servo's ceiling From the servo discussion on the PR (Jetrell, sensei-hacker): the smix speed field is the right low-pass for the FC output - commanded and physical surface trajectory then agree instead of the servo silently amplitude-clipping fast command content (command x, reverse before the servo arrives, and the surface only ever saw x/2). And sometimes less than the servo's full rate is wanted deliberately - flaps are the classic case, deployed slowly on purpose because the aerodynamic load rises with v^2 and can overload mounts, tracks and the actuator. But the field was uint8 in 10 us/s units: even 255 means a full sweep in 0.39 s (~0.20 s/60 deg). A mid-range digital servo (TowerPro MG92B, 0.08 s/60 deg - the servo on our maiden airframe) needs ~625; the fastest aerobatic HV class (0.05 s/60) needs ~833. Not declarable. speed becomes uint16 with MAX_SERVO_SPEED 1000 (full sweep in 0.1 s = the fastest servo class; beyond that no servo follows). Backward compatible by construction: same unit, so every stored value keeps its meaning; the CLI is text and takes the full range; the legacy MSP messages stay byte-identical and clamp the reported value to 255 - an old configurator shows 255 and can write at most 255, nothing is corrupted. A 16-bit MSP2 message plus the configurator field follow in their own focused PR. Mixer profile PG version bumped. --- docs/Mixer.md | 2 +- src/main/fc/fc_msp.c | 6 +++--- src/main/flight/mixer_profile.c | 2 +- src/main/flight/servos.h | 14 +++++++++++--- 4 files changed, 16 insertions(+), 8 deletions(-) diff --git a/docs/Mixer.md b/docs/Mixer.md index eb2901295a3..05adb774f61 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -49,7 +49,7 @@ Each servo mixing rule has the following parameters: * Servo index: defines which servo the rule will apply to. The absolute value of the index is not important, what matters is only the relative difference between the used indexes. The rule with the smaller servo index will apply to the first servo, the next higher servo index to the second servo, etc. More than one rule can use the same servo index. The output of the rules with the same servo index are added together to give the final output for the specified servo. * Input: the input for the mixing rule, see a summary of the input types table bellow. * Weight: percentage of the input to forward to the servo. Range [-1000, 1000]. Mixing rule output = input * weight. If the output of a set of mixing rules is lower/higher than the defined servo min/max the output is clipped (the servo will never travel farther than the set min/max). -* Speed: maximum rate of change of the mixing rule output. Used to limit the servo speed. 1 corresponds to maximum 10µs/s output rate of change. Set to 0 for no speed limit. For example: 10 = full sweep (1000 to 2000) in 10s, 100 = full sweep in 1s. +* Speed: maximum rate of change of the mixing rule output. Used to limit the servo speed. 1 corresponds to maximum 10µs/s output rate of change. Set to 0 for no speed limit. For example: 10 = full sweep (1000 to 2000) in 10s, 100 = full sweep in 1s, 625 = full sweep in 0.16s (a 0.08 s/60 deg digital servo's real ceiling). Maximum is 1000 (full sweep in 0.1 s, the fastest aerobatic HV servo class). Values above 255 are CLI-only for now: the legacy MSP messages report them clamped to 255, so an older configurator shows 255 and can only write up to 255 - existing setups are unaffected. | CLI input ID | Mixer input | Description | |----|--------------------------|------------------------------------------------------------------------------| diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cdb2d486fd5..025aefaf11b 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -561,7 +561,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, customServoMixers(i)->targetChannel); sbufWriteU8(dst, customServoMixers(i)->inputSource); sbufWriteU16(dst, customServoMixers(i)->rate); - sbufWriteU8(dst, customServoMixers(i)->speed); + sbufWriteU8(dst, MIN(customServoMixers(i)->speed, 255)); // legacy message stays 8-bit, clamped sbufWriteU8(dst, 0); sbufWriteU8(dst, 100); sbufWriteU8(dst, 0); @@ -584,7 +584,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, customServoMixers(i)->targetChannel); sbufWriteU8(dst, customServoMixers(i)->inputSource); sbufWriteU16(dst, customServoMixers(i)->rate); - sbufWriteU8(dst, customServoMixers(i)->speed); + sbufWriteU8(dst, MIN(customServoMixers(i)->speed, 255)); // legacy message stays 8-bit, clamped #ifdef USE_PROGRAMMING_FRAMEWORK sbufWriteU8(dst, customServoMixers(i)->conditionId); #else @@ -596,7 +596,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel); sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource); sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate); - sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed); + sbufWriteU8(dst, MIN(mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed, 255)); // legacy message stays 8-bit, clamped #ifdef USE_PROGRAMMING_FRAMEWORK sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId); #else diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index a39fbfeedd9..bea7919579a 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -37,7 +37,7 @@ bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 2); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index cc2ee80c29b..8359a279d9b 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -133,14 +133,22 @@ typedef struct servoMixer_s { uint8_t targetChannel; // servo that receives the output of the rule uint8_t inputSource; // input channel for this rule int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction - uint8_t speed; // reduces the speed of the rule, 0=unlimited speed + uint16_t speed; // limits the speed of the rule, in 10 us/s; + // 0 = unlimited. uint16 so a real servo's + // ceiling is expressible (0.08 s/60 deg + // needs ~625; the old uint8 topped out at + // ~0.20 s/60 deg). Legacy MSP messages + // clamp to 255 on output, CLI is unaffected. #ifdef USE_PROGRAMMING_FRAMEWORK int8_t conditionId; #endif } servoMixer_t; #define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) -#define MAX_SERVO_SPEED UINT8_MAX +// 1000 = 10000 us/s = full sweep in 0.1 s (~0.05 s/60 deg): the fastest +// aerobatic HV servo class - values beyond that command nothing a servo +// can follow +#define MAX_SERVO_SPEED 1000 #define SERVO_OUTPUT_MAX 2500 #define SERVO_OUTPUT_MIN 500 @@ -150,7 +158,7 @@ typedef struct servoMixerSwitch_s { //this is used to keep track of servoSpeedLimitFilter of servo rules during the mixer switch uint8_t targetChannel; // servo that receives the output of the rule int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction - uint8_t speed; // reduces the speed of the rule, 0=unlimited speed + uint16_t speed; // limits the speed of the rule, 0=unlimited (see servoMixer_t) float speedLimitFilterState; // rate limit filter for this rule } servoMixerSwitch_t; #define MAX_SERVO_RULES_SWITCH_CARRY (MAX_SERVO_RULES / 2) From c27620e51df81b3ecac36c63ead2171fcca817d3 Mon Sep 17 00:00:00 2001 From: pdani Date: Thu, 16 Jul 2026 10:06:41 +0200 Subject: [PATCH 071/108] Revert "smix: widen the speed limit to uint16 - declare a real servo's ceiling" This reverts commit 59638db1a7e38df51a9ede0df167edb02e527e6f. --- docs/Mixer.md | 2 +- src/main/fc/fc_msp.c | 6 +++--- src/main/flight/mixer_profile.c | 2 +- src/main/flight/servos.h | 14 +++----------- 4 files changed, 8 insertions(+), 16 deletions(-) diff --git a/docs/Mixer.md b/docs/Mixer.md index 05adb774f61..eb2901295a3 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -49,7 +49,7 @@ Each servo mixing rule has the following parameters: * Servo index: defines which servo the rule will apply to. The absolute value of the index is not important, what matters is only the relative difference between the used indexes. The rule with the smaller servo index will apply to the first servo, the next higher servo index to the second servo, etc. More than one rule can use the same servo index. The output of the rules with the same servo index are added together to give the final output for the specified servo. * Input: the input for the mixing rule, see a summary of the input types table bellow. * Weight: percentage of the input to forward to the servo. Range [-1000, 1000]. Mixing rule output = input * weight. If the output of a set of mixing rules is lower/higher than the defined servo min/max the output is clipped (the servo will never travel farther than the set min/max). -* Speed: maximum rate of change of the mixing rule output. Used to limit the servo speed. 1 corresponds to maximum 10µs/s output rate of change. Set to 0 for no speed limit. For example: 10 = full sweep (1000 to 2000) in 10s, 100 = full sweep in 1s, 625 = full sweep in 0.16s (a 0.08 s/60 deg digital servo's real ceiling). Maximum is 1000 (full sweep in 0.1 s, the fastest aerobatic HV servo class). Values above 255 are CLI-only for now: the legacy MSP messages report them clamped to 255, so an older configurator shows 255 and can only write up to 255 - existing setups are unaffected. +* Speed: maximum rate of change of the mixing rule output. Used to limit the servo speed. 1 corresponds to maximum 10µs/s output rate of change. Set to 0 for no speed limit. For example: 10 = full sweep (1000 to 2000) in 10s, 100 = full sweep in 1s. | CLI input ID | Mixer input | Description | |----|--------------------------|------------------------------------------------------------------------------| diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 025aefaf11b..cdb2d486fd5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -561,7 +561,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, customServoMixers(i)->targetChannel); sbufWriteU8(dst, customServoMixers(i)->inputSource); sbufWriteU16(dst, customServoMixers(i)->rate); - sbufWriteU8(dst, MIN(customServoMixers(i)->speed, 255)); // legacy message stays 8-bit, clamped + sbufWriteU8(dst, customServoMixers(i)->speed); sbufWriteU8(dst, 0); sbufWriteU8(dst, 100); sbufWriteU8(dst, 0); @@ -584,7 +584,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, customServoMixers(i)->targetChannel); sbufWriteU8(dst, customServoMixers(i)->inputSource); sbufWriteU16(dst, customServoMixers(i)->rate); - sbufWriteU8(dst, MIN(customServoMixers(i)->speed, 255)); // legacy message stays 8-bit, clamped + sbufWriteU8(dst, customServoMixers(i)->speed); #ifdef USE_PROGRAMMING_FRAMEWORK sbufWriteU8(dst, customServoMixers(i)->conditionId); #else @@ -596,7 +596,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel); sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource); sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate); - sbufWriteU8(dst, MIN(mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed, 255)); // legacy message stays 8-bit, clamped + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed); #ifdef USE_PROGRAMMING_FRAMEWORK sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId); #else diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index bea7919579a..a39fbfeedd9 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -37,7 +37,7 @@ bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 8359a279d9b..cc2ee80c29b 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -133,22 +133,14 @@ typedef struct servoMixer_s { uint8_t targetChannel; // servo that receives the output of the rule uint8_t inputSource; // input channel for this rule int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction - uint16_t speed; // limits the speed of the rule, in 10 us/s; - // 0 = unlimited. uint16 so a real servo's - // ceiling is expressible (0.08 s/60 deg - // needs ~625; the old uint8 topped out at - // ~0.20 s/60 deg). Legacy MSP messages - // clamp to 255 on output, CLI is unaffected. + uint8_t speed; // reduces the speed of the rule, 0=unlimited speed #ifdef USE_PROGRAMMING_FRAMEWORK int8_t conditionId; #endif } servoMixer_t; #define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) -// 1000 = 10000 us/s = full sweep in 0.1 s (~0.05 s/60 deg): the fastest -// aerobatic HV servo class - values beyond that command nothing a servo -// can follow -#define MAX_SERVO_SPEED 1000 +#define MAX_SERVO_SPEED UINT8_MAX #define SERVO_OUTPUT_MAX 2500 #define SERVO_OUTPUT_MIN 500 @@ -158,7 +150,7 @@ typedef struct servoMixerSwitch_s { //this is used to keep track of servoSpeedLimitFilter of servo rules during the mixer switch uint8_t targetChannel; // servo that receives the output of the rule int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction - uint16_t speed; // limits the speed of the rule, 0=unlimited (see servoMixer_t) + uint8_t speed; // reduces the speed of the rule, 0=unlimited speed float speedLimitFilterState; // rate limit filter for this rule } servoMixerSwitch_t; #define MAX_SERVO_RULES_SWITCH_CARRY (MAX_SERVO_RULES / 2) From c3b94117baf1daaa1af40aa1db823bf4e9e6485d Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 17 Jul 2026 09:50:48 +0200 Subject: [PATCH 072/108] ROTOR GUARD: autogyro tip-over catch (box, altitude-floor pattern) An autogyro's rotor is its wing: lift AND lateral-tilt roll authority scale with rotor rpm squared, and the rpm lives on the inflow through the disk. In slow flight or a botched launch the rpm decays, the tilt goes soft, and the aircraft rolls away with the stick at the stop - attitude control alone cannot recover it, and these airframes carry no rpm feedback. The ONE lever that restores authority is thrust (thrust -> speed -> inflow -> rpm). The guard is a box (ROTOR GUARD, permanent id 80) following the altitude-floor pattern. Detector: bank beyond rotor_guard_bank (default 60 deg - an autogyro never flies that on purpose) WHILE sinking past rotor_guard_sink, sustained 300 ms; the sink condition keeps deliberate maneuvers (climbing half-loop turns) from tripping it. Recovery: orientation-hold target wings-level plus rotor_guard_pitch nose DOWN (feed the disk), throttle floor cruise + rotor_guard_throttle_add (deliberately NOT pitch-scaled - the nose is down, pitch-to-throttle would starve the one lever that matters), motor forced running through a low stick, roll/pitch sticks suppressed like the floor (yaw stays steering). Release: bank under 20 deg and sink arrested, or stick takeover (centered once, then a fresh deflection). The altitude floor outranks the guard: height beats rotor rpm. Status: compiles and links (SITL 319/319); the bench proof against the JSBSim Auto-G2 rotor-rpm plant model (tip-over reproduced without the guard, caught with it) is the next step. --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_core.c | 9 +- src/main/fc/fc_msp_box.c | 3 + src/main/fc/rc_modes.h | 1 + src/main/fc/settings.yaml | 30 ++++++ src/main/flight/hover_throttle.c | 11 +++ src/main/flight/mixer.c | 7 +- src/main/flight/orientation_hold.c | 10 ++ src/main/flight/pid.c | 4 +- src/main/flight/rotor_guard.c | 127 ++++++++++++++++++++++++++ src/main/flight/rotor_guard.h | 52 +++++++++++ 12 files changed, 252 insertions(+), 7 deletions(-) create mode 100644 src/main/flight/rotor_guard.c create mode 100644 src/main/flight/rotor_guard.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 163ddb11e89..e4634dcad51 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -335,6 +335,8 @@ main_sources(COMMON_SRC flight/rate_dynamics.h flight/altitude_floor.c flight/altitude_floor.h + flight/rotor_guard.c + flight/rotor_guard.h flight/crash_detection.c flight/crash_detection.h flight/hover_throttle.c diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index d0062f26573..45cc6e0dfa2 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -139,7 +139,8 @@ #define PG_FIGURE_SEQUENCE 1049 #define PG_HOVER_THROTTLE_CONFIG 1050 #define PG_CRASH_DETECTION_CONFIG 1051 -#define PG_INAV_END PG_CRASH_DETECTION_CONFIG +#define PG_ROTOR_GUARD_CONFIG 1052 +#define PG_INAV_END PG_ROTOR_GUARD_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 2ccd54fc602..ee9a5d97cba 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -88,6 +88,7 @@ #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/orientation_hold.h" @@ -705,6 +706,7 @@ void processRx(timeUs_t currentTimeUs) #ifdef USE_ORIENTATION_HOLD DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); altitudeFloorUpdate(); + rotorGuardUpdate(); figureSequencerUpdate(); #endif @@ -712,9 +714,10 @@ void processRx(timeUs_t currentTimeUs) if (autoEnableAngle) { ENABLE_FLIGHT_MODE(ANGLE_MODE); #ifdef USE_ORIENTATION_HOLD - } else if (STATE(AIRPLANE) && altitudeFloorRecoveryActive()) { - // Automatic floor recovery: upright + climb, overrides the pilot's - // stabilised mode selection until back above the floor + } else if (STATE(AIRPLANE) && (altitudeFloorRecoveryActive() || rotorGuardRecoveryActive())) { + // Automatic safety recovery (altitude floor, or the autogyro + // tip-over guard): overrides the pilot's stabilised mode + // selection until the aircraft is caught ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); #endif } else if (IS_RC_MODE_ACTIVE(BOXANGLE)) { diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 318f065dd29..369f72a543b 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -121,6 +121,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .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 } }; @@ -306,6 +307,7 @@ void initActiveBoxIds(void) } ADD_ACTIVE_BOX(BOXPROPHANG); ADD_ACTIVE_BOX(BOXALTFLOOR); + ADD_ACTIVE_BOX(BOXROTORGUARD); ADD_ACTIVE_BOX(BOXFIGROLL); ADD_ACTIVE_BOX(BOXFIGLOOP); ADD_ACTIVE_BOX(BOXFIGPOINTROLL); @@ -485,6 +487,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 58b296b4958..56562f384cf 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -96,6 +96,7 @@ typedef enum { BOXFIGSEQ = 68, BOXATTLOCK = 69, BOXFSPIN = 70, + BOXROTORGUARD = 71, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9146dd8fbf2..80cc40977b5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4527,6 +4527,36 @@ groups: min: 5 max: 45 + - name: PG_ROTOR_GUARD_CONFIG + type: rotorGuardConfig_t + headers: ["flight/rotor_guard.h"] + condition: USE_ORIENTATION_HOLD + 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." + 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, negative = nose down: feeds the disk (inflow -> rotor rpm -> authority)" + default_value: -5 + field: recoveryPitchDeg + min: -20 + max: 10 + - name: rotor_guard_throttle_add + description: "Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins; the motor keeps running through a low stick during recovery." + default_value: 250 + field: throttleAddUs + min: 0 + max: 800 + - name: PG_THRUST_VECTORING_CONFIG type: thrustVectoringConfig_t headers: ["flight/thrust_vectoring.h"] diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 1eee342af55..9ab5c4cda28 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -44,6 +44,7 @@ #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" @@ -283,6 +284,16 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) return constrain(MAX(pilotThrottle, climbThrottle), getThrottleIdleValue(), getMaxThrottle()); } + // autogyro tip-over recovery: thrust is the ONLY lever that brings + // the rotor rpm (and with it the roll authority) back - a fixed + // floor above cruise, NOT pitch-scaled (the recovery pitch is nose + // DOWN, pitch-to-throttle would reduce it); more pilot throttle wins + if (ARMING_FLAG(ARMED) && rotorGuardRecoveryActive()) { + const int16_t guardThrottle = currentBatteryProfile->nav.fw.cruise_throttle + + rotorGuardConfig()->throttleAddUs; + return constrain(MAX(pilotThrottle, guardThrottle), + getThrottleIdleValue(), getMaxThrottle()); + } return pilotThrottle; } assistActive = false; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b5cf31c9044..90270bc2e4f 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -47,6 +47,7 @@ #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" @@ -705,8 +706,10 @@ motorStatus_e getMotorStatus(void) #ifdef USE_ORIENTATION_HOLD // the altitude floor recovery climbs on its own throttle floor - a // panic-chopped stick must not stop the motor that climb needs (the - // same override navigation gets via nav_overrides_motor_stop) - if (STATE(AIRPLANE) && altitudeFloorRecoveryActive()) { + // same override navigation gets via nav_overrides_motor_stop). The + // rotor guard recovery NEEDS the motor even more: thrust is its + // only means of restoring rotor rpm. + if (STATE(AIRPLANE) && (altitudeFloorRecoveryActive() || rotorGuardRecoveryActive())) { return MOTOR_RUNNING; } #endif diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index a6cd1de5fc5..92a0485e8ae 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -48,6 +48,7 @@ #include "drivers/time.h" #include "flight/altitude_floor.h" +#include "flight/rotor_guard.h" #include "flight/figure_sequencer.h" #include "navigation/navigation.h" @@ -434,6 +435,7 @@ static void orientationHoldRegulate(fpVector3_t *errDeg) #define OHOLD_SOURCE_FIGURE (-3) #define OHOLD_SOURCE_LOCK (-4) #define OHOLD_SOURCE_EXIT (-5) +#define OHOLD_SOURCE_ROTOR (-6) // Exit handover thresholds: engage only when the released attitude is far // enough from level that the instant Euler error would command full rates; @@ -819,6 +821,14 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FLOOR); orientationHoldTargetFromRP(&qDesired, 0.0f, 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()) { float figRoll, figPitch; orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 74176b247da..75f80eb62b9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -44,6 +44,7 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/altitude_floor.h" +#include "flight/rotor_guard.h" #include "flight/figure_sequencer.h" #include "flight/mixer_profile.h" #include "flight/orientation_hold.h" @@ -765,7 +766,8 @@ static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT) // 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(); + || altitudeFloorRecoveryActive() + || rotorGuardRecoveryActive(); // controlled spin (FLAT SPIN family or figure SPIN segment): the spin // command is a rotation about the EARTH VERTICAL - exactly the axis the // reduced attitude error leaves free - distributed onto the body axes diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c new file mode 100644 index 00000000000..6a47f8fe9aa --- /dev/null +++ b/src/main/flight/rotor_guard.c @@ -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/. + */ + +#include + +#include + +#ifdef USE_ORIENTATION_HOLD + +#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/rotor_guard.h" + +#include "navigation/navigation.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, PG_ROTOR_GUARD_CONFIG, 0); + +PG_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, + .bankDeg = SETTING_ROTOR_GUARD_BANK_DEFAULT, + .sinkCms = SETTING_ROTOR_GUARD_SINK_DEFAULT, + .recoveryPitchDeg = SETTING_ROTOR_GUARD_PITCH_DEFAULT, + .throttleAddUs = SETTING_ROTOR_GUARD_THROTTLE_ADD_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 AND no longer sinking +#define ROTOR_GUARD_RELEASE_BANK_DEG 20 + +static bool guardRecovery = false; +static timeMs_t tripStartMs = 0; +static bool sticksSeenCentered = false; + +void rotorGuardUpdate(void) +{ + if (!IS_RC_MODE_ACTIVE(BOXROTORGUARD) || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE)) { + 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; + sticksSeenCentered = false; + } + } else { + tripStartMs = 0; + } + } else { + // Release when the tilt authority is visibly back: wings level-ish + // and the sink arrested (the thrust floor restored the inflow) + if (bankDeg < ROTOR_GUARD_RELEASE_BANK_DEG && vz >= 0.0f) { + guardRecovery = false; + tripStartMs = 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; +} + +float rotorGuardRecoveryPitchDeg(void) +{ + return (float)rotorGuardConfig()->recoveryPitchDeg; +} + +#endif // USE_ORIENTATION_HOLD diff --git a/src/main/flight/rotor_guard.h b/src/main/flight/rotor_guard.h new file mode 100644 index 00000000000..5a6b5df0555 --- /dev/null +++ b/src/main/flight/rotor_guard.h @@ -0,0 +1,52 @@ +/* + * 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; // nose-down pitch target during recovery + // (negative = down): restores inflow + uint16_t throttleAddUs; // recovery throttle floor = cruise + this +} rotorGuardConfig_t; + +PG_DECLARE(rotorGuardConfig_t, rotorGuardConfig); + +void rotorGuardUpdate(void); +bool rotorGuardRecoveryActive(void); +float rotorGuardRecoveryPitchDeg(void); From 38f4223566568d1c9caf2b5bbb239eb83c27c7bc Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 17 Jul 2026 12:39:30 +0200 Subject: [PATCH 073/108] ROTOR GUARD: three fixes from the bench proof flights All three measured against the JSBSim Auto-G2 rotor-rpm plant: (1) nose-down only while the excursion persists - once the wings answer, the recovery levels off; a T/W<1 autogyro can never climb nose-down and the old release condition became unreachable. (2) release redesigned: wings held level for 1.5 s, NOT sink arrested - height is the altitude floor's job, and the gyro settles in a slow descent at any sane recovery attitude (the old condition pinned the recovery active into the ground). The box stays armed and simply re-trips on the next excursion. (3) minimum 5 s on the recovery before any release: the wings answering is the catch dynamics, not a healthy rotor - the rpm rebuilds from inflow over seconds, and an early release re-tipped DEEPER (-48 caught, released, -138 into the terrain, measured). Time on the throttle floor is the honest rpm proxy without rpm feedback. Proof: starved from 80 m, two catches, minimum altitude 17 m, ends flying level at ias 16 with the throttle returned. The unprotected contrast flight tips to 154 deg and impacts. --- src/main/flight/rotor_guard.c | 47 +++++++++++++++++++++++++++++------ 1 file changed, 40 insertions(+), 7 deletions(-) diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c index 6a47f8fe9aa..c5d26cd443c 100644 --- a/src/main/flight/rotor_guard.c +++ b/src/main/flight/rotor_guard.c @@ -58,11 +58,20 @@ PG_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, // 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 AND no longer sinking +// 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 static bool guardRecovery = false; static timeMs_t tripStartMs = 0; +static timeMs_t recoveryStartMs = 0; +static timeMs_t levelSinceMs = 0; static bool sticksSeenCentered = false; void rotorGuardUpdate(void) @@ -87,17 +96,32 @@ void rotorGuardUpdate(void) tripStartMs = millis(); } else if (millis() - tripStartMs > ROTOR_GUARD_TRIP_MS) { guardRecovery = true; + recoveryStartMs = millis(); + levelSinceMs = 0; sticksSeenCentered = false; } } else { tripStartMs = 0; } } else { - // Release when the tilt authority is visibly back: wings level-ish - // and the sink arrested (the thrust floor restored the inflow) - if (bankDeg < ROTOR_GUARD_RELEASE_BANK_DEG && vz >= 0.0f) { - guardRecovery = false; - tripStartMs = 0; + // 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 @@ -121,7 +145,16 @@ bool rotorGuardRecoveryActive(void) float rotorGuardRecoveryPitchDeg(void) { - return (float)rotorGuardConfig()->recoveryPitchDeg; + // Nose-down only while the roll excursion persists: it exists to feed + // the disk while the tilt has no authority. Once the wings answer + // again the recovery levels off - a T/W<1 autogyro can never climb + // nose-down, and the release condition (sink arrested) would + // otherwise never arrive (measured: a stable 1.2 m/s descent all the + // way into the ground). + if (ABS(attitude.values.roll) / 10.0f > ROTOR_GUARD_RELEASE_BANK_DEG + 10) { + return (float)rotorGuardConfig()->recoveryPitchDeg; + } + return 0.0f; } #endif // USE_ORIENTATION_HOLD From c8a034e0f019a9395bddb4b88ab21bdef7d0b939 Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 17 Jul 2026 18:08:53 +0200 Subject: [PATCH 074/108] SITL: safety-state word in debug slot 7; guard tuning documented The bench replay and gates could not SEE when a recovery owns the aircraft - the box readback only shows ARMED switches, so a figure silently flown under floor override would fake that figure's proof. fc_core publishes floor-armed / floor-recovery / rotor-guard-recovery as a bitmask in debug slot 7 (cycles through the simulator reply at 125 Hz); altitude_floor exports the armed state. The rotor guard setting descriptions now carry the SITL-proven Auto-G2 tuning (bank 45, throttle_add 380) and the reason the defaults differ. --- src/main/fc/fc_core.c | 7 +++++++ src/main/fc/settings.yaml | 4 ++-- src/main/flight/altitude_floor.c | 5 +++++ src/main/flight/altitude_floor.h | 3 +++ 4 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ee9a5d97cba..25a681944ac 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -708,6 +708,13 @@ void processRx(timeUs_t currentTimeUs) altitudeFloorUpdate(); rotorGuardUpdate(); figureSequencerUpdate(); + // 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 + debug[7] = (altitudeFloorArmed() ? 1 : 0) + | (altitudeFloorRecoveryActive() ? 2 : 0) + | (rotorGuardRecoveryActive() ? 4 : 0); #endif if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 80cc40977b5..7595e052c1a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4533,7 +4533,7 @@ groups: condition: USE_ORIENTATION_HOLD 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." + 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 @@ -4551,7 +4551,7 @@ groups: min: -20 max: 10 - name: rotor_guard_throttle_add - description: "Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins; the motor keeps running through a low stick during recovery." + description: "Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins; the motor keeps running through a low stick during recovery. Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink)." default_value: 250 field: throttleAddUs min: 0 diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index b7867850f68..387ff76ead9 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -114,6 +114,11 @@ bool altitudeFloorRecoveryActive(void) return floorRecovery; } +bool altitudeFloorArmed(void) +{ + return floorArmed; +} + float altitudeFloorRecoveryPitchDeg(void) { return (float)altitudeFloorConfig()->floorClimbPitch; diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h index 1f35703dd18..c8a35c96958 100644 --- a/src/main/flight/altitude_floor.h +++ b/src/main/flight/altitude_floor.h @@ -49,5 +49,8 @@ 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) float altitudeFloorRecoveryPitchDeg(void); From 53acaa7dd455499cc797fa60eaf069008172f7f1 Mon Sep 17 00:00:00 2001 From: pdani Date: Fri, 17 Jul 2026 21:56:46 +0200 Subject: [PATCH 075/108] floor: engage on breaking THROUGH the line, drop the prediction Daniel: a piloted trajectory is not predictable - the 3 s linear lookahead read every fast loop downline (30 m/s vertical, pulls out in 15 m at the 4 g limit) as a 90 m crash and silently co-flew the loops (measured via the safety-word instrumentation: 6-7 percent of figure frames under floor override across the fleet). The engage is now the original contract: sinking through the floor line triggers the recovery, nothing else. Above the line the sky belongs to the pilot; the height below the line is the recovery budget the user chooses with alt_floor_altitude (a dive catch consumes 15-25 m, documented). --- src/main/fc/settings.yaml | 2 +- src/main/flight/altitude_floor.c | 17 +++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7595e052c1a..0d7b07cce1b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4509,7 +4509,7 @@ groups: condition: USE_ORIENTATION_HOLD members: - name: alt_floor_altitude - description: "Altitude floor [m above home]. With the ALT FLOOR mode active and armed (climbed above floor + margin once), a predicted floor breach engages an automatic upright + climb recovery. Switch the mode off to land." + 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). Switch the mode off to land." default_value: 30 field: floorAltitude min: 5 diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 387ff76ead9..97b229eade5 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -51,12 +51,6 @@ PG_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, .floorClimbPitch = SETTING_ALT_FLOOR_CLIMB_PITCH_DEFAULT, ); -// How far ahead the sink prediction looks. Must cover the roll-to-upright -// time AND the Z estimator lag: under sustained sink the estimated altitude -// trails the true altitude by roughly vz * estimator time constant (~2-3 s -// with default baro weighting), so a short lookahead catches far too low. -#define ALT_FLOOR_LOOKAHEAD_S 3.0f - static bool floorArmed = false; // climbed above floor + margin once static bool floorRecovery = false; static bool sticksSeenCentered = false; @@ -84,8 +78,15 @@ void altitudeFloorUpdate(void) } if (!floorRecovery) { - // Predictive engage: catch before the floor, not at it - if (vz < 0.0f && (z + vz * ALT_FLOOR_LOOKAHEAD_S) < floorCm) { + // 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; sticksSeenCentered = false; } From 3acf28261211d2ac7914e7405fcb42a395775d8b Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 05:51:39 +0200 Subject: [PATCH 076/108] AHRS: attitude gate for GPS aiding on airplanes Yaw-from-course and the centrifugal compensation both assume coordinated forward flight - heading follows course and the lateral acceleration is v x omega. At aerobatic attitudes the assumption is broken and the aiding actively bends the estimate: measured in SITL with truth GPS injected, a prop hang carries +4.6 deg median pitch bias and 19.2 deg peak tilt divergence (clean without GPS: 0.0/2.4), inverted +7.1/16.5. ahrs_gps_aiding_max_tilt (default 60 deg, 0 = off): beyond this tilt from level both aiding paths fade out instantly - the raw accelerometer is the lesser error there - and fade back over 2 s after returning below the limit. Normal flight, including steep turns under 60 deg, keeps full GPS support. Proof (same truth-GPS A/B): hang +1.1 deg bias / 3.7 deg divergence, inverted -0.4 / 3.9 - back at the no-GPS baseline, with GPS live. imuConfig PG version bumped for the new field. --- src/main/fc/settings.yaml | 6 +++++ src/main/flight/imu.c | 46 +++++++++++++++++++++++++++++++++++++-- src/main/flight/imu.h | 1 + 3 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0d7b07cce1b..78140afc0f6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1567,6 +1567,12 @@ groups: field: acc_ignore_slope min: 0 max: 10 + - name: ahrs_gps_aiding_max_tilt + description: "Tilt from level [deg] beyond which ALL GPS-derived AHRS aiding (yaw from course, centrifugal compensation) fades out on an airplane - both assume coordinated forward flight and actively bend the attitude estimate in a hang, knife edge, inverted or spin (measured). Instant fade-out, 2 s fade-in after returning below the limit. 0 disables the gate." + default_value: 60 + field: gps_aiding_max_tilt + min: 0 + max: 90 - name: ahrs_gps_yaw_windcomp description: "Wind compensation in heading estimation from gps groundcourse(fixed wing only)" default_value: ON diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e5d2e3be75f..a0a97405128 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, 3); PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .dcm_kp_acc = SETTING_AHRS_DCM_KP_DEFAULT, // 0.20 * 10000 @@ -134,7 +136,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT, .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT, .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT, - .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT + .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT, + .gps_aiding_max_tilt = SETTING_AHRS_GPS_AIDING_MAX_TILT_DEFAULT ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) @@ -459,6 +462,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(); @@ -863,6 +873,15 @@ static void imuCalculateEstimatedAttitude(float dT) imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); } + // 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 fpVector3_t compensatedGravityBF_velned; @@ -968,6 +987,29 @@ 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. +static void imuUpdateGpsAidingTiltWeight(float dT) +{ + if (!STATE(AIRPLANE) || !imuConfig()->gps_aiding_max_tilt) { + gpsAidingTiltWeight = 1.0f; + return; + } + const float cosLimit = cos_approx(DEGREES_TO_RADIANS(imuConfig()->gps_aiding_max_tilt)); + 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/imu.h b/src/main/flight/imu.h index 60eb964bd68..33cb01fee9a 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -53,6 +53,7 @@ typedef struct imuConfig_s { uint8_t gps_yaw_windcomp; uint8_t inertia_comp_method; uint16_t gps_yaw_weight; + uint8_t gps_aiding_max_tilt; // [deg from level] beyond this tilt ALL GPS aiding fades out (0 = off) } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); From 09b647bec0b99ad8aad1dd884f6591f839438508 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 06:46:48 +0200 Subject: [PATCH 077/108] floor: document the pilot-override contract (Daniel's call) The pilot overrides the autopilot - held sticks keep steering, so a full held rudder drives a spin straight through the floor (measured: the recovery cannot arrest an autorotation whose driver stays live). The floor catches once the sticks are released; no yaw neutralization in the recovery. This is the design, now stated in the setting. --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 78140afc0f6..9cb4498e2eb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4515,7 +4515,7 @@ groups: condition: USE_ORIENTATION_HOLD 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). Switch the mode off to land." + 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). 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. Switch the mode off to land." default_value: 30 field: floorAltitude min: 5 From 16b7112a0dcf7bc16a080287d97aae3603ea9192 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 08:53:26 +0200 Subject: [PATCH 078/108] rotor guard: an idle throttle stick means landing - stand down The guard's only lever is thrust, and the pilot owns the thrust: with the throttle stick at idle the guard neither trips nor keeps a running recovery (instant release). A tip in the flare must not force a go-around, and a rollout tip must never get power on the ground. SITL-proven on the Auto-G2 (gated): guard armed, throttle pulled to idle, rotor starves and the gyro tips - recovery bit stays 0 and the FC throttle output stays at idle throughout; the tip pair (starve at 1120 us, above min_check) behaves unchanged on the same binary. --- src/main/fc/settings.yaml | 2 +- src/main/flight/rotor_guard.c | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9cb4498e2eb..9608bdefa5b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4557,7 +4557,7 @@ groups: min: -20 max: 10 - name: rotor_guard_throttle_add - description: "Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins; the motor keeps running through a low stick during recovery. Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink)." + description: "Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins, and an IDLE throttle stick disables the guard entirely (landing intent - the guard never spins the thrust up against a deliberate throttle-zero; pulling to idle releases a running recovery). Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink)." default_value: 250 field: throttleAddUs min: 0 diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c index c5d26cd443c..c36bab4b1f1 100644 --- a/src/main/flight/rotor_guard.c +++ b/src/main/flight/rotor_guard.c @@ -82,6 +82,19 @@ void rotorGuardUpdate(void) 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 From 41a62872dd3c7fb04d09fdbdd33f9b1f211b7e39 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 08:53:34 +0200 Subject: [PATCH 079/108] SITL: effective acc weight in debug slot 6 Companion to the slot-7 safety word: exposes the estimator's EFFECTIVE accelerometer weight (1000 = full trust) so the bench can verify the rate-ignore gate actually silences the centrifugally poisoned acc during sustained spins instead of trusting the label. --- src/main/flight/imu.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a0a97405128..cd1bac01489 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -918,6 +918,10 @@ static void imuCalculateEstimatedAttitude(float dT) float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compensatedGravityBF); accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler); const bool useAcc = (accWeight > 0.001f); + // 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 + debug[6] = lrintf(accWeight * 1000.0f); const float magWeight = imuGetPGainScaleFactor() * 1.0f; fpVector3_t measuredMagBF = {.v = {mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}}; From 4b2f58cb6bf2354eeb74a9af10463faa8d183a8a Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 13:25:19 +0200 Subject: [PATCH 080/108] FW_AEROBATICS: one runtime feature, default off (FW_LAUNCH pattern) Daniel's call after maintainer-psychology review: the whole suite must look like a small change you can switch off. Feature bit 5 (recycled unused slot, GEOZONE precedent), CLI 'feature FW_AEROBATICS': - off (default): none of the 12 boxes exist, the Modes tab looks exactly like upstream; the AHRS GPS-aiding tilt gate is bypassed (weight pinned 1.0); crash detection inert - behavior bit-identical to upstream with the feature off - on: the full aerobatics suite (holds, figures, floor, rotor guard, hover throttle, TVC, crash detection, tilt gate) - config.c boot scrub no longer clears bit 5 (it is a real feature now) --- src/main/fc/cli.c | 2 +- src/main/fc/config.c | 5 ++-- src/main/fc/config.h | 2 +- src/main/fc/fc_msp_box.c | 38 ++++++++++++++++++------------- src/main/flight/crash_detection.c | 5 +++- src/main/flight/imu.c | 11 +++++++-- 6 files changed, 40 insertions(+), 23 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9bb2c776883..41fcf1d4e87 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -160,7 +160,7 @@ 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", + "FW_AEROBATICS", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d3021317ae5..dd540184604 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 is FEATURE_FW_AEROBATICS now and + // must survive the boot scrub) + featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | 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..eeb2c00b9bb 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -37,7 +37,7 @@ 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 diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 369f72a543b..8bdb489f975 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -297,23 +297,29 @@ void initActiveBoxIds(void) if (sensors(SENSOR_ACC)) { ADD_ACTIVE_BOX(BOXANGLEHOLD); #ifdef USE_ORIENTATION_HOLD - 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); + // 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); } - 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 } } diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 4eebdb604ac..6bcde2fad6b 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -122,7 +122,10 @@ void crashDetectionUpdate(float dT) // 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. - if (!crashDetectionConfig()->crashDetection + // Part of the FW_AEROBATICS suite: without the feature the FC behaves + // exactly like upstream. + if (!feature(FEATURE_FW_AEROBATICS) + || !crashDetectionConfig()->crashDetection || !(STATE(AIRPLANE) || STATE(MULTIROTOR)) || !ARMING_FLAG(ARMED)) { inFlight = false; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index cd1bac01489..c8792ff4ec5 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -918,10 +918,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 + // 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]}}; @@ -1003,7 +1007,10 @@ float calculateCosTiltAngle(void) // normal flight regime keeps full GPS support. static void imuUpdateGpsAidingTiltWeight(float dT) { - if (!STATE(AIRPLANE) || !imuConfig()->gps_aiding_max_tilt) { + // 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) || !imuConfig()->gps_aiding_max_tilt) { gpsAidingTiltWeight = 1.0f; return; } From e59422911acf88cddc2d40cac845de8006558d1b Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 13:25:19 +0200 Subject: [PATCH 081/108] review fixes: three real defects - figure sequencer: aborting mid-IMPULSE/SPIN (box off, disarm) left the transient command flags latched; pidOrientationHold then kept applying stale open-loop full-rate commands AHEAD of the floor/rotor recovery target, defeating the catch. The flags now die with the figure (verified at the pid.c impulse branch). - MSP2_INAV_SET_FIGURE_SEQUENCE: validate the segment type BEFORE writing the PG - a rejected frame no longer leaves half-written parameters behind. - MSP outbuf: >512KB targets without FLASHFS kept a 512 B reply buffer while the full box-name list is ~736 B - serializeBoxNamesReply() failed and broke the Modes tab. Non-FLASHFS + USE_ORIENTATION_HOLD now gets 1024 B (the SITL-only bump moves to the generic path). --- src/main/fc/fc_msp.c | 12 +++++++----- src/main/flight/figure_sequencer.c | 9 +++++++++ src/main/msp/msp_serial.h | 7 +++++++ 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cdb2d486fd5..e18640f109d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2390,16 +2390,18 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) 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 = sbufReadU8(src); + seg->type = segType; seg->p1 = sbufReadU16(src); seg->p2 = sbufReadU16(src); seg->p3 = sbufReadU16(src); seg->flags = sbufReadU8(src); - if (seg->type >= FIGSEG_TYPE_COUNT) { - seg->type = FIGSEG_END; - return MSP_RESULT_ERROR; - } } else return MSP_RESULT_ERROR; break; diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index b5bc86ccb2a..b2061c281e8 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -154,6 +154,15 @@ void figureSequencerUpdate(void) 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; } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 5daa102d8ce..488085ab41b 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -62,6 +62,13 @@ typedef enum { #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_ORIENTATION_HOLD) +// 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 From 3d8c34adfac9219bf28da9813f0614ca88b8d95b Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 13:25:20 +0200 Subject: [PATCH 082/108] review hygiene: gated debug writes, folded defines, honest comments - debug[6] (acc weight) and debug[7] (safety word) are SITL-only now: raw debug[] writes on a real target clobber the user's selected debug channel - common.h: USE_ORIENTATION_HOLD/USE_THRUST_VECTORING/USE_CRASH_ DETECTION fold into the EXISTING >512KB block (no second gate, and flash-tight F411/F722 no longer pay for crash detection); SITL enables all three in its target.h - docs told the truth already except: altitude_floor.h still said 'predicted' (the law is breakthrough), fc_core's priority comment omitted the rotor-guard recovery, orientation_hold.h named the wrong slew setting for preset entries, the held-twist comment described a path that does not exist, one German word - regimeGainFreeze resets the limit-cycle detector with the regime (a frozen half-wave no longer backs the learned gain off on re-entry); the hover elevation 45 exists once, named --- src/main/fc/fc_core.c | 8 ++++++-- src/main/flight/altitude_floor.h | 10 ++++++---- src/main/flight/orientation_hold.c | 27 +++++++++++++++++++-------- src/main/flight/orientation_hold.h | 8 +++++--- src/main/target/SITL/target.h | 8 ++++---- src/main/target/common.h | 19 +++++++------------ 6 files changed, 47 insertions(+), 33 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 25a681944ac..5bd52b88636 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -698,7 +698,7 @@ void processRx(timeUs_t currentTimeUs) bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce; /* Disable stabilised modes initially, will be enabled as required with priority - * auto ANGLE (failsafe/nav) > ALT FLOOR recovery > ANGLE > HORIZON > ORIENTATION HOLD > ANGLEHOLD + * 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); @@ -708,13 +708,17 @@ void processRx(timeUs_t currentTimeUs) altitudeFloorUpdate(); rotorGuardUpdate(); figureSequencerUpdate(); +#if defined(SITL_BUILD) // 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 + // 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). debug[7] = (altitudeFloorArmed() ? 1 : 0) | (altitudeFloorRecoveryActive() ? 2 : 0) | (rotorGuardRecoveryActive() ? 4 : 0); +#endif #endif if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h index c8a35c96958..bfddccc1558 100644 --- a/src/main/flight/altitude_floor.h +++ b/src/main/flight/altitude_floor.h @@ -30,10 +30,12 @@ #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, a predicted floor -// breach 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. Switch the box off to land. +// 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 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 92a0485e8ae..9511455fac6 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -359,7 +359,8 @@ static void orientationHoldComputeFullAttitudeError(fpVector3_t *errDeg, const f // 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 fortgeschrieben, the relative angle stays small. +// 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; @@ -389,8 +390,9 @@ static float orientationHoldSlewTargetFull(fpQuaternion_t *qSoll, const fpQuater // 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. Held-twist sources (course hold bridging) will skip -// this re-anchoring and feed the full error instead. +// 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; static void orientationHoldComputeFullAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget); @@ -632,7 +634,10 @@ static bool hoverOscDetectAxis(hoverOscDetector_t *d, float sigDeg, float dT) } // freeze the learned value when a regime ends; write it back once so the -// disarm save picks it up +// 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]; @@ -644,6 +649,7 @@ static void regimeGainFreeze(oholdRegime_e regime) } g->wasActive = false; } + g->osc[0] = g->osc[1] = (hoverOscDetector_t){ 0 }; } static void regimeGainFreezeAll(void) @@ -653,6 +659,11 @@ static void regimeGainFreezeAll(void) } } +// 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) @@ -662,7 +673,7 @@ static oholdRegime_e regimeGainActiveRegime(void) // 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))) > 45.0f + 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: @@ -690,8 +701,7 @@ static void regimeGainUpdate(const fpVector3_t *errDeg, float dT) const oholdRegime_e active = regimeGainActiveRegime(); for (int r = 0; r < OHOLD_REGIME_COUNT; r++) { if (r != active && regimeGain[r].wasActive) { - regimeGainFreeze((oholdRegime_e)r); - regimeGain[r].osc[0] = regimeGain[r].osc[1] = (hoverOscDetector_t){ 0 }; + regimeGainFreeze((oholdRegime_e)r); // clears the osc detector too } } if (active == OHOLD_REGIME_NONE) { @@ -1081,7 +1091,8 @@ int16_t orientationHoldLoadGovernorThrottle(int16_t throttle) // COMMAND; the closed rate loop refines it - it deflects no further than // the achieved rate demands. #define AUTHORITY_MIN_SCALE 0.5f -#define AUTHORITY_HOVER_ELEVATION_DEG 45.0f +// AUTHORITY_HOVER_ELEVATION_DEG is defined once, further up (shared with +// the hover learning-regime gate) float orientationHoldAuthorityScale(void) { diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 45b808136cd..94f6fa21380 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -106,9 +106,11 @@ bool orientationHoldExitSlewPending(void); // 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 (fig_roll_rate -// for preset entries), so the error stays small and the entry path is an -// explicit trajectory. Returns false when no orientation hold box is active. +// 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 diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index bc5206058e8..87fa9a81bb2 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -63,7 +63,6 @@ #define USE_MAG #define USE_BARO #define USE_PITOT_FAKE -#define MSP_PORT_OUTBUF_SIZE 1024 // no FLASHFS on SITL; the full box-name list exceeds 512 #define USE_IMU_FAKE #define USE_FAKE_BARO #define USE_FAKE_MAG @@ -83,11 +82,12 @@ #define MAX_GEOZONES_IN_CONFIG 63 #define MAX_VERTICES_IN_CONFIG 126 -// The orientation-hold 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. +// 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_ORIENTATION_HOLD #define USE_THRUST_VECTORING +#define USE_CRASH_DETECTION #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? #undef USE_VCP diff --git a/src/main/target/common.h b/src/main/target/common.h index fe9192eba91..d5f981e463c 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -74,18 +74,6 @@ #define USE_SMITH_PREDICTOR #define USE_RATE_DYNAMICS #define USE_EXTENDED_CMS_MENUS -// The orientation-hold aerobatics suite (holds, figures, thrust vectoring) -// is large (~12 KB) and experimental. Restrict it to targets with room to -// spare: 512 KB boards (F722, F411, ...) are exempt and keep their flash. -// SITL has no MCU_FLASH_SIZE and enables it in its own target.h. -#if (MCU_FLASH_SIZE > 512) -#define USE_ORIENTATION_HOLD -#define USE_THRUST_VECTORING -#endif -// Crash detection (impact + stillness -> motor cut) is small and useful on -// any platform, not only with the orientation-hold modes, so it is its own -// feature. -#define USE_CRASH_DETECTION // Allow default rangefinders #define USE_RANGEFINDER @@ -217,6 +205,13 @@ //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_ORIENTATION_HOLD +#define USE_THRUST_VECTORING +#define USE_CRASH_DETECTION #define USE_VTX_FFPV #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT From fc3a6ce98e01a89e3ef4ba364db04aadf367ded5 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 20:05:26 +0200 Subject: [PATCH 083/108] floor: a catch latches the interrupted mode; the recovery ends in an orbit Daniel's contracts, each bench-proven on the floor_spin scenario: LATCH - the pilot sets the floor high enough; if it has to catch while ANY aerobatic mode is active (every hold, every figure, the sequencer, 3D lock - switch forgotten), that mode is latched OUT: it must not restart when the recovery releases and dive straight back in a loop. The latch clears only when the pilot switches the mode away. ORBIT - back at the floor the aircraft does NOT hand back: it circles the breach point and waits, the pilot gets time to collect themselves. Healthy position estimate -> the REAL fixed-wing loiter flies it (forced poshold anchored on the breach via the geozone flag path; the anchor is injected in the POSHOLD initialize because the forced event re-fires per RX cycle and would re-anchor 'here'). No healthy estimate -> constant-bank circle (needs no heading). nav_fw_loiter_radius must match the airframe speed (documented: R >= v^2/(g tan bank) - hunting at full bank otherwise); the recovery climb-throttle floor stands down while the nav owns pitch and throttle (measured: it ballooned the orbit 70 -> 212 m). TAKEOVER reads RAW receiver sticks, not rcCommand: the fixed-wing nav writes rcCommand to fly the loiter and the mix released the orbit by itself (measured). TELEMETRY - the radio speaks it (EdgeTX voice via the CRSF flight-mode text): FLOR/FLOF on floor arm/disarm (HRST transient idiom), CTCH during the catch, ORBT while orbiting. --- src/main/fc/fc_core.c | 12 ++- src/main/fc/settings.yaml | 2 +- src/main/flight/altitude_floor.c | 118 +++++++++++++++++++++++++--- src/main/flight/altitude_floor.h | 19 ++++- src/main/flight/hover_throttle.c | 7 +- src/main/flight/orientation_hold.c | 119 ++++++++++++++++++++++++----- src/main/flight/orientation_hold.h | 6 ++ src/main/flight/pid.c | 7 +- src/main/navigation/navigation.c | 61 ++++++++++++++- src/main/navigation/navigation.h | 8 ++ src/main/telemetry/crsf.c | 29 +++++++ 11 files changed, 351 insertions(+), 37 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 5bd52b88636..265261ad07a 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -717,7 +717,9 @@ void processRx(timeUs_t currentTimeUs) // channel the user selected (review finding). debug[7] = (altitudeFloorArmed() ? 1 : 0) | (altitudeFloorRecoveryActive() ? 2 : 0) - | (rotorGuardRecoveryActive() ? 4 : 0); + | (rotorGuardRecoveryActive() ? 4 : 0) + | (navigationPositionEstimateIsHealthy() ? 8 : 0) + | (altitudeFloorOrbitActive() ? 16 : 0); #endif #endif @@ -725,10 +727,14 @@ void processRx(timeUs_t currentTimeUs) if (autoEnableAngle) { ENABLE_FLIGHT_MODE(ANGLE_MODE); #ifdef USE_ORIENTATION_HOLD - } else if (STATE(AIRPLANE) && (altitudeFloorRecoveryActive() || rotorGuardRecoveryActive())) { + } 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 + // 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)) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9608bdefa5b..c606e9bbc79 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4515,7 +4515,7 @@ groups: condition: USE_ORIENTATION_HOLD 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). 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. Switch the mode off to land." + 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 diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 97b229eade5..8c8f8ed9de3 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -30,6 +30,7 @@ #include "common/axis.h" #include "common/maths.h" +#include "common/vector.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -40,9 +41,12 @@ #include "fc/settings.h" #include "flight/altitude_floor.h" +#include "flight/imu.h" #include "navigation/navigation.h" +#include "rx/rx.h" + PG_REGISTER_WITH_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, PG_ALTITUDE_FLOOR_CONFIG, 0); PG_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, @@ -53,7 +57,25 @@ PG_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, 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) { @@ -61,6 +83,11 @@ void altitudeFloorUpdate(void) || !navIsAltitudeEstimateTrusted()) { floorArmed = false; floorRecovery = false; + floorOrbit = false; + if (orbitViaNav) { + navAbortFloorOrbit(); + orbitViaNav = false; + } return; } @@ -88,24 +115,54 @@ void altitudeFloorUpdate(void) // 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 { - // Release when back above floor + margin and climbing - the climb - // ends at the margin, it does not run away upward - if (z > (floorCm + marginCm) && vz > 0.0f) { - floorRecovery = 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()) { + navActivateFloorOrbitAt(&breachPos); + orbitViaNav = true; + } } - // ... or when the pilot takes over after the catch: the sticks must - // return to center ONCE first (the panic-held down-elevator from - // the dive is not a takeover), a fresh roll/pitch deflection then - // hands control back immediately. Yaw stays steering, not release. - const bool deflected = ABS(rcCommand[ROLL]) > rcControlsConfig()->deadband - || ABS(rcCommand[PITCH]) > rcControlsConfig()->deadband; + if (orbitViaNav) { + // the poshold FSM re-anchors on current position at init - + // keep the breach point asserted every cycle + navAssertFloorOrbitTarget(&breachPos); + } + // 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) { + navAbortFloorOrbit(); + orbitViaNav = false; + } } } } @@ -122,7 +179,48 @@ bool altitudeFloorArmed(void) 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 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_ORIENTATION_HOLD diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h index bfddccc1558..974345cf377 100644 --- a/src/main/flight/altitude_floor.h +++ b/src/main/flight/altitude_floor.h @@ -54,5 +54,22 @@ bool altitudeFloorRecoveryActive(void); // True once the floor is armed (climbed above floor + margin) bool altitudeFloorArmed(void); -// Recovery pitch target (deg, nose up) +// 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); + +// 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/hover_throttle.c b/src/main/flight/hover_throttle.c index 9ab5c4cda28..fd694d2a040 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -278,7 +278,12 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) // gets at least the airframe's cruise throttle plus the standard // pitch-to-throttle compensation for the recovery climb angle - // more pilot throttle always wins - if (ARMING_FLAG(ARMED) && altitudeFloorRecoveryActive()) { + // ... but NOT while the orbit runs on the nav loiter: the nav owns + // pitch AND throttle there, and a parallel climb-throttle floor + // pumps energy against its altitude hold (measured: ballooned the + // 70 m orbit to 212 m) + if (ARMING_FLAG(ARMED) && altitudeFloorRecoveryActive() + && !altitudeFloorOrbitViaNav()) { const int16_t climbThrottle = currentBatteryProfile->nav.fw.cruise_throttle + lrintf(altitudeFloorRecoveryPitchDeg() * currentBatteryProfile->nav.fw.pitch_to_throttle); return constrain(MAX(pilotThrottle, climbThrottle), diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 9511455fac6..6f2183bd554 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -137,10 +137,71 @@ static const orientationHoldPreset_t * orientationHoldActivePreset(void) 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) { - return figureSequencerRequested() || orientationHoldActivePreset() != NULL - || IS_RC_MODE_ACTIVE(BOXATTLOCK); + // 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) @@ -427,18 +488,6 @@ static void orientationHoldRegulate(fpVector3_t *errDeg) quaternionNormalize(&qSollState, &qSollState); } -// 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) - // 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 @@ -479,6 +528,25 @@ 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 @@ -771,6 +839,7 @@ void orientationHoldResetSourceTracking(void) activeTargetSource = OHOLD_SOURCE_NONE; } exitSlewActive = false; + floorLatchedSource = OHOLD_SOURCE_NONE; // disarm/mode-exit hygiene // leaving the mode ends every learning regime: freeze the learned // gains (landing straight out of a hold and disarming must not lose them) @@ -828,8 +897,20 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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); - orientationHoldTargetFromRP(&qDesired, 0.0f, altitudeFloorRecoveryPitchDeg()); + // 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 @@ -839,7 +920,8 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) orientationHoldCheckSourceSwitch(OHOLD_SOURCE_ROTOR); orientationHoldTargetFromRP(&qDesired, 0.0f, rotorGuardRecoveryPitchDeg()); slewRateDegS = 0.0f; - } else if (figureSequencerRequested()) { + } else if (figureSequencerRequested() + && !orientationHoldFloorLatchBlocks(OHOLD_SOURCE_FIGURE)) { float figRoll, figPitch; orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE); figureSequencerGetTarget(&figRoll, &figPitch); @@ -866,7 +948,8 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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)) { + } 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 @@ -894,7 +977,7 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } } else { const orientationHoldPreset_t *preset = orientationHoldActivePreset(); - if (!preset) { + if (!preset || orientationHoldFloorLatchBlocks(preset->box)) { return false; } orientationHoldCheckSourceSwitch(preset->box); diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 94f6fa21380..f6ac6e9ae30 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -104,6 +104,12 @@ bool orientationHoldIsRequested(void); // 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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 75f80eb62b9..9125724d0c4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1440,11 +1440,14 @@ void FAST_CODE pidController(float dT) } #ifdef USE_ORIENTATION_HOLD else if (FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { - // WAIT_POS banks toward home: feed the coordinated turn rates + // 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 (figureSequencerGetTurnBank(&bankDeg)) { + if (orientationHoldTurnCoordinationBank(&bankDeg)) { pidTurnAssistant(pidState, DEGREES_TO_RADIANS(bankDeg), 0.0f); } } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 641b92aee52..7b6596a40d8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -253,6 +253,13 @@ navigationPosControl_t posControl; navSystemStatus_t NAV_Status; static bool landingDetectorIsActive; +#ifdef USE_ORIENTATION_HOLD +// altitude-floor orbit anchor: consumed by the POSHOLD initialize while +// the forced hold is active (see navActivateFloorOrbitAt) +static fpVector3_t floorOrbitTarget; +static bool floorOrbitTargetValid = false; +#endif + EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; // Blackbox states @@ -1345,6 +1352,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n fpVector3_t targetHoldPos; calculateInitialHoldPosition(&targetHoldPos); +#ifdef USE_ORIENTATION_HOLD + // altitude-floor orbit: loiter the BREACH 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 breach) + if (posControl.flags.forcedPosholdActive && floorOrbitTargetValid) { +#if defined(SITL_BUILD) + debug[4] = 4242; // orbit-anchor init path taken +#endif + setDesiredPosition(&floorOrbitTarget, posControl.actualState.yaw, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_SUCCESS; + } +#if defined(SITL_BUILD) + debug[4] = 1111; // default hold-here init path +#endif +#endif setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); } @@ -4595,8 +4620,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_ORIENTATION_HOLD) + // geozone avoidance hold, or the altitude-floor orbit loitering + // the breach point if (posControl.flags.forcedPosholdActive) { return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; } @@ -5214,6 +5242,37 @@ void abortForcedPosHold(void) } #endif +#ifdef USE_ORIENTATION_HOLD +/*----------------------------------------------------------- + * Altitude-floor orbit: the recovery hands the aircraft to the REAL + * fixed-wing loiter (Daniel: use the loitering machinery, not a + * hand-rolled orbit) - forced position hold anchored on the breach + * point. Reuses the forcedPoshold flag/FSM path the geozone built; the + * anchor itself is injected in the POSHOLD initialize (the forced event + * re-fires per RX cycle and would otherwise re-anchor "here"). + *-----------------------------------------------------------*/ +void navActivateFloorOrbitAt(const fpVector3_t *pos) +{ + floorOrbitTarget = *pos; + floorOrbitTargetValid = true; + posControl.flags.forcedPosholdActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void navAssertFloorOrbitTarget(const fpVector3_t *pos) +{ + // keep the stored anchor fresh (cheap; the initialize consumes it) + floorOrbitTarget = *pos; +} + +void navAbortFloorOrbit(void) +{ + floorOrbitTargetValid = 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 3e9b21357ad..a894aa9798f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -233,6 +233,14 @@ void abortForcedPosHold(void); #endif +#ifdef USE_ORIENTATION_HOLD +// Altitude-floor orbit: forced fixed-wing loiter anchored on the floor +// breach point (the real nav loiter - wind-corrected, nav_fw_loiter_radius) +void navActivateFloorOrbitAt(const fpVector3_t *pos); +void navAssertFloorOrbitTarget(const fpVector3_t *pos); +void navAbortFloorOrbit(void); +#endif + #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 #endif diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index e8214fc8c1f..0ff7b3c879d 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_ORIENTATION_HOLD + // 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_ORIENTATION_HOLD + } 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 From 5ed62abef83cf65acc4721dd50dc600d487f6a7e Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 20:05:26 +0200 Subject: [PATCH 084/108] AHRS: escape and re-seed the magnetic antipode The mag correction is a cross product - its torque scales with sin(error) and vanishes at 180 deg even though the error is maximal. After a flat spin the estimate parked there (measured: 184 deg off, stable 60+ s; GPS-COG is equally blind, same idiom). Two stages: past 90 deg the error vector is rescaled to full pull (walks off the saddle), and if the residual stays past 90 deg for a full second the estimate is re-seeded about earth Z so mag north snaps into place - the mag-driven sibling of the existing multirotor GPS yaw reset. Post-spin heading recovers to a few degrees within the catch phase (bench-measured); a clean sustained 27 deg circle holds 0.1 deg yaw error with GPS+mag and 6 deg on mag alone. --- src/main/flight/imu.c | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c8792ff4ec5..e31a50fbd9a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -430,6 +430,47 @@ 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 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); } From b4c8a200796cc0da23b105d4278c1e36d6478732 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 21:22:21 +0200 Subject: [PATCH 085/108] floor: defend the line against a held stick; expose the orbit form The nav poshold honors pilot pitch as a climb-rate override, so a panic-held down-elevator rode the orbit back through the floor (measured: 24 m under a 55 m line, ground impact on the chopped dive). A held stick is not a takeover: sinking back through the line now drops the orbit and re-engages the aggressive climb (attitude force + throttle floor) until the height is recaptured. The SITL safety word gains bit 32 (orbitViaNav) so the bench can discriminate the nav loiter from the degraded constant-bank orbit, and the orbit debug slots 4/5 (heading error to / distance from the breach anchor) replace the leftover 4242/1111 init-path markers. --- src/main/fc/fc_core.c | 3 ++- src/main/flight/altitude_floor.c | 29 +++++++++++++++++++++++++++++ src/main/navigation/navigation.c | 6 ------ 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 265261ad07a..e74e3f40896 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -719,7 +719,8 @@ void processRx(timeUs_t currentTimeUs) | (altitudeFloorRecoveryActive() ? 2 : 0) | (rotorGuardRecoveryActive() ? 4 : 0) | (navigationPositionEstimateIsHealthy() ? 8 : 0) - | (altitudeFloorOrbitActive() ? 16 : 0); + | (altitudeFloorOrbitActive() ? 16 : 0) + | (altitudeFloorOrbitViaNav() ? 32 : 0); #endif #endif diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 8c8f8ed9de3..9ebaec58095 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -23,11 +23,14 @@ */ #include +#include #include #ifdef USE_ORIENTATION_HOLD +#include "build/debug.h" + #include "common/axis.h" #include "common/maths.h" #include "common/vector.h" @@ -124,6 +127,20 @@ void altitudeFloorUpdate(void) 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) { + navAbortFloorOrbit(); + 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 @@ -144,6 +161,18 @@ void altitudeFloorUpdate(void) // keep the breach point asserted every cycle navAssertFloorOrbitTarget(&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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7b6596a40d8..50418d9a237 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1359,16 +1359,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n // current position each time and circles itself (measured: clean // 52 m circle drifting 250 m from the breach) if (posControl.flags.forcedPosholdActive && floorOrbitTargetValid) { -#if defined(SITL_BUILD) - debug[4] = 4242; // orbit-anchor init path taken -#endif setDesiredPosition(&floorOrbitTarget, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SUCCESS; } -#if defined(SITL_BUILD) - debug[4] = 1111; // default hold-here init path -#endif #endif setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); } From 9cc7bed5ffcb85e42a98c154b99f9ae31cee9e12 Mon Sep 17 00:00:00 2001 From: pdani Date: Sat, 18 Jul 2026 23:12:05 +0200 Subject: [PATCH 086/108] refactor: the aerobatics suite is called, not inlined (hook diet) Pure code motion, no behavior change - proven by the full SITL gate suite (19 shows, floor trio, fig_abort, gyro pair/land): - pid.c: the orientation-hold rate controller body moves to flight/orientation_hold.c (orientationHoldApplyRateTargets); the pid hook is a thin adapter mapping per-axis state through oholdAxisRate_t (118 -> 21 lines, three module includes dropped). - servos.c: the TVC input-row computation moves to flight/thrust_vectoring.c (thrustVectoringApplyInputs); the mixer keeps a one-line call. - hover_throttle.c: the recovery throttle floors move to their owning modules (altitudeFloorClimbThrottleUs with its nav-orbit suppression, rotorGuardThrottleFloorUs); the throttle path takes the highest claim. - fc_core.c: the SITL safety word is composed by the module (orientationHoldDebugSafetyWord), the core loop only writes the slot. - crash detection defaults OFF (explicit opt-in upstream; the bench provisions it ON for the crash proofs). Knife left/right and the spin family were already one mechanism (one preset table, one earth-vertical distribution for 4 attitudes x 2 directions); MSP2_INAV_ORIENTATION_HOLD_TEST stays for the level-1 bench mirror and is excluded at PR-slice assembly instead. --- src/main/fc/fc_core.c | 14 +-- src/main/fc/settings.yaml | 4 +- src/main/flight/altitude_floor.c | 18 ++++ src/main/flight/altitude_floor.h | 6 ++ src/main/flight/hover_throttle.c | 32 ++----- src/main/flight/orientation_hold.c | 137 +++++++++++++++++++++++++++++ src/main/flight/orientation_hold.h | 22 +++++ src/main/flight/pid.c | 121 +++---------------------- src/main/flight/rotor_guard.c | 15 ++++ src/main/flight/rotor_guard.h | 4 + src/main/flight/servos.c | 10 +-- src/main/flight/thrust_vectoring.c | 14 +++ src/main/flight/thrust_vectoring.h | 4 + 13 files changed, 243 insertions(+), 158 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e74e3f40896..f577f61c1d1 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -709,18 +709,8 @@ void processRx(timeUs_t currentTimeUs) rotorGuardUpdate(); figureSequencerUpdate(); #if defined(SITL_BUILD) - // 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). - debug[7] = (altitudeFloorArmed() ? 1 : 0) - | (altitudeFloorRecoveryActive() ? 2 : 0) - | (rotorGuardRecoveryActive() ? 4 : 0) - | (navigationPositionEstimateIsHealthy() ? 8 : 0) - | (altitudeFloorOrbitActive() ? 16 : 0) - | (altitudeFloorOrbitViaNav() ? 32 : 0); + // bench safety word (SITL only); composed by the module + debug[7] = orientationHoldDebugSafetyWord(); #endif #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c606e9bbc79..4d20858f092 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4725,7 +4725,7 @@ groups: condition: USE_CRASH_DETECTION members: - name: crash_detection - description: "Cut the motor after a crash while staying armed: a sharp acceleration spike near the accelerometer's full-scale, followed by the airframe lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed). Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. The impact threshold is DERIVED from the detected accelerometer (15% below full-scale), not set here - a spike that near saturation is an impact on any airframe, and the stillness that must follow is what tells a crash from a hard 3D figure. ON by default." - default_value: ON + description: "Cut the motor after a crash while staying armed: a sharp acceleration spike near the accelerometer's full-scale, followed by the airframe lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed). Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. The impact threshold is DERIVED from the detected accelerometer (15% below full-scale), not set here - a spike that near saturation is an impact on any airframe, and the stillness that must follow is what tells a crash from a hard 3D figure. OFF by default - an explicit opt-in." + default_value: OFF field: crashDetection type: bool diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 9ebaec58095..fa73ee06f1c 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -48,6 +48,8 @@ #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); @@ -239,6 +241,22 @@ bool altitudeFloorOrbitActive(void) return floorOrbit; } +int16_t altitudeFloorClimbThrottleUs(void) +{ + // The recovery climb must not ride whatever throttle the pilot froze + // in the dive (a panic chop leaves idle): at least the airframe's + // cruise throttle plus the standard pitch-to-throttle compensation + // for the climb angle. 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) { + return 0; + } + return currentBatteryProfile->nav.fw.cruise_throttle + + lrintf(altitudeFloorRecoveryPitchDeg() * currentBatteryProfile->nav.fw.pitch_to_throttle); +} + bool altitudeFloorOrbitViaNav(void) { return orbitViaNav; diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h index 974345cf377..e4a1fba794e 100644 --- a/src/main/flight/altitude_floor.h +++ b/src/main/flight/altitude_floor.h @@ -70,6 +70,12 @@ bool altitudeFloorOrbitActive(void); // 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. +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/hover_throttle.c b/src/main/flight/hover_throttle.c index fd694d2a040..4c9aa0932af 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -273,30 +273,14 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) return knifeInvertedAssistApply(pilotThrottle, elevDeg); } assistActive = false; - // the altitude floor recovery must not climb on whatever throttle - // the pilot froze in the dive (a panic chop leaves idle): the climb - // gets at least the airframe's cruise throttle plus the standard - // pitch-to-throttle compensation for the recovery climb angle - - // more pilot throttle always wins - // ... but NOT while the orbit runs on the nav loiter: the nav owns - // pitch AND throttle there, and a parallel climb-throttle floor - // pumps energy against its altitude hold (measured: ballooned the - // 70 m orbit to 212 m) - if (ARMING_FLAG(ARMED) && altitudeFloorRecoveryActive() - && !altitudeFloorOrbitViaNav()) { - const int16_t climbThrottle = currentBatteryProfile->nav.fw.cruise_throttle - + lrintf(altitudeFloorRecoveryPitchDeg() * currentBatteryProfile->nav.fw.pitch_to_throttle); - return constrain(MAX(pilotThrottle, climbThrottle), - getThrottleIdleValue(), getMaxThrottle()); - } - // autogyro tip-over recovery: thrust is the ONLY lever that brings - // the rotor rpm (and with it the roll authority) back - a fixed - // floor above cruise, NOT pitch-scaled (the recovery pitch is nose - // DOWN, pitch-to-throttle would reduce it); more pilot throttle wins - if (ARMING_FLAG(ARMED) && rotorGuardRecoveryActive()) { - const int16_t guardThrottle = currentBatteryProfile->nav.fw.cruise_throttle - + rotorGuardConfig()->throttleAddUs; - return constrain(MAX(pilotThrottle, guardThrottle), + // Recovery throttle floors are OWNED by their modules (the floor's + // climb math and its nav-orbit suppression, the rotor guard's + // fixed add): this path only takes the highest claim - and more + // pilot throttle always wins. + const int16_t recoveryFloor = MAX(altitudeFloorClimbThrottleUs(), + rotorGuardThrottleFloorUs()); + if (ARMING_FLAG(ARMED) && recoveryFloor > 0) { + return constrain(MAX(pilotThrottle, recoveryFloor), getThrottleIdleValue(), getMaxThrottle()); } return pilotThrottle; diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 6f2183bd554..0ed5ffc9863 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -1197,4 +1197,141 @@ float orientationHoldAuthorityScale(void) 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): the spin + // command is a rotation about the EARTH VERTICAL - exactly the axis the + // reduced attitude error leaves free - distributed onto the body axes + // via the earth-up direction in the body frame. At flat/inverted that + // is the yaw axis, at knife edge the pitch axis, at the hang the roll + // axis (torque roll). Rates along this axis leave the tilt untouched, + // so holding and spinning never fight (bench math mirror, section H). + float spinYawNorm; + const bool spinSegment = figureSequencerGetSpinCommand(&spinYawNorm); + const bool spinPreset = orientationHoldIsSpinAboutVertical(); + float spinRateDps = 0.0f; + fpVector3_t upBody; + 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(); + if (spinSegment || spinPreset) { + orientationHoldUpInBody(&upBody); + // AIRCRAFT-referenced stick sense: the body axis nearest the + // vertical receives the stick with its own positive sign - right + // rudder yaws the airframe right at flat AND inverted (so the + // rotation seen from above reverses when inverted, exactly like a + // real aircraft), and maps to positive pitch at the knife edge. + // The sign flip does not disturb the tilt (the distribution stays + // along the free axis either way). + float dominant = upBody.z; + if (fabsf(upBody.y) > fabsf(dominant)) { + dominant = upBody.y; + } + if (fabsf(upBody.x) > fabsf(dominant)) { + dominant = upBody.x; + } + if (dominant < 0.0f) { + vectorScale(&upBody, &upBody, -1.0f); + } + } + + // 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); + } + + float stickRate = (stickOffsets && axis != FD_YAW) ? 0.0f : axes[axis].stickRateDps; + if (spinSegment || spinPreset) { + if (axis == FD_YAW) { + stickRate = 0.0f; // the rudder is consumed by the spin command + } + rateTarget += spinRateDps * upBody.v[axis]; + } + 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_ORIENTATION_HOLD diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index f6ac6e9ae30..3159001e267 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -27,6 +27,8 @@ #include #include +#include "common/axis.h" +#include "common/filter.h" #include "common/quaternion.h" #include "common/vector.h" @@ -181,3 +183,23 @@ int16_t orientationHoldLoadGovernorThrottle(int16_t throttle); // 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 9125724d0c4..ba1da4abe88 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -43,9 +43,6 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" -#include "flight/altitude_floor.h" -#include "flight/rotor_guard.h" -#include "flight/figure_sequencer.h" #include "flight/mixer_profile.h" #include "flight/orientation_hold.h" #include "flight/rpm_filter.h" @@ -732,119 +729,21 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam #ifdef USE_ORIENTATION_HOLD // 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. Sticks -// remain live as rate commands on top of the stabilisation. +// 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) { - 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++) { - pidStates[axis].rateTarget = 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; + 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 (!orientationHoldComputeError(&errDeg, dT)) { - return; + if (!orientationHoldApplyRateTargets(axes, dT)) { + return; // no active hold source: the stick rate targets stand } - - // 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): the spin - // command is a rotation about the EARTH VERTICAL - exactly the axis the - // reduced attitude error leaves free - distributed onto the body axes - // via the earth-up direction in the body frame. At flat/inverted that - // is the yaw axis, at knife edge the pitch axis, at the hang the roll - // axis (torque roll). Rates along this axis leave the tilt untouched, - // so holding and spinning never fight (bench math mirror, section H). - float spinYawNorm; - const bool spinSegment = figureSequencerGetSpinCommand(&spinYawNorm); - const bool spinPreset = orientationHoldIsSpinAboutVertical(); - float spinRateDps = 0.0f; - fpVector3_t upBody; - 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 = pidStates[FD_YAW].rateTarget; - } - // 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(); - if (spinSegment || spinPreset) { - orientationHoldUpInBody(&upBody); - // AIRCRAFT-referenced stick sense: the body axis nearest the - // vertical receives the stick with its own positive sign - right - // rudder yaws the airframe right at flat AND inverted (so the - // rotation seen from above reverses when inverted, exactly like a - // real aircraft), and maps to positive pitch at the knife edge. - // The sign flip does not disturb the tilt (the distribution stays - // along the free axis either way). - float dominant = upBody.z; - if (fabsf(upBody.y) > fabsf(dominant)) { - dominant = upBody.y; - } - if (fabsf(upBody.x) > fabsf(dominant)) { - dominant = upBody.x; - } - if (dominant < 0.0f) { - vectorScale(&upBody, &upBody, -1.0f); - } - } - - // 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(&pidStates[axis].angleFilterState, rateTarget, pidBank()->pid[PID_LEVEL].I, dT); - } - - float stickRate = (stickOffsets && axis != FD_YAW) ? 0.0f : pidStates[axis].rateTarget; - if (spinSegment || spinPreset) { - if (axis == FD_YAW) { - stickRate = 0.0f; // the rudder is consumed by the spin command - } - rateTarget += spinRateDps * upBody.v[axis]; - } - pidStates[axis].rateTarget = constrainf(stickRate + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + pidStates[axis].rateTarget = axes[axis].rateTargetDps; } } #endif diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c index c36bab4b1f1..fd16460b89a 100644 --- a/src/main/flight/rotor_guard.c +++ b/src/main/flight/rotor_guard.c @@ -46,6 +46,8 @@ #include "navigation/navigation.h" +#include "sensors/battery.h" + PG_REGISTER_WITH_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, PG_ROTOR_GUARD_CONFIG, 0); PG_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, @@ -156,6 +158,19 @@ bool rotorGuardRecoveryActive(void) return guardRecovery; } +int16_t rotorGuardThrottleFloorUs(void) +{ + // Thrust is the ONLY lever that brings the rotor rpm (and with it + // the roll authority) back: a fixed floor above cruise, NOT + // pitch-scaled - the recovery pitch is nose DOWN and pitch-to- + // throttle would reduce it. 0 = no claim on the throttle. + if (!guardRecovery) { + return 0; + } + return currentBatteryProfile->nav.fw.cruise_throttle + + rotorGuardConfig()->throttleAddUs; +} + float rotorGuardRecoveryPitchDeg(void) { // Nose-down only while the roll excursion persists: it exists to feed diff --git a/src/main/flight/rotor_guard.h b/src/main/flight/rotor_guard.h index 5a6b5df0555..a0c9bc5ca81 100644 --- a/src/main/flight/rotor_guard.h +++ b/src/main/flight/rotor_guard.h @@ -50,3 +50,7 @@ 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 a5f11a3d040..0e2a5ed8c37 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -378,15 +378,7 @@ 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 - { - // 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); - } + thrustVectoringApplyInputs(input, mixerThrottleCommand); #endif input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value diff --git a/src/main/flight/thrust_vectoring.c b/src/main/flight/thrust_vectoring.c index 148ca7b8913..d4e65d6fa89 100644 --- a/src/main/flight/thrust_vectoring.c +++ b/src/main/flight/thrust_vectoring.c @@ -22,6 +22,8 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include + #include #ifdef USE_THRUST_VECTORING @@ -33,6 +35,7 @@ #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); @@ -54,4 +57,15 @@ float thrustVectoringGain(float thrustFraction) 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 index c44797280af..dca3c095335 100644 --- a/src/main/flight/thrust_vectoring.h +++ b/src/main/flight/thrust_vectoring.h @@ -44,3 +44,7 @@ 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); From 90c0bdb9e94e7029df3abc8f6f11f59cb03c1641 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 07:36:30 +0200 Subject: [PATCH 087/108] AHRS: gate the magnetic-antipode reseed behind FW_AEROBATICS The reseed sat in the core Mahony mag update ungated - it ran for every user with a compass, not just 3D flight. The failure mode it fixes (heading estimate parked on the antipode) was only ever measured after a sustained sub-cruise flat spin; a normal fixed wing never enters that regime. Gate both halves (the sub-90 error escape and the hard re-seed) behind feature(FEATURE_FW_AEROBATICS), matching the tilt gate in the same file, so the shared estimator is byte-identical with the feature off. --- src/main/flight/imu.c | 81 ++++++++++++++++++++++++------------------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e31a50fbd9a..db661a23b78 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -430,45 +430,54 @@ 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 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); - } + // 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)) { + // 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; + // 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; } - } else { - magAntipodeTimeS = 0.0f; } // Rotate error back into body frame From 744f5bb1806bf78d01cac3468f68ff0475f2ce3c Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 09:24:32 +0200 Subject: [PATCH 088/108] crash detection: own FEATURE_CRASH_DETECTION bit, decoupled from aerobatics Crash detection was gated behind FEATURE_FW_AEROBATICS plus a crash_detection bool setting - wrong for a standalone, platform-general feature. Give it its own runtime feature bit (recycled 1<<9), the GUI-visible master enable, default off. The redundant config group and bool setting are removed (the feature bit IS the enable), so it no longer depends on the aerobatics suite - a crash detector is useful on any airframe. SITL builds clean. --- src/main/fc/cli.c | 2 +- src/main/fc/config.c | 6 +++--- src/main/fc/config.h | 2 +- src/main/fc/settings.yaml | 11 ----------- src/main/flight/crash_detection.c | 16 +++++++--------- src/main/flight/crash_detection.h | 8 ++------ 6 files changed, 14 insertions(+), 31 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 41fcf1d4e87..d6e53c80258 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -161,7 +161,7 @@ static uint8_t commandBatchErrorCount = 0; static const char * const featureNames[] = { "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE", "FW_AEROBATICS", "SOFTSERIAL", "GPS", "RPM_FILTERS", - "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", + "CRASH_DETECTION", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE", diff --git a/src/main/fc/config.c b/src/main/fc/config.c index dd540184604..5077d7d6977 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -206,9 +206,9 @@ void validateAndFixConfig(void) accelerometerConfigMutable()->acc_notch_hz = 0; } - // Disable unused features (bit 5 is FEATURE_FW_AEROBATICS now and - // must survive the boot scrub) - featureClear(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 eeb2c00b9bb..0af6d687d29 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -41,7 +41,7 @@ typedef enum { 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/settings.yaml b/src/main/fc/settings.yaml index 4d20858f092..8ab307bfdea 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4718,14 +4718,3 @@ groups: field: hoverBaroWeight min: 0 max: 150 - - - name: PG_CRASH_DETECTION_CONFIG - type: crashDetectionConfig_t - headers: ["flight/crash_detection.h"] - condition: USE_CRASH_DETECTION - members: - - name: crash_detection - description: "Cut the motor after a crash while staying armed: a sharp acceleration spike near the accelerometer's full-scale, followed by the airframe lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed). Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. The impact threshold is DERIVED from the detected accelerometer (15% below full-scale), not set here - a spike that near saturation is an impact on any airframe, and the stillness that must follow is what tells a crash from a hard 3D figure. OFF by default - an explicit opt-in." - default_value: OFF - field: crashDetection - type: bool diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 6bcde2fad6b..372a2cebe27 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -54,11 +54,9 @@ #include "sensors/barometer.h" #include "sensors/gyro.h" -PG_REGISTER_WITH_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, PG_CRASH_DETECTION_CONFIG, 0); - -PG_RESET_TEMPLATE(crashDetectionConfig_t, crashDetectionConfig, - .crashDetection = SETTING_CRASH_DETECTION_DEFAULT, -); +// 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 @@ -122,10 +120,10 @@ void crashDetectionUpdate(float dT) // 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. - // Part of the FW_AEROBATICS suite: without the feature the FC behaves - // exactly like upstream. - if (!feature(FEATURE_FW_AEROBATICS) - || !crashDetectionConfig()->crashDetection + // 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; diff --git a/src/main/flight/crash_detection.h b/src/main/flight/crash_detection.h index 1b9d7098bf1..f96246fe2c8 100644 --- a/src/main/flight/crash_detection.h +++ b/src/main/flight/crash_detection.h @@ -39,12 +39,8 @@ // 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). -typedef struct crashDetectionConfig_s { - uint8_t crashDetection; // master enable; the impact threshold itself - // is derived from the accel full-scale, not set -} crashDetectionConfig_t; - -PG_DECLARE(crashDetectionConfig_t, crashDetectionConfig); +// 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); From 10ac09200ff0bbee9db9afbbaf2fa6f727017c7a Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 09:44:12 +0200 Subject: [PATCH 089/108] soaring: thermal soaring module (net vario + wind-shifted centering) Standalone thermal soaring, compile-gated by USE_SOARING (> 512 KB targets + SITL), runtime by the pilot's SOARING mode. Independent of the aerobatics suite - own PR. flight/soaring.c: - net (total-energy) variometer from the pitot airspeed and the vertical estimate, own polar sink compensated with the EXACT cos of the bank (ArduSoar uses a small-angle approximation that drifts at the 35-45 deg thermal bank) - thermal centering: sin/cos gradient of the vario deviation over each turn (points at the strongest climb) plus a wind-drift shift - the thermal is locked to the air mass so the circle slides at wind * dt, unscaled (ArduSoar scales wind by climb/strength without physical basis and blows up in weak lift) - cruise <-> thermal state machine, altitude band, drift clamp Seven SOAR_* parameters (PG_SOARING_CONFIG). SITL debug slots 0-3 carry the net vario and centre estimate for the bench. Builds clean; the nav loiter integration and the bench centering gate follow. --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_core.c | 6 + src/main/fc/settings.yaml | 48 ++++++ src/main/flight/soaring.c | 207 ++++++++++++++++++++++++++ src/main/flight/soaring.h | 70 +++++++++ src/main/target/SITL/target.h | 1 + src/main/target/common.h | 4 + 8 files changed, 340 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/soaring.c create mode 100644 src/main/flight/soaring.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e4634dcad51..8baf2ad35ac 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -337,6 +337,8 @@ main_sources(COMMON_SRC 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 diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 45cc6e0dfa2..632c48f9265 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -140,7 +140,8 @@ #define PG_HOVER_THROTTLE_CONFIG 1050 #define PG_CRASH_DETECTION_CONFIG 1051 #define PG_ROTOR_GUARD_CONFIG 1052 -#define PG_INAV_END PG_ROTOR_GUARD_CONFIG +#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/fc/fc_core.c b/src/main/fc/fc_core.c index f577f61c1d1..77a1f3bd501 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -91,6 +91,7 @@ #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" @@ -1026,6 +1027,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs) 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/settings.yaml b/src/main/fc/settings.yaml index 8ab307bfdea..6b2160ae80b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4718,3 +4718,51 @@ groups: field: hoverBaroWeight min: 0 max: 150 + + - 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_value: 80 + 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/soaring.c b/src/main/flight/soaring.c new file mode 100644 index 00000000000..a34cb9b9925 --- /dev/null +++ b/src/main/flight/soaring.c @@ -0,0 +1,207 @@ +/* + * 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_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_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/imu.h" +#include "flight/soaring.h" +#include "flight/wind_estimator.h" + +#include "navigation/navigation.h" + +#include "sensors/acceleration.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, +); + +// Slow low-pass for the DC part of the net vario: the sin/cos gradient +// correlates the DEVIATION from this mean, so a strong-but-uniform column +// does not bias the shift. A few circle periods. +#define SOAR_VARIO_MEAN_TAU_S 10.0f +// Gradient low-pass ~ one circle period: it takes a full turn to see which +// side of the circle climbs best. +#define SOAR_GRAD_TAU_S 6.0f +// Converts the vario gradient [m/s] into a centre shift rate [cm/s] at +// centreGainPct = 100. The bench tunes centreGainPct on top; small, because +// "slowly" - a circle that chases noise eier and 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 gradient would walk the loiter out of the sky). +#define SOAR_CENTRE_MAX_DRIFT_CM 30000.0f // 300 m + +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 varioMean = 0.0f; +static float gradN = 0.0f, gradE = 0.0f; +static float vPrev = 0.0f; +static float netVarioCms = 0.0f; + +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 + const float sink = (soaringConfig()->sinkLevelCms / 100.0f) + / (cosRoll * fast_fsqrtf(cosRoll)); // /cos^1.5 + 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)) { + soarActive = false; + thermalling = false; + return; + } + soarActive = true; + + const float netVario = computeNetVarioMs(dT); + netVarioCms = netVario * 100.0f; + varioMean += (netVario - varioMean) * MIN(dT / SOAR_VARIO_MEAN_TAU_S, 1.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: net lift over the trigger, inside the altitude band + if (netVario > triggerMs + && alt > soaringConfig()->altMinM && alt < soaringConfig()->altMaxM) { + thermalling = true; + breachAnchor.x = thermalCentre.x = getEstimatedActualPosition(X); + breachAnchor.y = thermalCentre.y = getEstimatedActualPosition(Y); + breachAnchor.z = thermalCentre.z = getEstimatedActualPosition(Z); + gradN = gradE = 0.0f; + } + return; + } + + // CENTERING: correlate the vario deviation against the bearing from the + // current centre estimate to the aircraft over one turn (sin/cos = the + // first harmonic, pointing at the strongest climb), then slide the centre + // that way AND with the wind (the thermal is locked to the air mass - + // wind * dt is the exact drift, no climb/strength scaling). + const float dx = getEstimatedActualPosition(X) - thermalCentre.x; // north cm + const float dy = getEstimatedActualPosition(Y) - thermalCentre.y; // east cm + const float bearing = atan2_approx(dy, dx); + const float dv = netVario - varioMean; + const float a = MIN(dT / SOAR_GRAD_TAU_S, 1.0f); + gradN += (dv * cos_approx(bearing) - gradN) * a; + gradE += (dv * sin_approx(bearing) - gradE) * a; + + const float k = (soaringConfig()->centreGainPct / 100.0f) * SOAR_CENTRE_GAIN_SCALE; + thermalCentre.x += (k * gradN + getEstimatedWindSpeed(X)) * dT; // cm + thermalCentre.y += (k * gradE + getEstimatedWindSpeed(Y)) * dT; + + // 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] +#endif + + // leave the thermal: lift collapsed, or out of the altitude band + if (netVario < exitMs + || alt > soaringConfig()->altMaxM || alt < soaringConfig()->altMinM) { + thermalling = false; + } +} + +bool soaringActive(void) +{ + return soarActive; +} + +bool soaringThermalling(void) +{ + return thermalling; +} + +void soaringThermalCentre(fpVector3_t *centre) +{ + *centre = thermalCentre; +} + +float soaringNetVarioCms(void) +{ + return netVarioCms; +} + +#endif // USE_SOARING diff --git a/src/main/flight/soaring.h b/src/main/flight/soaring.h new file mode 100644 index 00000000000..575a45b549a --- /dev/null +++ b/src/main/flight/soaring.h @@ -0,0 +1,70 @@ +/* + * 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); diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 87fa9a81bb2..8557fdfad86 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -88,6 +88,7 @@ #define USE_ORIENTATION_HOLD #define USE_THRUST_VECTORING #define USE_CRASH_DETECTION +#define USE_SOARING #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? #undef USE_VCP diff --git a/src/main/target/common.h b/src/main/target/common.h index d5f981e463c..f5874d95779 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -212,6 +212,10 @@ #define USE_ORIENTATION_HOLD #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 From 72aaffc0be6b7ac7e722d09b44b4c63b0d496a5d Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 10:18:53 +0200 Subject: [PATCH 090/108] nav: generalise the wandering forced-poshold anchor; soaring drives it The floor-orbit anchor (navActivateFloorOrbitAt / floorOrbitTarget) was named and gated for the aerobatics floor only. Rename it neutral (navForcedPosholdActivateAt / navForcedPosholdAnchor) and widen the gate to USE_ORIENTATION_HOLD || USE_SOARING so the altitude floor and thermal soaring share the same loiter machinery - without renaming USE_ORIENTATION_HOLD itself (still the aerobatics gate). Soaring now activates the anchor on the thermal-centre estimate at thermal entry, re-asserts it each cycle (the wind-shifted centre), and clears it on exit. Floor behaviour is unchanged: floor_dive and floor_spin gates both PASS on the renamed path. --- src/main/flight/altitude_floor.c | 10 +++--- src/main/flight/soaring.c | 9 +++++ src/main/navigation/navigation.c | 62 ++++++++++++++++---------------- src/main/navigation/navigation.h | 13 +++---- 4 files changed, 53 insertions(+), 41 deletions(-) diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index fa73ee06f1c..44e135941b8 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -90,7 +90,7 @@ void altitudeFloorUpdate(void) floorRecovery = false; floorOrbit = false; if (orbitViaNav) { - navAbortFloorOrbit(); + navForcedPosholdClear(); orbitViaNav = false; } return; @@ -139,7 +139,7 @@ void altitudeFloorUpdate(void) if (floorOrbit && vz < 0.0f && z < floorCm) { floorOrbit = false; if (orbitViaNav) { - navAbortFloorOrbit(); + navForcedPosholdClear(); orbitViaNav = false; } } @@ -154,14 +154,14 @@ void altitudeFloorUpdate(void) if (!floorOrbit && z > (floorCm + marginCm) && vz > 0.0f) { floorOrbit = true; if (navigationPositionEstimateIsHealthy()) { - navActivateFloorOrbitAt(&breachPos); + navForcedPosholdActivateAt(&breachPos); orbitViaNav = true; } } if (orbitViaNav) { // the poshold FSM re-anchors on current position at init - // keep the breach point asserted every cycle - navAssertFloorOrbitTarget(&breachPos); + navForcedPosholdAssert(&breachPos); } #if defined(SITL_BUILD) // bench telemetry (demuxed into the flight CSV): how the recovery @@ -191,7 +191,7 @@ void altitudeFloorUpdate(void) floorRecovery = false; floorOrbit = false; if (orbitViaNav) { - navAbortFloorOrbit(); + navForcedPosholdClear(); orbitViaNav = false; } } diff --git a/src/main/flight/soaring.c b/src/main/flight/soaring.c index a34cb9b9925..73917d02932 100644 --- a/src/main/flight/soaring.c +++ b/src/main/flight/soaring.c @@ -116,6 +116,9 @@ void soaringUpdate(float dT) // 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; @@ -139,6 +142,8 @@ void soaringUpdate(float dT) breachAnchor.y = thermalCentre.y = getEstimatedActualPosition(Y); breachAnchor.z = thermalCentre.z = getEstimatedActualPosition(Z); gradN = gradE = 0.0f; + // hand the loiter to the real nav machinery, anchored here + navForcedPosholdActivateAt(&thermalCentre); } return; } @@ -159,6 +164,9 @@ void soaringUpdate(float dT) const float k = (soaringConfig()->centreGainPct / 100.0f) * SOAR_CENTRE_GAIN_SCALE; thermalCentre.x += (k * gradN + getEstimatedWindSpeed(X)) * dT; // cm thermalCentre.y += (k * gradE + getEstimatedWindSpeed(Y)) * dT; + // keep the loiter anchored on the moving centre estimate (the POSHOLD + // initialize re-fires per RX cycle and would otherwise re-anchor "here") + navForcedPosholdAssert(&thermalCentre); // clamp the estimate to a sane radius around where the climb was found const float driftX = thermalCentre.x - breachAnchor.x; @@ -181,6 +189,7 @@ void soaringUpdate(float dT) if (netVario < exitMs || alt > soaringConfig()->altMaxM || alt < soaringConfig()->altMinM) { thermalling = false; + navForcedPosholdClear(); // hand the loiter back to the pilot / cruise } } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 50418d9a237..a0577440343 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -253,11 +253,12 @@ navigationPosControl_t posControl; navSystemStatus_t NAV_Status; static bool landingDetectorIsActive; -#ifdef USE_ORIENTATION_HOLD -// altitude-floor orbit anchor: consumed by the POSHOLD initialize while -// the forced hold is active (see navActivateFloorOrbitAt) -static fpVector3_t floorOrbitTarget; -static bool floorOrbitTargetValid = false; +#if defined(USE_ORIENTATION_HOLD) || 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; @@ -1352,14 +1353,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n fpVector3_t targetHoldPos; calculateInitialHoldPosition(&targetHoldPos); -#ifdef USE_ORIENTATION_HOLD - // altitude-floor orbit: loiter the BREACH 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 breach) - if (posControl.flags.forcedPosholdActive && floorOrbitTargetValid) { - setDesiredPosition(&floorOrbitTarget, posControl.actualState.yaw, +#if defined(USE_ORIENTATION_HOLD) || 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; } @@ -4616,9 +4618,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } #endif -#if defined(USE_GEOZONE) || defined(USE_ORIENTATION_HOLD) - // geozone avoidance hold, or the altitude-floor orbit loitering - // the breach point +#if defined(USE_GEOZONE) || defined(USE_ORIENTATION_HOLD) || 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; } @@ -5236,32 +5238,32 @@ void abortForcedPosHold(void) } #endif -#ifdef USE_ORIENTATION_HOLD +#if defined(USE_ORIENTATION_HOLD) || defined(USE_SOARING) /*----------------------------------------------------------- - * Altitude-floor orbit: the recovery hands the aircraft to the REAL - * fixed-wing loiter (Daniel: use the loitering machinery, not a - * hand-rolled orbit) - forced position hold anchored on the breach - * point. Reuses the forcedPoshold flag/FSM path the geozone built; the - * anchor itself is injected in the POSHOLD initialize (the forced event - * re-fires per RX cycle and would otherwise re-anchor "here"). + * 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 navActivateFloorOrbitAt(const fpVector3_t *pos) +void navForcedPosholdActivateAt(const fpVector3_t *pos) { - floorOrbitTarget = *pos; - floorOrbitTargetValid = true; + navForcedPosholdAnchor = *pos; + navForcedPosholdAnchorValid = true; posControl.flags.forcedPosholdActive = true; navProcessFSMEvents(selectNavEventFromBoxModeInput()); } -void navAssertFloorOrbitTarget(const fpVector3_t *pos) +void navForcedPosholdAssert(const fpVector3_t *pos) { // keep the stored anchor fresh (cheap; the initialize consumes it) - floorOrbitTarget = *pos; + navForcedPosholdAnchor = *pos; } -void navAbortFloorOrbit(void) +void navForcedPosholdClear(void) { - floorOrbitTargetValid = false; + navForcedPosholdAnchorValid = false; posControl.flags.forcedPosholdActive = false; navProcessFSMEvents(selectNavEventFromBoxModeInput()); } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index a894aa9798f..4f25ba6a363 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -233,12 +233,13 @@ void abortForcedPosHold(void); #endif -#ifdef USE_ORIENTATION_HOLD -// Altitude-floor orbit: forced fixed-wing loiter anchored on the floor -// breach point (the real nav loiter - wind-corrected, nav_fw_loiter_radius) -void navActivateFloorOrbitAt(const fpVector3_t *pos); -void navAssertFloorOrbitTarget(const fpVector3_t *pos); -void navAbortFloorOrbit(void); +#if defined(USE_ORIENTATION_HOLD) || 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 From 61050d928ee2c124df19bb8a1636734e8810bded Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 14:32:19 +0200 Subject: [PATCH 091/108] soaring: default soar_sink_level from the measured easyglider polar Bench glide-polar sweep (phugoid averaged) gives the 1.8 m motor glider a min sink of 0.61 m/s at 8 m/s (best L/D 13.2) - a healthy polar, confirming the plant model is fine. Set the net-vario sink compensation default to 60 cm/s (was a guessed 80). Measured, not guessed. --- src/main/fc/settings.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6b2160ae80b..6c611de9591 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4755,8 +4755,8 @@ groups: 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_value: 80 + 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 From 13400a1fc87ac7f93a5f91153d159bd9e0a71ff3 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 17:05:53 +0200 Subject: [PATCH 092/108] docs: regenerate Settings.md Catch up the autogenerated CLI reference with settings.yaml (soar_*, rotor_guard_*, ahrs_gps_aiding_max_tilt, crash_detection now a feature bit). Co-Authored-By: Claude Opus 4.8 --- docs/Settings.md | 132 +++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 121 insertions(+), 11 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ea8f0e8cc62..80e9e0de4e6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -252,6 +252,16 @@ Inertial Measurement Unit KP Gain for compass measurements --- +### ahrs_gps_aiding_max_tilt + +Tilt from level [deg] beyond which ALL GPS-derived AHRS aiding (yaw from course, centrifugal compensation) fades out on an airplane - both assume coordinated forward flight and actively bend the attitude estimate in a hang, knife edge, inverted or spin (measured). Instant fade-out, 2 s fade-in after returning below the limit. 0 disables the gate. + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 90 | + +--- + ### ahrs_gps_yaw_weight Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass @@ -394,7 +404,7 @@ 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), a predicted floor breach engages an automatic upright + climb recovery. Switch the mode off to land. +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 | | --- | --- | --- | @@ -622,16 +632,6 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho --- -### crash_detection - -Cut the motor after a crash while staying armed: a sharp acceleration spike near the accelerometer's full-scale, followed by the airframe lying still within 3 s (no rotation, resting 1 g, frozen baro altitude and - with a GPS fix - no ground speed). Moving the throttle to zero and up again re-allows the motor (short bursts help locating the aircraft in high grass). Arms only once clearly in flight (nav launch completed or throttle held above cruise for a moment), so a hand-launched aircraft can be carried armed. The impact threshold is DERIVED from the detected accelerometer (15% below full-scale), not set here - a spike that near saturation is an impact on any airframe, and the stillness that must follow is what tells a crash from a hard 3D figure. ON by default. - -| Default | Min | Max | -| --- | --- | --- | -| ON | OFF | ON | - ---- - ### cruise_power Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit @@ -6102,6 +6102,46 @@ 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_pitch + +Pitch target [deg] during rotor guard recovery, negative = nose down: feeds the disk (inflow -> rotor rpm -> authority) + +| Default | Min | Max | +| --- | --- | --- | +| -5 | -20 | 10 | + +--- + +### 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_add + +Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins, and an IDLE throttle stick disables the guard entirely (landing intent - the guard never spins the thrust up against a deliberate throttle-zero; pulling to idle releases a running recovery). Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink). + +| Default | Min | Max | +| --- | --- | --- | +| 250 | 0 | 800 | + +--- + ### 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 @@ -6482,6 +6522,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 From 37eaf354da26fe54aaf4c67527c418edd5984782 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 18:26:44 +0200 Subject: [PATCH 093/108] aerobatics: rename USE_ORIENTATION_HOLD compile flag to USE_FW_AEROBATICS The flag grew from gating just the orientation-hold mode to gating the whole fixed-wing aerobatics core (orientation holds, figure sequencer, altitude floor, rotor guard, the MSP handlers). Its name no longer matched its scope, and the runtime side is already FEATURE_FW_AEROBATICS / cli "FW_AEROBATICS". Pure rename of the compile flag; the ORIENTATION_HOLD_MODE flight mode and the BOXINVERTED/BOXKNIFE* boxes keep their names. Co-Authored-By: Claude Opus 4.8 --- src/main/fc/fc_core.c | 10 +++++----- src/main/fc/fc_msp.c | 6 +++--- src/main/fc/fc_msp_box.c | 4 ++-- src/main/fc/settings.yaml | 10 +++++----- src/main/flight/altitude_floor.c | 4 ++-- src/main/flight/figure_sequencer.c | 4 ++-- src/main/flight/hover_throttle.c | 4 ++-- src/main/flight/mixer.c | 4 ++-- src/main/flight/orientation_hold.c | 4 ++-- src/main/flight/pid.c | 6 +++--- src/main/flight/rotor_guard.c | 4 ++-- src/main/msp/msp_serial.h | 2 +- src/main/navigation/navigation.c | 8 ++++---- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_pos_estimator.c | 4 ++-- src/main/target/SITL/target.h | 2 +- src/main/target/common.h | 2 +- src/main/telemetry/crsf.c | 4 ++-- 18 files changed, 42 insertions(+), 42 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 77a1f3bd501..3eed06e2b13 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -704,7 +704,7 @@ void processRx(timeUs_t currentTimeUs) DISABLE_FLIGHT_MODE(ANGLE_MODE); DISABLE_FLIGHT_MODE(HORIZON_MODE); DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE); -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); altitudeFloorUpdate(); rotorGuardUpdate(); @@ -718,7 +718,7 @@ void processRx(timeUs_t currentTimeUs) if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { if (autoEnableAngle) { ENABLE_FLIGHT_MODE(ANGLE_MODE); -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS } else if (STATE(AIRPLANE) && ((altitudeFloorRecoveryActive() && !altitudeFloorOrbitViaNav()) || rotorGuardRecoveryActive())) { @@ -730,7 +730,7 @@ void processRx(timeUs_t currentTimeUs) ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); #endif } else if (IS_RC_MODE_ACTIVE(BOXANGLE)) { -#ifdef USE_ORIENTATION_HOLD +#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) @@ -743,7 +743,7 @@ void processRx(timeUs_t currentTimeUs) } } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS } else if (STATE(AIRPLANE) && orientationHoldIsRequested()) { ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE); #endif @@ -752,7 +752,7 @@ void processRx(timeUs_t currentTimeUs) } } -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS if (!FLIGHT_MODE(ORIENTATION_HOLD_MODE)) { orientationHoldResetSourceTracking(); } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e18640f109d..a4c8f4fcaa2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -567,7 +567,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 0); } break; -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS case MSP2_INAV_FIGURE_SEQUENCE: for (int i = 0; i < MAX_FIGURE_SEQUENCE_SEGMENTS; i++) { sbufWriteU8(dst, figureSequence(i)->type); @@ -2386,7 +2386,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS case MSP2_INAV_SET_FIGURE_SEQUENCE: sbufReadU8Safe(&tmp_u8, src); if ((dataSize == 9) && (tmp_u8 < MAX_FIGURE_SEQUENCE_SEGMENTS)) { @@ -4498,7 +4498,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif -#ifdef USE_ORIENTATION_HOLD +#ifdef 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. diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 8bdb489f975..17be6baa53c 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -296,7 +296,7 @@ void initActiveBoxIds(void) } if (sensors(SENSOR_ACC)) { ADD_ACTIVE_BOX(BOXANGLEHOLD); -#ifdef USE_ORIENTATION_HOLD +#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 @@ -487,7 +487,7 @@ 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_ORIENTATION_HOLD +#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); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c611de9591..447b6b79dd5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4512,7 +4512,7 @@ groups: - name: PG_ALTITUDE_FLOOR_CONFIG type: altitudeFloorConfig_t headers: ["flight/altitude_floor.h"] - condition: USE_ORIENTATION_HOLD + 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." @@ -4536,7 +4536,7 @@ groups: - name: PG_ROTOR_GUARD_CONFIG type: rotorGuardConfig_t headers: ["flight/rotor_guard.h"] - condition: USE_ORIENTATION_HOLD + 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." @@ -4584,7 +4584,7 @@ groups: - name: PG_ORIENTATION_HOLD_CONFIG type: orientationHoldConfig_t headers: ["flight/orientation_hold.h"] - condition: USE_ORIENTATION_HOLD + condition: USE_FW_AEROBATICS members: - name: ohold_inverted_pitch_trim description: "Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias)" @@ -4662,7 +4662,7 @@ groups: - name: PG_FIGURE_SEQUENCER_CONFIG type: figureSequencerConfig_t headers: ["flight/figure_sequencer.h"] - condition: USE_ORIENTATION_HOLD + condition: USE_FW_AEROBATICS members: - name: fig_roll_rate description: "Roll rate [deg/s] flown by the FIGURE ROLL and FIGURE 4PT ROLL modes" @@ -4704,7 +4704,7 @@ groups: - name: PG_HOVER_THROTTLE_CONFIG type: hoverThrottleConfig_t headers: ["flight/hover_throttle.h"] - condition: USE_ORIENTATION_HOLD + condition: USE_FW_AEROBATICS members: - name: ohold_hover_thr_min description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. The altitude/vz loop gains themselves are not settings: they derive at runtime from the learned hover point (throttle-to-thrust slope), see hover_throttle.c." diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 44e135941b8..31be3f3912f 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -27,7 +27,7 @@ #include -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS #include "build/debug.h" @@ -270,4 +270,4 @@ float altitudeFloorDistanceM(void) return (getEstimatedActualPosition(Z) - floorCm) / 100.0f; } -#endif // USE_ORIENTATION_HOLD +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c index b2061c281e8..f5771d31a41 100644 --- a/src/main/flight/figure_sequencer.c +++ b/src/main/flight/figure_sequencer.c @@ -26,7 +26,7 @@ #include -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS #include "common/axis.h" #include "common/maths.h" @@ -481,4 +481,4 @@ void figureSequencerGetTarget(float *rollDeg, float *pitchDeg) *pitchDeg = targetPitchDeg; } -#endif // USE_ORIENTATION_HOLD +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 4c9aa0932af..e281201ee7d 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -27,7 +27,7 @@ #include -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS #include "common/axis.h" #include "common/maths.h" @@ -375,4 +375,4 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) return outUs; } -#endif // USE_ORIENTATION_HOLD +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 90270bc2e4f..453ca81c843 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -593,7 +593,7 @@ void FAST_CODE mixTable(void) #endif } else { mixerThrottleCommand = rcCommand[THROTTLE]; -#ifdef USE_ORIENTATION_HOLD +#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 @@ -703,7 +703,7 @@ motorStatus_e getMotorStatus(void) const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) { -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS // the altitude floor recovery climbs on its own throttle floor - a // panic-chopped stick must not stop the motor that climb needs (the // same override navigation gets via nav_overrides_motor_stop). The diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 0ed5ffc9863..ef067679426 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -27,7 +27,7 @@ #include -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS #include "common/axis.h" #include "common/maths.h" @@ -1334,4 +1334,4 @@ uint32_t orientationHoldDebugSafetyWord(void) } #endif -#endif // USE_ORIENTATION_HOLD +#endif // USE_FW_AEROBATICS diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ba1da4abe88..16d91e2ed93 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -726,7 +726,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } } -#ifdef USE_ORIENTATION_HOLD +#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 @@ -1300,7 +1300,7 @@ void FAST_CODE pidController(float dT) const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; angleHoldIsLevel = false; -#ifdef USE_ORIENTATION_HOLD +#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); @@ -1337,7 +1337,7 @@ void FAST_CODE pidController(float dT) pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } -#ifdef USE_ORIENTATION_HOLD +#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 diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c index fd16460b89a..1beaaaaf342 100644 --- a/src/main/flight/rotor_guard.c +++ b/src/main/flight/rotor_guard.c @@ -26,7 +26,7 @@ #include -#ifdef USE_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS #include "common/axis.h" #include "common/maths.h" @@ -185,4 +185,4 @@ float rotorGuardRecoveryPitchDeg(void) return 0.0f; } -#endif // USE_ORIENTATION_HOLD +#endif // USE_FW_AEROBATICS diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 488085ab41b..78912432a4c 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -62,7 +62,7 @@ typedef enum { #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_ORIENTATION_HOLD) +#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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a0577440343..26281e221bf 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -253,7 +253,7 @@ navigationPosControl_t posControl; navSystemStatus_t NAV_Status; static bool landingDetectorIsActive; -#if defined(USE_ORIENTATION_HOLD) || defined(USE_SOARING) +#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). @@ -1353,7 +1353,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n fpVector3_t targetHoldPos; calculateInitialHoldPosition(&targetHoldPos); -#if defined(USE_ORIENTATION_HOLD) || defined(USE_SOARING) +#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 @@ -4618,7 +4618,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } #endif -#if defined(USE_GEOZONE) || defined(USE_ORIENTATION_HOLD) || defined(USE_SOARING) +#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) { @@ -5238,7 +5238,7 @@ void abortForcedPosHold(void) } #endif -#if defined(USE_ORIENTATION_HOLD) || defined(USE_SOARING) +#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). diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 4f25ba6a363..25abe9e1aed 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -233,7 +233,7 @@ void abortForcedPosHold(void); #endif -#if defined(USE_ORIENTATION_HOLD) || defined(USE_SOARING) +#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). diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 3f917c8009c..27e650faac4 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -501,7 +501,7 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < max_eph_epv)) { if (posEstimator.gps.epv < max_eph_epv -#ifdef USE_ORIENTATION_HOLD +#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, @@ -640,7 +640,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; -#ifdef USE_ORIENTATION_HOLD +#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 diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 8557fdfad86..56cc43ecc60 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -85,7 +85,7 @@ // 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_ORIENTATION_HOLD +#define USE_FW_AEROBATICS #define USE_THRUST_VECTORING #define USE_CRASH_DETECTION #define USE_SOARING diff --git a/src/main/target/common.h b/src/main/target/common.h index f5874d95779..b3ecdb7c442 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,7 +209,7 @@ // 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_ORIENTATION_HOLD +#define USE_FW_AEROBATICS #define USE_THRUST_VECTORING #define USE_CRASH_DETECTION // Thermal soaring (net-energy vario + wind-shifted thermal centering): diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 0ff7b3c879d..8ed9218e3ec 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -466,7 +466,7 @@ static void crsfFrameFlightMode(sbuf_t *dst) crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); static uint8_t hrstSent = 0; -#ifdef USE_ORIENTATION_HOLD +#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 @@ -499,7 +499,7 @@ 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_ORIENTATION_HOLD +#ifdef USE_FW_AEROBATICS } else if (altitudeFloorRecoveryActive()) { flightMode = altitudeFloorOrbitActive() ? "ORBT" : "CTCH"; } else if (florSent > 0) { From 0357a61de4b7b7bf315eb50bab357fee1924471b Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 19:02:21 +0200 Subject: [PATCH 094/108] crash: compare the rotation squared instead of taking a sqrt sqrt(r^2+p^2+y^2) < 15 is exactly r^2+p^2+y^2 < 15^2 - drop the per-loop sqrt on these flash- and cycle-tight boards. Behaviour is unchanged. The comment records why the crash stillness keeps the vector magnitude (not the landing detector's mean-abs averageAbsGyroRates, which blinds to a single-axis wreck) and its own baro-rate + GPS ground speed (the impact corrupts the fused velocity for ~4.5 s). Co-Authored-By: Claude Opus 4.8 --- src/main/flight/crash_detection.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/flight/crash_detection.c b/src/main/flight/crash_detection.c index 372a2cebe27..0f6ba13d6cc 100644 --- a/src/main/flight/crash_detection.c +++ b/src/main/flight/crash_detection.c @@ -199,10 +199,18 @@ void crashDetectionUpdate(float dT) } impactWindowS -= dT; - const float rateMagDps = fast_fsqrtf(sq((float)gyroRateDps(FD_ROLL)) - + sq((float)gyroRateDps(FD_PITCH)) - + sq((float)gyroRateDps(FD_YAW))); - bool still = rateMagDps < CRASH_STILL_RATE_DPS + // 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; From ef6fc03dafa44a8dff582e0f511b51d94aa223e2 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 19:12:26 +0200 Subject: [PATCH 095/108] msp: keep the orientation-hold test injection off flight hardware MSP2_INAV_ORIENTATION_HOLD_TEST is a bench/HIL level-1 verification hook (inject two quaternions, read back the attitude error and rate target) - no place on a real FC. Gate its handler on SITL_BUILD as well as USE_FW_AEROBATICS (which it needs for orientationHoldComputeAttitudeError). Also add the missing EOF newline to msp_protocol_v2_inav.h. Co-Authored-By: Claude Opus 4.8 --- src/main/fc/fc_msp.c | 3 ++- src/main/msp/msp_protocol_v2_inav.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a4c8f4fcaa2..64f4ef44491 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4498,7 +4498,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif -#ifdef USE_FW_AEROBATICS +// 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. diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index b9f56c36d1f..97e94f081d8 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -136,4 +136,4 @@ #define MSP2_INAV_FIGURE_SEQUENCE 0x2240 #define MSP2_INAV_SET_FIGURE_SEQUENCE 0x2241 -#define MSP2_INAV_ORIENTATION_HOLD_TEST 0x2242 //in/out: level-1 test injection, 8x float32 (q_est wxyz, q_target wxyz) -> 6x float32 (err_deg xyz, rate_target_dps xyz); pure computation \ No newline at end of file +#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 From 688177db1f2e2b951fd87084762c5fb0ae0266e6 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 20:03:56 +0200 Subject: [PATCH 096/108] soaring: idle the motor while thermalling so it soars on the lift The module found and centred a thermal but never touched the throttle - a measure-only soarer. Add the ArduSoar motor handling: while BOXSOARING is active and the aircraft is circling a thermal (thermalling already implies armed + airplane + pitot and altitude within [soar_alt_min, soar_alt_max]), force the throttle to idle so the glider soars on the lift. It is a throttle-to-idle override, not a motor stop/disarm: the control surfaces keep flying the loiter, and normal throttle returns the instant the thermal is left or the aircraft sinks below soar_alt_min (both drop thermalling). Reuse the fixed-wing throttle-override hook the neighbours use - soaringThrottleApply() bends mixerThrottleCommand in mixTable()'s FW branch, the same apply-chain as hoverThrottleApply / orientationHoldLoadGovernorThrottle, not the getMotorStatus() -> MOTOR_STOPPED path (that hard-stops a multirotor's PID-mixed motors; soaring wants a soft idle). Applied before the aerobatics paths so an altitude-floor / rotor-guard recovery climb can still override the idle. No new setting: thermalling + soar_alt_min already encode the condition. The motor state stays observable via soaringThermalling() and the throttle log; SITL adds debug[4] = the applied idle throttle to the existing SITL_BUILD block. FW only, gated on USE_SOARING; a non-soaring build is byte-unaffected. Co-Authored-By: Claude Opus 4.8 --- src/main/flight/mixer.c | 9 +++++++++ src/main/flight/soaring.c | 15 +++++++++++++++ src/main/flight/soaring.h | 5 +++++ 3 files changed, 29 insertions(+) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 453ca81c843..bde8ca27b08 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -51,6 +51,7 @@ #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" @@ -593,6 +594,14 @@ 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); diff --git a/src/main/flight/soaring.c b/src/main/flight/soaring.c index 73917d02932..a9b5c11c112 100644 --- a/src/main/flight/soaring.c +++ b/src/main/flight/soaring.c @@ -43,6 +43,7 @@ #include "fc/settings.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/soaring.h" #include "flight/wind_estimator.h" @@ -183,6 +184,7 @@ void soaringUpdate(float dT) 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] = getThrottleIdleValue(); // applied idle throttle [us] #endif // leave the thermal: lift collapsed, or out of the altitude band @@ -213,4 +215,17 @@ float soaringNetVarioCms(void) return netVarioCms; } +int16_t soaringThrottleApply(int16_t throttle) +{ + // While circling a thermal the glider soars on the lift with the motor + // idled: a throttle-to-idle override, NOT a motor stop - 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 getThrottleIdleValue(); + } + return throttle; +} + #endif // USE_SOARING diff --git a/src/main/flight/soaring.h b/src/main/flight/soaring.h index 575a45b549a..442df58c977 100644 --- a/src/main/flight/soaring.h +++ b/src/main/flight/soaring.h @@ -68,3 +68,8 @@ 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); From 4e399d65c325d07c6623b65ae4a5f808cb878957 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 19 Jul 2026 21:19:24 +0200 Subject: [PATCH 097/108] aerobatics: hide 11 tuning settings behind clean fixed inits Reduces the aerobatics CLI surface (maintainer feedback: ~30 settings is too many) without losing runtime tunability. The 11 removed settings are values the controller either regulates away or that are fixed airframe properties, not things a pilot should pick: ohold_inverted/knife_left/knife_right_pitch_trim, ohold_knife_speed_ff feed-forwards the integrating throttle assist regulates to vz->0; seed at neutral 0 (field kept, no CLI setting). ohold_hover/inverted/knife/figure_gain seeds for the limit-cycle gain learner, which converges to the highest non-oscillating gain from any start; fixed 100 %, read back on the new DEBUG_FW_AEROBATICS blackbox channel instead of a setting. ohold_hover_thr_min redundant with INAV's motor idle; the hover floor is now just getThrottleIdleValue(). Field removed (PG_HOVER_THROTTLE_CONFIG 3->4). ohold_hover_baro_weight fixed init 100 (field kept, runtime-tunable). ahrs_gps_aiding_max_tilt a fixed property of the coordinated-flight assumption, not a knob; now #define GPS_AIDING_MAX_TILT_DEG 60, on/off stays on the FEATURE_FW_AEROBATICS bit. Field removed (PG_IMU_CONFIG 3->4). The two PG version bumps reset hoverThrottleConfig and imuConfig to defaults on flashing. Kept fields keep the reversibility Daniel asked for: a future CLI setting can be re-added without touching the control code. Co-Authored-By: Claude Opus 4.8 --- docs/Settings.md | 110 ----------------------------- src/main/build/debug.h | 1 + src/main/fc/cli.c | 3 +- src/main/fc/settings.yaml | 72 ------------------- src/main/flight/hover_throttle.c | 17 ++--- src/main/flight/hover_throttle.h | 9 --- src/main/flight/imu.c | 15 ++-- src/main/flight/imu.h | 1 - src/main/flight/orientation_hold.c | 34 ++++++--- 9 files changed, 47 insertions(+), 215 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 80e9e0de4e6..fea9f89ce9e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -252,16 +252,6 @@ Inertial Measurement Unit KP Gain for compass measurements --- -### ahrs_gps_aiding_max_tilt - -Tilt from level [deg] beyond which ALL GPS-derived AHRS aiding (yaw from course, centrifugal compensation) fades out on an airplane - both assume coordinated forward flight and actively bend the attitude estimate in a hang, knife edge, inverted or spin (measured). Instant fade-out, 2 s fade-in after returning below the limit. 0 disables the gate. - -| Default | Min | Max | -| --- | --- | --- | -| 60 | 0 | 90 | - ---- - ### ahrs_gps_yaw_weight Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass @@ -4572,106 +4562,6 @@ Target slew rate [deg/s] for entering an orientation hold preset (INVERTED, KNIF --- -### ohold_figure_gain - -Learned angle-gain scale [%] while a figure flies, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable - flying the same figures repeatedly converges it. 100 = reference gains. - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 30 | 100 | - ---- - -### ohold_hover_baro_weight - -Baro position weight (x100) while the hover throttle owns the altitude, applied as a floor over inav_w_z_baro_p. Hovering thrust pollutes the accelerometer, the baro deserves more trust than in forward flight. 0 keeps the global weight. - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 0 | 150 | - ---- - -### ohold_hover_gain - -LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain. - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 30 | 100 | - ---- - -### ohold_hover_thr_min - -Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. The altitude/vz loop gains themselves are not settings: they derive at runtime from the learned hover point (throttle-to-thrust slope), see hover_throttle.c. - -| Default | Min | Max | -| --- | --- | --- | -| 1000 | 1000 | 1800 | - ---- - -### ohold_inverted_gain - -Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains. - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 30 | 100 | - ---- - -### ohold_inverted_pitch_trim - -Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias) - -| Default | Min | Max | -| --- | --- | --- | -| 0 | -15 | 15 | - ---- - -### ohold_knife_gain - -Learned angle-gain scale [%] for the knife edge holds, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains. - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 30 | 100 | - ---- - -### ohold_knife_left_pitch_trim - -Pitch trim [deg] on the KNIFE EDGE LEFT hold target, positive = nose above the horizon, held via the rudder. Separate per side: the body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other side, so left/right = shared fuselage-lift part +/- prop part. Reversed prop rotation swaps the sides - -| Default | Min | Max | -| --- | --- | --- | -| 0 | -15 | 15 | - ---- - -### ohold_knife_right_pitch_trim - -Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the horizon, held via the rudder. See ohold_knife_left_pitch_trim for why the sides differ - -| Default | Min | Max | -| --- | --- | --- | -| 0 | -15 | 15 | - ---- - -### ohold_knife_speed_ff - -Knife edge speed feedforward: extra nose-above-horizon angle [deg] per half throttle of speed deficit. The fuselage side force carries the weight and scales with speed squared, so flying slower needs more nose angle immediately - this feeds it forward from the throttle (the speed proxy) instead of waiting for an altitude error. 0 = off. - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 30 | - ---- - ### 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. 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/fc/cli.c b/src/main/fc/cli.c index d6e53c80258..58a58d593d1 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -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/settings.yaml b/src/main/fc/settings.yaml index 447b6b79dd5..616041b5a40 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1567,12 +1567,6 @@ groups: field: acc_ignore_slope min: 0 max: 10 - - name: ahrs_gps_aiding_max_tilt - description: "Tilt from level [deg] beyond which ALL GPS-derived AHRS aiding (yaw from course, centrifugal compensation) fades out on an airplane - both assume coordinated forward flight and actively bend the attitude estimate in a hang, knife edge, inverted or spin (measured). Instant fade-out, 2 s fade-in after returning below the limit. 0 disables the gate." - default_value: 60 - field: gps_aiding_max_tilt - min: 0 - max: 90 - name: ahrs_gps_yaw_windcomp description: "Wind compensation in heading estimation from gps groundcourse(fixed wing only)" default_value: ON @@ -4586,60 +4580,12 @@ groups: headers: ["flight/orientation_hold.h"] condition: USE_FW_AEROBATICS members: - - name: ohold_inverted_pitch_trim - description: "Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias)" - default_value: 0 - field: invertedPitchTrim - min: -15 - max: 15 - - name: ohold_knife_left_pitch_trim - description: "Pitch trim [deg] on the KNIFE EDGE LEFT hold target, positive = nose above the horizon, held via the rudder. Separate per side: the body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other side, so left/right = shared fuselage-lift part +/- prop part. Reversed prop rotation swaps the sides" - default_value: 0 - field: knifeLeftPitchTrim - min: -15 - max: 15 - - name: ohold_knife_speed_ff - description: "Knife edge speed feedforward: extra nose-above-horizon angle [deg] per half throttle of speed deficit. The fuselage side force carries the weight and scales with speed squared, so flying slower needs more nose angle immediately - this feeds it forward from the throttle (the speed proxy) instead of waiting for an altitude error. 0 = off." - default_value: 0 - field: knifeSpeedFF - min: 0 - max: 30 - 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_inverted_gain - description: "Learned angle-gain scale [%] for the inverted hold, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains." - default_value: 100 - field: invertedGainLearned - min: 30 - max: 100 - - name: ohold_knife_gain - description: "Learned angle-gain scale [%] for the knife edge holds, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable. 100 = reference gains." - default_value: 100 - field: knifeGainLearned - min: 30 - max: 100 - - name: ohold_figure_gain - description: "Learned angle-gain scale [%] while a figure flies, relative to the normal-flight gains. Maintained by the firmware's limit-cycle detector and saved on disarm; editable - flying the same figures repeatedly converges it. 100 = reference gains." - default_value: 100 - field: figureGainLearned - min: 30 - max: 100 - - name: ohold_hover_gain - description: "LEARNED hover angle-gain scale [%]. The prop hang limit-cycle detector writes this at hang exit and it is saved on disarm; the next hang and the next flight start at the learned value instead of oscillating down again. Editable, but normally maintained by the firmware. 100 = full angle gain." - default_value: 100 - field: hoverGainLearned - min: 30 - max: 100 - - name: ohold_knife_right_pitch_trim - description: "Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the horizon, held via the rudder. See ohold_knife_left_pitch_trim for why the sides differ" - default_value: 0 - field: knifeRightPitchTrim - min: -15 - max: 15 - 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 @@ -4701,24 +4647,6 @@ groups: min: 0 max: 30 - - name: PG_HOVER_THROTTLE_CONFIG - type: hoverThrottleConfig_t - headers: ["flight/hover_throttle.h"] - condition: USE_FW_AEROBATICS - members: - - name: ohold_hover_thr_min - description: "Hover throttle floor [us]. The hover altitude controller never cuts the throttle below this, preserving the control authority that scales with thrust - prop wash over the control surfaces as well as thrust vectoring (an updraft otherwise starves the attitude authority; excess lift is accepted as a climb). Find it by experiment, slightly below the hover throttle. 1000 = no floor beyond motor idle. The altitude/vz loop gains themselves are not settings: they derive at runtime from the learned hover point (throttle-to-thrust slope), see hover_throttle.c." - default_value: 1000 - field: minThrottle - min: 1000 - max: 1800 - - name: ohold_hover_baro_weight - description: "Baro position weight (x100) while the hover throttle owns the altitude, applied as a floor over inav_w_z_baro_p. Hovering thrust pollutes the accelerometer, the baro deserves more trust than in forward flight. 0 keeps the global weight." - default_value: 100 - field: hoverBaroWeight - min: 0 - max: 150 - - name: PG_SOARING_CONFIG type: soaringConfig_t headers: ["flight/soaring.h"] diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index e281201ee7d..7dd4aede307 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -59,11 +59,13 @@ #include "sensors/battery.h" -PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 4); PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, - .minThrottle = SETTING_OHOLD_HOVER_THR_MIN_DEFAULT, - .hoverBaroWeight = SETTING_OHOLD_HOVER_BARO_WEIGHT_DEFAULT, + // 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 @@ -349,11 +351,10 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) const float zErrM = (targetAltCm - z) / 100.0f; const float climbMs = climbCms / 100.0f; - // throttle floor: never cut the throttle below what 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 = MAX(getThrottleIdleValue(), - (int16_t)hoverThrottleConfig()->minThrottle); + // 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 diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h index ca0f57d6e04..6e29abeff75 100644 --- a/src/main/flight/hover_throttle.h +++ b/src/main/flight/hover_throttle.h @@ -43,15 +43,6 @@ // constants; no pilot can pick "microseconds per meter per second" better // than that identity does. typedef struct hoverThrottleConfig_s { - uint16_t minThrottle; // throttle floor [us] while hovering: preserves - // the control authority that scales with thrust, - // prop wash over the surfaces as well as thrust - // vectoring (an updraft otherwise makes the PID - // cut the throttle and with it the authority). - // One propeller, one floor: the same value - // covers both steering paths. Found by - // experiment near the model's hover throttle; - // 1000 = no floor beyond the motor idle. uint8_t hoverBaroWeight; // baro position weight (x100) while the hover // throttle owns the altitude: hovering thrust // pollutes the accelerometer Z, the baro diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index db661a23b78..426c2d5fc6c 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -124,7 +124,7 @@ static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_sl static void imuUpdateGpsAidingTiltWeight(float dT); static float gpsAidingTiltWeight = 1.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 3); +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 @@ -136,8 +136,7 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT, .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT, .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT, - .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT, - .gps_aiding_max_tilt = SETTING_AHRS_GPS_AIDING_MAX_TILT_DEFAULT + .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) @@ -1055,16 +1054,20 @@ float calculateCosTiltAngle(void) // 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) || !imuConfig()->gps_aiding_max_tilt) { + if (!feature(FEATURE_FW_AEROBATICS) || !STATE(AIRPLANE)) { gpsAidingTiltWeight = 1.0f; return; } - const float cosLimit = cos_approx(DEGREES_TO_RADIANS(imuConfig()->gps_aiding_max_tilt)); + const float cosLimit = cos_approx(DEGREES_TO_RADIANS(GPS_AIDING_MAX_TILT_DEG)); if (calculateCosTiltAngle() < cosLimit) { gpsAidingTiltWeight = 0.0f; } else { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 33cb01fee9a..60eb964bd68 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -53,7 +53,6 @@ typedef struct imuConfig_s { uint8_t gps_yaw_windcomp; uint8_t inertia_comp_method; uint16_t gps_yaw_weight; - uint8_t gps_aiding_max_tilt; // [deg from level] beyond this tilt ALL GPS aiding fades out (0 = off) } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index ef067679426..904f10ec626 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -29,6 +29,8 @@ #ifdef USE_FW_AEROBATICS +#include "build/debug.h" + #include "common/axis.h" #include "common/maths.h" #include "common/quaternion.h" @@ -65,17 +67,25 @@ PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 2); PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, - .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT, - .knifeLeftPitchTrim = SETTING_OHOLD_KNIFE_LEFT_PITCH_TRIM_DEFAULT, - .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT, - .hoverGainLearned = SETTING_OHOLD_HOVER_GAIN_DEFAULT, - .invertedGainLearned = SETTING_OHOLD_INVERTED_GAIN_DEFAULT, - .knifeGainLearned = SETTING_OHOLD_KNIFE_GAIN_DEFAULT, - .figureGainLearned = SETTING_OHOLD_FIGURE_GAIN_DEFAULT, + // 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, - .knifeSpeedFF = SETTING_OHOLD_KNIFE_SPEED_FF_DEFAULT, .loadLimitG = SETTING_OHOLD_LOAD_LIMIT_DEFAULT, ); @@ -766,6 +776,14 @@ static void regimeGainUpdate(const fpVector3_t *errDeg, float dT) 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) { From 7b8d567108cf03cbba0b3c291d87f76bb127d4c7 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 20 Jul 2026 07:03:29 +0200 Subject: [PATCH 098/108] docs: frame the altitude floor as the panic net; drop the stale look-ahead The floor is the panic safety net: diving low, the pilot forgets to flip the aerobatic mode switch back, and whatever mode is selected (any hold, figure, or spin) the floor switches it OUT and flies a normal stable upright attitude that pulls up and away - a stable attitude is what prevents the crash. Also documents the mode LATCH (the interrupted mode stays locked until the flight-mode switch is cycled, so a held/forgotten switch cannot drop straight back into the ground). Fixes a stale claim: the doc said a "predicted breach (sink rate looked ahead a few seconds)" triggers the recovery. The firmware deliberately does NOT predict (altitude_floor.c: "A piloted trajectory is not predictable ... the old 3 s lookahead"); sinking THROUGH the floor line is the trigger, matching the alt_floor_altitude setting description. Co-Authored-By: Claude Opus 4.8 --- docs/OrientationHold.md | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md index 0326257f189..56b3350e9e5 100644 --- a/docs/OrientationHold.md +++ b/docs/OrientationHold.md @@ -138,23 +138,34 @@ it as "hold whatever I'm doing" for improvised 3D. ### FLOOR (altitude safety floor) -Problem being solved: practicing low 3D means a mistake reaches the -ground before you do. +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. -- A predicted breach (sink rate looked ahead a few seconds) engages an - automatic upright + climb recovery that OVERRIDES the selected mode. - It catches out of a dive with the elevator still held, and out of a - spin. +- 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 climb ends at floor + `alt_floor_margin`. To take over earlier: - center the sticks once, then any fresh roll/pitch input hands control - back immediately. Switching the box off always ends it. +- The catch LATCHES the aerobatic mode out: the interrupted mode stays + locked until the pilot moves the flight-mode switch away and back - a + forgotten or held-on switch cannot drop the aircraft straight back + into the ground. +- The climb ends at floor + `alt_floor_margin`; back above it and + climbing, control returns. To take over earlier: center the sticks + once, then any fresh roll/pitch input hands control back immediately. + Switching the FLOOR box off always ends it. ### Figures (F ROLL, F LOOP, F 4PT, F SEQ) From 481e959759bb15398de6edb2b533d79a8d13b926 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 20 Jul 2026 08:08:28 +0200 Subject: [PATCH 099/108] fw(aerobatics): hold the floor latch through the un-requested state The floor catch latches the interrupted hold/figure box out so it cannot restart on recovery release and dive back into the floor (the fly-up / fall-back loop). But the latch was cleared by orientationHoldResetSource Tracking(), which fc_core calls whenever ORIENTATION_HOLD_MODE is inactive - and the latch is exactly WHY the mode is inactive (it blocks the interrupted box). So the moment the recovery ended, the reset cleared the latch, the box re-engaged, and the aircraft flew straight back into its aerobatic attitude on the next stick touch (measured: brief stick touch after an inverted catch drove the roll back to 176 deg). Clear the latch only on a real exit - the pilot deselecting the latched mode (orientationHoldFloorLatchTick) or disarm - never just because the latch itself made the hold un-requested. Co-Authored-By: Claude Opus 4.8 --- src/main/flight/orientation_hold.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index 904f10ec626..e5c2c2b7faf 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -857,7 +857,16 @@ void orientationHoldResetSourceTracking(void) activeTargetSource = OHOLD_SOURCE_NONE; } exitSlewActive = false; - floorLatchedSource = OHOLD_SOURCE_NONE; // disarm/mode-exit hygiene + // 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) From ea480cc831d2031fd9378341a0c814407ce48651 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 20 Jul 2026 08:14:48 +0200 Subject: [PATCH 100/108] docs: floor latch is universal (any figure) and re-engages only on a switch cycle Co-Authored-By: Claude Opus 4.8 --- docs/OrientationHold.md | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md index 56b3350e9e5..f29a764bab8 100644 --- a/docs/OrientationHold.md +++ b/docs/OrientationHold.md @@ -158,14 +158,19 @@ that prevents the crash; the floor just makes the aircraft take one. 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 the aerobatic mode out: the interrupted mode stays - locked until the pilot moves the flight-mode switch away and back - a - forgotten or held-on switch cannot drop the aircraft straight back - into the ground. -- The climb ends at floor + `alt_floor_margin`; back above it and - climbing, control returns. To take over earlier: center the sticks - once, then any fresh roll/pitch input hands control back immediately. - Switching the FLOOR box off always ends it. +- 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) From 50d5f2242816674560f75a84967d3d57c4554cfc Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:54 +0200 Subject: [PATCH 101/108] fw(estimator): C/N0 fast-loss gate (NAV-SIG top-5 mean, -12 dB collapse, +6 recover, NO_FIX same cycle) + HITL C/N0 injection Co-Authored-By: Claude Fable 5 --- src/main/fc/fc_msp.c | 14 +++++++++ src/main/fc/runtime_config.h | 2 ++ src/main/io/gps.c | 59 ++++++++++++++++++++++++++++++++++++ src/main/io/gps.h | 4 +++ src/main/io/gps_ublox.c | 36 ++++++++++++++++++++++ 5 files changed, 115 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 64f4ef44491..e0b5bd277d2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4283,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); @@ -4395,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 diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 428efcae6ac..fc34b64647a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -203,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 { @@ -215,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/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: From b604cb2aa703879dfc35be5e3107542e403be0b5 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:54 +0200 Subject: [PATCH 102/108] fw(estimator): residual-gate hook pinned with measurements; DR aero-constraint coast; knife baro ram-fade; mag tilt gate Co-Authored-By: Claude Fable 5 --- src/main/flight/imu.c | 24 ++++++- .../navigation/navigation_pos_estimator.c | 71 ++++++++++++++++++- 2 files changed, 92 insertions(+), 3 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 426c2d5fc6c..e93c97dd751 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -402,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). @@ -436,7 +446,12 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // 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)) { + 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 @@ -921,6 +936,13 @@ 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, diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 27e650faac4..9455f9d4801 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -509,6 +509,13 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) // (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; @@ -639,7 +646,48 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); 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_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 @@ -727,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; From 95607cb349942ae5a1a7b4a2759a55387af60c62 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:54 +0200 Subject: [PATCH 103/108] fw(aerobatics): throttle cap-only, no exceptions - recoveries fly MIN(need, stick), keep-alive removed, floor power gated nose-up Co-Authored-By: Claude Fable 5 --- src/main/flight/altitude_floor.c | 29 ++++++++++++----- src/main/flight/altitude_floor.h | 3 ++ src/main/flight/hover_throttle.c | 36 ++++++++++++++++------ src/main/flight/mixer.c | 14 ++++----- src/main/navigation/navigation_fixedwing.c | 14 +++++++++ 5 files changed, 71 insertions(+), 25 deletions(-) diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c index 31be3f3912f..7c5008390d3 100644 --- a/src/main/flight/altitude_floor.c +++ b/src/main/flight/altitude_floor.c @@ -241,16 +241,29 @@ 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 must not ride whatever throttle the pilot froze - // in the dive (a panic chop leaves idle): at least the airframe's - // cruise throttle plus the standard pitch-to-throttle compensation - // for the climb angle. 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) { + // 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 diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h index e4a1fba794e..f69854e72cb 100644 --- a/src/main/flight/altitude_floor.h +++ b/src/main/flight/altitude_floor.h @@ -74,6 +74,9 @@ bool altitudeFloorOrbitViaNav(void); // 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 diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c index 7dd4aede307..40ec10e0d23 100644 --- a/src/main/flight/hover_throttle.c +++ b/src/main/flight/hover_throttle.c @@ -234,8 +234,13 @@ static int16_t knifeInvertedAssistApply(int16_t pilotThrottle, float elevDeg) 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(), getMaxThrottle()); + getThrottleIdleValue(), pilotThrottle); } bool hoverThrottleIsEngaged(void) @@ -275,15 +280,21 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) return knifeInvertedAssistApply(pilotThrottle, elevDeg); } assistActive = false; - // Recovery throttle floors are OWNED by their modules (the floor's - // climb math and its nav-orbit suppression, the rotor guard's - // fixed add): this path only takes the highest claim - and more - // pilot throttle always wins. + // 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) { - return constrain(MAX(pilotThrottle, recoveryFloor), - getThrottleIdleValue(), getMaxThrottle()); + // 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; } @@ -361,8 +372,10 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) // 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, getMaxThrottle()); + floorThrottle, pilotThrottle); // thrust supports the weight with its vertical component only: // compensate the tilt away from the zenith (capped, the elevation @@ -371,7 +384,12 @@ int16_t hoverThrottleApply(int16_t pilotThrottle) const float correction = (HOVER_THR_STIFFNESS_S2 * spanUsPerG / GRAVITY_MSS * zErrM - HOVER_THR_DAMPING_S * spanUsPerG / GRAVITY_MSS * climbMs) / vertical; - const int16_t outUs = constrain(lrintf(iTermUs + correction), floorThrottle, getMaxThrottle()); + // 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; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index bde8ca27b08..09beeb2643f 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -713,14 +713,12 @@ motorStatus_e getMotorStatus(void) if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) { #ifdef USE_FW_AEROBATICS - // the altitude floor recovery climbs on its own throttle floor - a - // panic-chopped stick must not stop the motor that climb needs (the - // same override navigation gets via nav_overrides_motor_stop). The - // rotor guard recovery NEEDS the motor even more: thrust is its - // only means of restoring rotor rpm. - if (STATE(AIRPLANE) && (altitudeFloorRecoveryActive() || rotorGuardRecoveryActive())) { - return MOTOR_RUNNING; - } + // 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 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 90b3a9ebd0b..4fe7605f887 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -111,6 +111,20 @@ void resetFixedWingAltitudeController(void) bool adjustFixedWingAltitudeFromRCInput(void) { +#if defined(USE_FW_AEROBATICS) || defined(USE_SOARING) + // Forced poshold (floor orbit / thermal loiter): NO RC altitude adjust. + // The nav loiter WRITES rcCommand to fly the circle, and this adjust + // reads rcCommand[PITCH] - the loiter's own pitch output fed back as a + // "pilot climb command" is a positive feedback loop that rode the + // floor orbit away from its anchor with the sticks untouched + // (measured: smooth pitch 8->23 deg, ~+4 m/s, 60 m past the anchor + // Z while nav actual == estimator Z). The anchor Z is the contract; + // pilot pitch is a TAKEOVER there, not a climb knob - same raw-stick + // lesson as the floor's release detection. + if (posControl.flags.forcedPosholdActive) { + return false; + } +#endif int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500); if (rcAdjustment) { From 87cb6423af0dd4483dd677ae95966b5fe41585b0 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:54 +0200 Subject: [PATCH 104/108] fw(aerobatics): heading anchors use the negated-yaw attitude convention; assist gate on the full error length; entry ramp removed Co-Authored-By: Claude Fable 5 --- src/main/fc/fc_core.c | 9 ++ src/main/flight/orientation_hold.c | 220 ++++++++++++++++++++++------- src/main/flight/orientation_hold.h | 8 ++ 3 files changed, 186 insertions(+), 51 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3eed06e2b13..70cba7069d1 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -712,6 +712,15 @@ void processRx(timeUs_t currentTimeUs) #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 diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c index e5c2c2b7faf..5c7225ebcf7 100644 --- a/src/main/flight/orientation_hold.c +++ b/src/main/flight/orientation_hold.c @@ -63,8 +63,10 @@ #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, 2); +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 @@ -86,6 +88,8 @@ PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, .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, ); @@ -117,8 +121,9 @@ 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, 90.0f }, // + PROP HANG (torque roll) { 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) @@ -132,8 +137,9 @@ static const orientationHoldPreset_t * orientationHoldActivePreset(void) 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]; - if (IS_RC_MODE_ACTIVE(BOXPROPHANG)) return &orientationHoldSpinPresets[3]; - return &orientationHoldSpinPresets[4]; + // + 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 @@ -466,13 +472,23 @@ static float orientationHoldSlewTargetFull(fpQuaternion_t *qSoll, const fpQuater // 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) { - // figure on a line: the full error adds the heading (twist) - // component the reduced error deliberately drops + 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); @@ -530,6 +546,8 @@ static void orientationHoldCheckSourceSwitch(int source) // 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; } } @@ -589,7 +607,9 @@ void orientationHoldUpInBody(fpVector3_t *upBody) // hold is purely additive. State lives next to the source tracking above. static void figureCaptureYawAnchor(void) { - const float halfPsiRad = DECIDEGREES_TO_RADIANS(attitude.values.yaw) * 0.5f; + // 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; @@ -920,6 +940,8 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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. @@ -986,6 +1008,9 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) } 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. @@ -1038,12 +1063,14 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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 stick offsets, ANGLE semantics around the rotated reference: - // deflection = body-frame angle offset from the preset, held while - // deflected; centered sticks return the target slowly. Yaw stays a - // rate command (the free axis). While this is active the rate path - // must not also feed roll/pitch sticks as rates, see - // orientationHoldSticksAreTargetOffsets(). + // 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) { @@ -1052,6 +1079,66 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) 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; @@ -1061,7 +1148,11 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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. - if (fabsf(entryErr.x) < 25.0f && fabsf(entryErr.y) < 25.0f && pitchOffDeg == 0.0f + // 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); @@ -1072,16 +1163,49 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) holdRefAltCm = getEstimatedActualPosition(Z); } - if (rollOffDeg != 0.0f || pitchOffDeg != 0.0f) { - const fpVector3_t offVec = { .v = { rollOffDeg, pitchOffDeg, 0.0f } }; + if (fabsf(carveVec.x) > 0.01f || fabsf(carveVec.y) > 0.01f || fabsf(carveVec.z) > 0.01f) { fpQuaternion_t qOff; - quatFromRotVecDeg(&qOff, &offVec); + quatFromRotVecDeg(&qOff, &carveVec); quaternionMultiply(&qDesired, &qDesired, &qOff); - // carving: keep following at the entry rate + // carving / leaning: keep following at the entry rate } else if (orientationHoldConfig()->stickAngleMaxDeg > 0 && presetSlewCaptured) { - // sticks centered after capture: gentle return to the preset + // 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) { @@ -1090,7 +1214,7 @@ bool orientationHoldComputeError(fpVector3_t *errDeg, float dT) // 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 + const float remainingDeg = (figureLineAnchored || pilotFullError) ? orientationHoldSlewTargetFull(&qSollState, &qDesired, governedStepDeg) : orientationHoldSlewTarget(&qSollState, &qDesired, governedStepDeg); if (remainingDeg < 1.0f) { @@ -1262,18 +1386,19 @@ bool orientationHoldApplyRateTargets(oholdAxisRate_t axes[XYZ_AXIS_COUNT], float const bool stickOffsets = orientationHoldSticksAreTargetOffsets() || altitudeFloorRecoveryActive() || rotorGuardRecoveryActive(); - // controlled spin (FLAT SPIN family or figure SPIN segment): the spin - // command is a rotation about the EARTH VERTICAL - exactly the axis the - // reduced attitude error leaves free - distributed onto the body axes - // via the earth-up direction in the body frame. At flat/inverted that - // is the yaw axis, at knife edge the pitch axis, at the hang the roll - // axis (torque roll). Rates along this axis leave the tilt untouched, - // so holding and spinning never fight (bench math mirror, section H). + // 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; - fpVector3_t upBody; if (spinSegment) { spinRateDps = spinYawNorm * currentControlProfile->stabilized.rates[FD_YAW] * 10.0f; } else if (spinPreset) { @@ -1290,26 +1415,10 @@ bool orientationHoldApplyRateTargets(oholdAxisRate_t axes[XYZ_AXIS_COUNT], float #define SPIN_ABOUT_VERTICAL_MAX_DPS 180.0f spinRateDps = constrainf(spinRateDps, -SPIN_ABOUT_VERTICAL_MAX_DPS, SPIN_ABOUT_VERTICAL_MAX_DPS) * orientationHoldLoadGovernorScale(); - if (spinSegment || spinPreset) { - orientationHoldUpInBody(&upBody); - // AIRCRAFT-referenced stick sense: the body axis nearest the - // vertical receives the stick with its own positive sign - right - // rudder yaws the airframe right at flat AND inverted (so the - // rotation seen from above reverses when inverted, exactly like a - // real aircraft), and maps to positive pitch at the knife edge. - // The sign flip does not disturb the tilt (the distribution stays - // along the free axis either way). - float dominant = upBody.z; - if (fabsf(upBody.y) > fabsf(dominant)) { - dominant = upBody.y; - } - if (fabsf(upBody.x) > fabsf(dominant)) { - dominant = upBody.x; - } - if (dominant < 0.0f) { - vectorScale(&upBody, &upBody, -1.0f); - } - } + // 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 @@ -1331,12 +1440,21 @@ bool orientationHoldApplyRateTargets(oholdAxisRate_t axes[XYZ_AXIS_COUNT], float rateTarget = pt1FilterApply4(axes[axis].levelFilter, rateTarget, pidBank()->pid[PID_LEVEL].I, dT); } - float stickRate = (stickOffsets && axis != FD_YAW) ? 0.0f : axes[axis].stickRateDps; + // 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 } - rateTarget += spinRateDps * upBody.v[axis]; + if (axis == spinAxis) { + rateTarget += spinRateDps; // body-axis rotation per contract + } } axes[axis].rateTargetDps = constrainf(stickRate + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); } diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h index 3159001e267..4cdc59465b9 100644 --- a/src/main/flight/orientation_hold.h +++ b/src/main/flight/orientation_hold.h @@ -71,6 +71,14 @@ typedef struct orientationHoldConfig_s { // 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, From c8249a2db632d685baf53ee1bbcf1999b0b8776d Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:54 +0200 Subject: [PATCH 105/108] fw(guard): commanded-attitude limiter under the GYRO box + guard tuning Co-Authored-By: Claude Fable 5 --- src/main/flight/pid.c | 13 ++++++++ src/main/flight/rotor_guard.c | 56 ++++++++++++++++++++++++++--------- src/main/flight/rotor_guard.h | 19 ++++++++++-- 3 files changed, 71 insertions(+), 17 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 16d91e2ed93..67b477170f0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -45,6 +45,7 @@ #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" @@ -647,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); diff --git a/src/main/flight/rotor_guard.c b/src/main/flight/rotor_guard.c index 1beaaaaf342..2e87f75faff 100644 --- a/src/main/flight/rotor_guard.c +++ b/src/main/flight/rotor_guard.c @@ -42,19 +42,23 @@ #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, 0); +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, - .throttleAddUs = SETTING_ROTOR_GUARD_THROTTLE_ADD_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 @@ -69,12 +73,16 @@ PG_RESET_TEMPLATE(rotorGuardConfig_t, rotorGuardConfig, // 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) { @@ -114,6 +122,12 @@ void rotorGuardUpdate(void) 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; @@ -160,25 +174,39 @@ bool rotorGuardRecoveryActive(void) int16_t rotorGuardThrottleFloorUs(void) { - // Thrust is the ONLY lever that brings the rotor rpm (and with it - // the roll authority) back: a fixed floor above cruise, NOT - // pitch-scaled - the recovery pitch is nose DOWN and pitch-to- - // throttle would reduce it. 0 = no claim on the throttle. + // 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; } - return currentBatteryProfile->nav.fw.cruise_throttle - + rotorGuardConfig()->throttleAddUs; + 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) { - // Nose-down only while the roll excursion persists: it exists to feed - // the disk while the tilt has no authority. Once the wings answer - // again the recovery levels off - a T/W<1 autogyro can never climb - // nose-down, and the release condition (sink arrested) would - // otherwise never arrive (measured: a stable 1.2 m/s descent all the - // way into the ground). + // 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; } diff --git a/src/main/flight/rotor_guard.h b/src/main/flight/rotor_guard.h index a0c9bc5ca81..38b8105afda 100644 --- a/src/main/flight/rotor_guard.h +++ b/src/main/flight/rotor_guard.h @@ -40,9 +40,22 @@ typedef struct rotorGuardConfig_s { // purpose - excursion past it while sinking is // the tip-over signature uint16_t sinkCms; // minimum sink rate [cm/s] to qualify - int8_t recoveryPitchDeg; // nose-down pitch target during recovery - // (negative = down): restores inflow - uint16_t throttleAddUs; // recovery throttle floor = cruise + this + 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); From d136e908015e08db8aea3951115f14d46061ad29 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:55 +0200 Subject: [PATCH 106/108] fw(soaring): lift-grid centering v3 - wind-riding origin, rolling window, argmax steering Co-Authored-By: Claude Fable 5 --- src/main/flight/soaring.c | 257 +++++++++++++++++++++++++++++++------- 1 file changed, 214 insertions(+), 43 deletions(-) diff --git a/src/main/flight/soaring.c b/src/main/flight/soaring.c index a9b5c11c112..7d8683b059e 100644 --- a/src/main/flight/soaring.c +++ b/src/main/flight/soaring.c @@ -23,6 +23,7 @@ */ #include +#include #include #include @@ -38,6 +39,7 @@ #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" @@ -49,7 +51,10 @@ #include "navigation/navigation.h" +#include "rx/rx.h" + #include "sensors/acceleration.h" +#include "sensors/battery.h" #include "sensors/pitotmeter.h" #include "sensors/sensors.h" @@ -65,29 +70,65 @@ PG_RESET_TEMPLATE(soaringConfig_t, soaringConfig, .centreGainPct = SETTING_SOAR_CENTRE_GAIN_DEFAULT, ); -// Slow low-pass for the DC part of the net vario: the sin/cos gradient -// correlates the DEVIATION from this mean, so a strong-but-uniform column -// does not bias the shift. A few circle periods. -#define SOAR_VARIO_MEAN_TAU_S 10.0f -// Gradient low-pass ~ one circle period: it takes a full turn to see which -// side of the circle climbs best. -#define SOAR_GRAD_TAU_S 6.0f -// Converts the vario gradient [m/s] into a centre shift rate [cm/s] at -// centreGainPct = 100. The bench tunes centreGainPct on top; small, because -// "slowly" - a circle that chases noise eier and loses the thermal. +// 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 gradient would walk the loiter out of the sky). -#define SOAR_CENTRE_MAX_DRIFT_CM 30000.0f // 300 m +// (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 varioMean = 0.0f; -static float gradN = 0.0f, gradE = 0.0f; 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) { @@ -105,8 +146,27 @@ static float computeNetVarioMs(float dT) 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 + / (cosRoll * fast_fsqrtf(cosRoll)) // /cos^1.5 + * (1.0f - motorFactor); return energyRate + sink; // air w [m/s] } @@ -128,46 +188,152 @@ void soaringUpdate(float dT) const float netVario = computeNetVarioMs(dT); netVarioCms = netVario * 100.0f; - varioMean += (netVario - varioMean) * MIN(dT / SOAR_VARIO_MEAN_TAU_S, 1.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: net lift over the trigger, inside the altitude band - if (netVario > triggerMs + // 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); - gradN = gradE = 0.0f; + 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; } - // CENTERING: correlate the vario deviation against the bearing from the - // current centre estimate to the aircraft over one turn (sin/cos = the - // first harmonic, pointing at the strongest climb), then slide the centre - // that way AND with the wind (the thermal is locked to the air mass - - // wind * dt is the exact drift, no climb/strength scaling). - const float dx = getEstimatedActualPosition(X) - thermalCentre.x; // north cm - const float dy = getEstimatedActualPosition(Y) - thermalCentre.y; // east cm - const float bearing = atan2_approx(dy, dx); - const float dv = netVario - varioMean; - const float a = MIN(dT / SOAR_GRAD_TAU_S, 1.0f); - gradN += (dv * cos_approx(bearing) - gradN) * a; - gradE += (dv * sin_approx(bearing) - gradE) * a; - - const float k = (soaringConfig()->centreGainPct / 100.0f) * SOAR_CENTRE_GAIN_SCALE; - thermalCentre.x += (k * gradN + getEstimatedWindSpeed(X)) * dT; // cm - thermalCentre.y += (k * gradE + getEstimatedWindSpeed(Y)) * dT; - // keep the loiter anchored on the moving centre estimate (the POSHOLD + // 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(&thermalCentre); + navForcedPosholdAssert(&anchor); // clamp the estimate to a sane radius around where the climb was found const float driftX = thermalCentre.x - breachAnchor.x; @@ -184,11 +350,15 @@ void soaringUpdate(float dT) 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] = getThrottleIdleValue(); // applied idle throttle [us] + debug[4] = PWM_RANGE_MIN; // applied throttle: motor OFF [us] #endif - // leave the thermal: lift collapsed, or out of the altitude band - if (netVario < exitMs + // 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 @@ -217,13 +387,14 @@ float soaringNetVarioCms(void) int16_t soaringThrottleApply(int16_t throttle) { - // While circling a thermal the glider soars on the lift with the motor - // idled: a throttle-to-idle override, NOT a motor stop - the control + // 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 getThrottleIdleValue(); + return PWM_RANGE_MIN; } return throttle; } From 56e9f2469b7e03542b5b121224e589ae744460af Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 13:26:55 +0200 Subject: [PATCH 107/108] docs+settings: estimator/cap-only/guard settings + regenerated references Co-Authored-By: Claude Fable 5 --- docs/OrientationHold.md | 16 +++++++++-- docs/Settings.md | 60 +++++++++++++++++++++++++++++++++++---- src/main/fc/settings.yaml | 44 +++++++++++++++++++++++----- 3 files changed, 105 insertions(+), 15 deletions(-) diff --git a/docs/OrientationHold.md b/docs/OrientationHold.md index f29a764bab8..6b210adaeca 100644 --- a/docs/OrientationHold.md +++ b/docs/OrientationHold.md @@ -10,9 +10,19 @@ 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: bench-validated against a closed-loop JSBSim simulation -(deterministic lockstep, 50-case gust matrix, replay videos); first -hardware flights are upcoming. Treat everything here as experimental. +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 diff --git a/docs/Settings.md b/docs/Settings.md index fea9f89ce9e..29eb6805521 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4592,6 +4592,26 @@ Rate [deg/s] the hold target returns to the preset after the roll/pitch sticks c --- +### 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. @@ -6002,13 +6022,43 @@ Autogyro tip-over guard (ROTOR GUARD mode): bank angle [deg] beyond which, while --- +### 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, negative = nose down: feeds the disk (inflow -> rotor rpm -> authority) +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 | | --- | --- | --- | -| -5 | -20 | 10 | +| 35 | 10 | 60 | --- @@ -6022,13 +6072,13 @@ Minimum sink rate [cm/s] for the tip-over detection - a banked climb or a flown --- -### rotor_guard_throttle_add +### rotor_guard_throttle_boost -Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins, and an IDLE throttle stick disables the guard entirely (landing intent - the guard never spins the thrust up against a deliberate throttle-zero; pulling to idle releases a running recovery). Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink). +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 | | --- | --- | --- | -| 250 | 0 | 800 | +| 25 | 0 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 616041b5a40..e7a86491304 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4545,17 +4545,35 @@ groups: min: 10 max: 1000 - name: rotor_guard_pitch - description: "Pitch target [deg] during rotor guard recovery, negative = nose down: feeds the disk (inflow -> rotor rpm -> authority)" - default_value: -5 + 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_add - description: "Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins, and an IDLE throttle stick disables the guard entirely (landing intent - the guard never spins the thrust up against a deliberate throttle-zero; pulling to idle releases a running recovery). Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink)." - default_value: 250 - field: throttleAddUs + - 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: 800 + 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 @@ -4604,6 +4622,18 @@ groups: 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 From 43f7fadf3b13200746e42ce3be3ec65bd923f2c8 Mon Sep 17 00:00:00 2001 From: pdani Date: Mon, 27 Jul 2026 20:37:33 +0200 Subject: [PATCH 108/108] fw(nav): drop the forced-poshold RC-alt-adjust guard - measured a bit-identical no-op The guard blocked adjustFixedWingAltitudeFromRCInput during forced poshold, defending against a real structural feedback (the nav loiter writes rcCommand, this function reads rcCommand[PITCH] as a pilot climb command). A/B on the floor-orbit flight was bit-identical with and without it: the observed orbit climb was the missing-pitot centrifugal compensation, fixed separately. Unproven defensive code is not worth its upstream diff; the feedback class stays documented here. Co-Authored-By: Claude Fable 5 --- src/main/navigation/navigation_fixedwing.c | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4fe7605f887..90b3a9ebd0b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -111,20 +111,6 @@ void resetFixedWingAltitudeController(void) bool adjustFixedWingAltitudeFromRCInput(void) { -#if defined(USE_FW_AEROBATICS) || defined(USE_SOARING) - // Forced poshold (floor orbit / thermal loiter): NO RC altitude adjust. - // The nav loiter WRITES rcCommand to fly the circle, and this adjust - // reads rcCommand[PITCH] - the loiter's own pitch output fed back as a - // "pilot climb command" is a positive feedback loop that rode the - // floor orbit away from its anchor with the sticks untouched - // (measured: smooth pitch 8->23 deg, ~+4 m/s, 60 m past the anchor - // Z while nav actual == estimator Z). The anchor Z is the contract; - // pilot pitch is a TAKEOVER there, not a climb knob - same raw-stick - // lesson as the floor's release detection. - if (posControl.flags.forcedPosholdActive) { - return false; - } -#endif int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500); if (rcAdjustment) {