diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e5416bf176..c0d61dcc2e 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1114,7 +1114,8 @@ const clivalue_t valueTable[] = { { "idle_pid_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_pid_limit) }, { "idle_max_increase", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_max_increase) }, #endif - + { "level_race_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, level_race_mode) }, + // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0579cad919..729a99588d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -130,7 +130,7 @@ static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit; #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 13); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 14); void resetPidProfile(pidProfile_t *pidProfile) { @@ -218,6 +218,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .ff_smooth_factor = 37, .ff_boost = 15, .dyn_lpf_curve_expo = 0, + .level_race_mode = false, ); #ifndef USE_D_MIN pidProfile->pid[PID_ROLL].D = 30; @@ -553,6 +554,8 @@ static FAST_RAM_ZERO_INIT bool useIntegratedYaw; static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax; #endif +static FAST_RAM_ZERO_INIT bool levelRaceMode; + void pidResetIterm(void) { for (int axis = 0; axis < 3; axis++) { @@ -751,6 +754,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f; interpolatedSpInit(pidProfile); #endif + + levelRaceMode = pidProfile->level_race_mode; } void pidInit(const pidProfile_t *pidProfile) @@ -1294,6 +1299,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim #if defined(USE_ACC) const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE); const bool levelModeActive = FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive; + const bool levelRaceModeActive = levelRaceMode && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && !gpsRescueIsActive; // Keep track of when we entered a self-level mode so that we can // add a guard time before crash recovery can activate. @@ -1356,8 +1362,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); } // Yaw control is GYRO based, direct sticks control is applied to rate PID + // When Race Mode is active PITCH control is also GYRO based in level or horizon mode #if defined(USE_ACC) - if (levelModeActive && (axis != FD_YAW)) { + if (levelModeActive && (axis != FD_YAW) && !(levelRaceModeActive && (axis == FD_PITCH))) { currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); } #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index afe45b31ce..1e25b7a0ec 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -189,6 +189,7 @@ typedef struct pidProfile_s { uint8_t ff_spike_limit; // FF stick extrapolation lookahead period in ms uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter + uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 9fde0016c2..3a007d0552 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -143,6 +143,7 @@ void setDefaultTestSettings(void) { pidProfile->abs_control_gain = 0, pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL, pidProfile->launchControlGain = 40, + pidProfile->level_race_mode = false, gyro.targetLooptime = 8000; }