From 012d1958962c7db2ae351b6e35056f8f35b895e4 Mon Sep 17 00:00:00 2001 From: pdani Date: Sun, 5 Jul 2026 08:58:15 +0200 Subject: [PATCH 01/16] 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 02/16] 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 03/16] 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 04/16] 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 05/16] 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 06/16] 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 07/16] 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 08/16] 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 09/16] 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 10/16] 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 11/16] 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 12/16] 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 13/16] 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 14/16] 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 15/16] 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 16/16] 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`.