diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 07aa97b34e..0e17df1830 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -976,6 +976,8 @@ const clivalue_t valueTable[] = { { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, { "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) }, { "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) }, + { "roll_level_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_ROLL]) }, + { "pitch_level_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_PITCH]) }, // PG_SERIAL_CONFIG { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index d798ebdb8b..3cea3b791b 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -392,6 +392,9 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] = { "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType}, 0 }, { "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1}, 0 }, + { "ROLL LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_ROLL], 0, 100, 1, 10 }, 0 }, + { "PITCH LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_PITCH], 0, 100, 1, 10 }, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index f9d96bf341..7c824ea2e0 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -37,7 +37,7 @@ controlRateConfig_t *currentControlRateProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4); void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) { @@ -65,6 +65,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .tpaMode = TPA_MODE_D, .profileName = { 0 }, .quickRatesRcExpo = 0, + .levelExpo[FD_ROLL] = 0, + .levelExpo[FD_PITCH] = 0, ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 91336287fd..107e162662 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -68,6 +68,7 @@ typedef struct controlRateConfig_s { uint8_t tpaMode; // Controls which PID terms TPA effects char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates + uint8_t levelExpo[2]; // roll/pitch level mode expo } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e42c9364da..340fa888c1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -333,11 +333,23 @@ float pidApplyThrustLinearization(float motorOutput) #endif #if defined(USE_ACC) +// calculate the stick deflection while applying level mode expo +static float getLevelModeRcDeflection(uint8_t axis) +{ + const float stickDeflection = getRcDeflection(axis); + if (axis < FD_YAW) { + const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f; + return power3(stickDeflection) * expof + stickDeflection * (1 - expof); + } else { + return stickDeflection; + } +} + // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling STATIC_UNIT_TESTED float calcHorizonLevelStrength(void) { // start with 1.0 at center stick, 0.0 at max stick deflection: - float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); + float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH))); // 0 at level, 90 at vertical, 180 at inverted (degrees): const float currentInclination = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)) / 10.0f; @@ -395,7 +407,7 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void) STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination // rcDeflection is in range [-1.0, 1.0] - float angle = pidProfile->levelAngleLimit * getRcDeflection(axis); + float angle = pidProfile->levelAngleLimit * getLevelModeRcDeflection(axis); #ifdef USE_GPS_RESCUE angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES #endif diff --git a/src/test/Makefile b/src/test/Makefile index 63716fb128..dff77dab49 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -376,6 +376,7 @@ pid_unittest_SRC := \ $(USER_DIR)/common/filter.c \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/drivers/accgyro/gyro_sync.c \ + $(USER_DIR)/fc/controlrate_profile.c \ $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/flight/pid.c \ $(USER_DIR)/flight/pid_init.c \ diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 0de9872d3c..788f2cafd0 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -34,18 +34,21 @@ int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t debugMode; extern "C" { + #include "platform.h" + #include "build/debug.h" + #include "common/axis.h" #include "common/maths.h" #include "common/filter.h" + #include "config/config.h" #include "config/config_reset.h" - #include "pg/pg.h" - #include "pg/pg_ids.h" #include "drivers/sound_beeper.h" #include "drivers/time.h" + #include "fc/controlrate_profile.h" #include "fc/core.h" #include "fc/rc.h" @@ -59,6 +62,9 @@ extern "C" { #include "io/gps.h" + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "sensors/gyro.h" #include "sensors/acceleration.h" @@ -66,6 +72,7 @@ extern "C" { attitudeEulerAngles_t attitude; PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); bool unitLaunchControlActive = false; launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL; @@ -87,6 +94,7 @@ extern "C" { UNUSED(currentPidSetpoint); return value; } + void initRcProcessing(void) { } } pidProfile_t *pidProfile; @@ -179,6 +187,10 @@ void resetTest(void) { unitLaunchControlActive = false; pidProfile->launchControlMode = unitLaunchControlMode; pidInit(pidProfile); + loadControlRateProfile(); + + currentControlRateProfile->levelExpo[FD_ROLL] = 0; + currentControlRateProfile->levelExpo[FD_PITCH] = 0; // Run pidloop for a while after reset for (int loop = 0; loop < 20; loop++) { @@ -379,6 +391,19 @@ TEST(pidControllerTest, testPidLevel) { EXPECT_FLOAT_EQ(0, currentPidSetpoint); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); EXPECT_FLOAT_EQ(0, currentPidSetpoint); + + // Test level mode expo + enableFlightMode(ANGLE_MODE); + attitude.values.roll = 0; + attitude.values.pitch = 0; + setStickPosition(FD_ROLL, 0.5f); + setStickPosition(FD_PITCH, -0.5f); + currentControlRateProfile->levelExpo[FD_ROLL] = 50; + currentControlRateProfile->levelExpo[FD_PITCH] = 26; + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(85.9375, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(-110.6875, currentPidSetpoint); }