diff --git a/src/main/config/config.c b/src/main/config/config.c index 938613caa..ad55a4097 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -159,6 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D_f[YAW] = 0.05f; pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; + pidProfile->H_sensitivity = 10.0f; } #ifdef GPS diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 22ee35fc9..b55ef9cd8 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -101,9 +101,28 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control float delta, deltaSum; float dT; int axis; + float horizonLevelStrength = 1; dT = (float)cycleTime * 0.000001f; + if (FLIGHT_MODE(HORIZON_MODE)) { + + if(abs(rcCommand[FD_ROLL]) > abs(rcCommand[FD_PITCH])){ + axis = FD_ROLL; + } + else { + axis = FD_PITCH; + } + + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - abs(rcCommand[axis])) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->H_sensitivity == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (10 / pidProfile->H_sensitivity)) + 1, 0, 1); + } + } + // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode @@ -134,7 +153,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->H_level; + AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; } } } diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index a6fadf55f..6461ce21e 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -42,6 +42,7 @@ typedef struct pidProfile_s { float D_f[3]; float A_level; float H_level; + float H_sensitivity; } pidProfile_t; typedef enum { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2628ba93a..b1a315630 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -392,6 +392,7 @@ const clivalue_t valueTable[] = { { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 }, { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 }, + { "sensitivity_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b43409bf5..5c2d834ee 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -846,7 +846,7 @@ static bool processOutCommand(uint8_t cmdMSP) if (i == PIDLEVEL) { serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); - serialize8(0); + serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity * 10.0f), 0, 250)); } else { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); @@ -1165,7 +1165,7 @@ static bool processInCommand(void) if (i == PIDLEVEL) { currentProfile->pidProfile.A_level = (float)read8() / 10.0f; currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - read8(); + currentProfile->pidProfile.H_sensitivity = (float)read8() / 10.0f; } else { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8();