diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 804a539a42..23160d7922 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -591,8 +591,7 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis) } } -static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT) -{ +static float computePidLevelTarget(flight_dynamics_index_t axis) { // This is ROLL/PITCH, run ANGLE/HORIZON controllers float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); @@ -620,6 +619,12 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10); } + return angleTarget; +} + +static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT) +{ + #ifdef USE_SECONDARY_IMU float actual; if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) { @@ -1126,14 +1131,15 @@ void FAST_CODE pidController(float dT) } // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { - const float horizonRateMagnitude = calcHorizonRateMagnitude(); - pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude, dT); - pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude, dT); - canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON - levelingEnabled = true; - } else { - levelingEnabled = false; + const float horizonRateMagnitude = calcHorizonRateMagnitude(); + levelingEnabled = false; + for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) { + float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis)); + pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); + canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON + levelingEnabled = true; + } } if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 4b415785fb..cb013e087d 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -817,3 +817,35 @@ uint32_t getLoiterRadius(uint32_t loiterRadius) { return loiterRadius; #endif } + +float getFlightAxisAngleOverride(uint8_t axis, float angle) { + if (flightAxisOverride[axis].angleTargetActive) { + return flightAxisOverride[axis].angleTarget; + } else { + return angle; + } +} + +float getFlightAxisRateOverride(uint8_t axis, float rate) { + if (flightAxisOverride[axis].rateTargetActive) { + return flightAxisOverride[axis].rateTarget; + } else { + return rate; + } +} + +bool isFlightAxisAngleOverrideActive(uint8_t axis) { + if (flightAxisOverride[axis].angleTargetActive) { + return true; + } else { + return false; + } +} + +bool isFlightAxisRateOverrideActive(uint8_t axis) { + if (flightAxisOverride[axis].rateTargetActive) { + return true; + } else { + return false; + } +} \ No newline at end of file diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 3d33c89580..8eb602b456 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -217,3 +217,7 @@ float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); uint32_t getLoiterRadius(uint32_t loiterRadius); +float getFlightAxisAngleOverride(uint8_t axis, float angle); +float getFlightAxisRateOverride(uint8_t axis, float rate); +bool isFlightAxisAngleOverrideActive(uint8_t axis); +bool isFlightAxisRateOverrideActive(uint8_t axis);