Self-level modes expo

Adds angle setpoint roll/pitch expo for self-level modes.

Previously the angle setpoint was a simple linear calculation based on the stick deflection percentage and the angle limit. This makes control very jumpy around center stick and people would often resort to adding expo in their radios to compensate. This then adds the complication of wanting expo in the radio when in self-level but not when in acro - leading to complicated mixes, virtual switches, etc.

This PR adds separate self-level expo settings for roll/pitch so the user can customize the axis behavior. Yaw is excluded because this axis uses normal rates and expo controlling the rotational rate around the yaw axis and not an angle setpoint. The roll/pitch expo can range from 0 (off) to 100 (max) like other RC expo settings. For example:
```
set roll_level_expo = 30
set pitch_level_expo = 40
```
The default values are 0 which disables any expo and behaves as before.

The settings are available in the CMS rate profile menu.

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) },

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 }
 };

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,
         );
     }
 }

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);

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

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 \

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);
 }