Mixer smoothing (#398)

* feat(mixer_smoothing): smoothing out the motor output calculation removing the harsh motorMix constraining. Also introduced thrust linearization and 'AirMode 2.0'

* feat: QuickFlash's predictive AirMode, porting from another impl (#3)

* min/max fix, thanks to borisbstyle

* predictiveAirMode activation logic refactor

* applyAirMode become applyMixerClipAdjustment

* refactoring and simplification

* rolling back some useless changes

* cleared out thrust linearization formula and fixed the linear throttle (there was a division by 100 too much)

* removed unnecessary sign management

* removed unnecessary change

* fix

* fixes, removed throttle linearization for the moment

* moved PID scaling

* removed DEBUG_WRONG_PIDSUM_SIGN

* thrust linearization formula changed (actual matematical inverse) and reintroduced thruttle linearization

* removed duplicated code

* mixer_impl

* mixing yaw separately

* fixes and temporarily put mixerImpl on OSD

* mix & roll/pitch mix rate

* norm fix

* wip

* fixes

* final, maybe...

* cleanup

* unlinear throttle fix

* removing avg controller's caused thrust/motor

* backup

* two pass mixer. Version 1.0.0

* two level thrust linearization

* code cleaning

* TL from idle level and TPA disabled when TL is enabled

* fix SPA and motorOutputIdleLevel

* changed desmos link for TL graphs

* fix: applying AirMode level (so AirMode OFF) given throttleMotor, so that with boht linear_throttle ON and OFF the transition is the same

* removed mixer impl from OSD and another code cleaning

* mixerInitProfile called into pidInitConfig

* rewording

* fix: fixed thrust-linearization disabling. Code cleaning

* 65 default value for linear_thrust_low_output

* little code simplification

* 3d mode fix and cleanup

Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com>
Co-authored-by: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>

src/main/blackbox/blackbox.c

@@ -1204,6 +1204,11 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d",                currentPidProfile->pidSumLimitYaw);
         BLACKBOX_PRINT_HEADER_LINE("iterm_rotation", "%d",                  currentPidProfile->iterm_rotation);
         BLACKBOX_PRINT_HEADER_LINE("throttle_boost", "%d",                  currentPidProfile->throttle_boost);
+        BLACKBOX_PRINT_HEADER_LINE("linear_thrust_low_output",  "%d",       currentPidProfile->linear_thrust_low_output);
+        BLACKBOX_PRINT_HEADER_LINE("linear_thrust_high_output", "%d",       currentPidProfile->linear_thrust_high_output);
+        BLACKBOX_PRINT_HEADER_LINE("linear_throttle", "%d",                 currentPidProfile->linear_throttle);
+        BLACKBOX_PRINT_HEADER_LINE("mixer_impl", "%d",                      currentPidProfile->mixer_impl);
+        BLACKBOX_PRINT_HEADER_LINE("mixer_laziness", "%d",                  currentPidProfile->mixer_laziness);
         // End of EmuFlight controller parameters
         BLACKBOX_PRINT_HEADER_LINE("deadband", "%d",                        rcControlsConfig()->deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d",                    rcControlsConfig()->yaw_deadband);

src/main/cms/cms_menu_imu.c

@@ -76,7 +76,11 @@ static uint16_t dtermBoost;
 static uint8_t dtermBoostLimit;
 static uint8_t tempPid[3][3];
 static uint8_t tempPidWc[3];
-
+static uint8_t linear_thrust_low_output;
+static uint8_t linear_thrust_high_output;
+static uint8_t linear_throttle;
+static mixerImplType_e mixer_impl;
+static uint8_t mixer_laziness;
 
 static uint8_t tmpRateProfileIndex;
 static uint8_t rateProfileIndex;
@@ -91,6 +95,10 @@ static const char * const cms_throttleVbatCompTypeLabels[] = {
     "OFF", "BOOST", "LIMIT", "BOTH"
 };
 
+static const char * const cms_mixerImplTypeLabels[] = {
+    "LEGACY", "SMOOTH", "2PASS"
+};
+
 static long cmsx_menuImu_onEnter(void) {
     pidProfileIndex = getCurrentPidProfileIndex();
     tmpPidProfileIndex = pidProfileIndex + 1;
@@ -138,6 +146,11 @@ static long cmsx_PidAdvancedRead(void) {
     itermRelaxThreshold = pidProfile->iterm_relax_threshold;
     itermRelaxThresholdYaw = pidProfile->iterm_relax_threshold_yaw;
     itermWindup = pidProfile->itermWindupPointPercent;
+    linear_thrust_low_output = pidProfile->linear_thrust_low_output;
+    linear_thrust_high_output = pidProfile->linear_thrust_high_output;
+    linear_throttle = pidProfile->linear_throttle;
+    mixer_impl = pidProfile->mixer_impl;
+    mixer_laziness = pidProfile->mixer_laziness;
     return 0;
 }
 
@@ -164,6 +177,11 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self) {
     pidProfile->iterm_relax_threshold = itermRelaxThreshold;
     pidProfile->iterm_relax_threshold_yaw = itermRelaxThresholdYaw;
     pidProfile->itermWindupPointPercent = itermWindup;
+    pidProfile->linear_thrust_low_output = linear_thrust_low_output;
+    pidProfile->linear_thrust_high_output = linear_thrust_high_output;
+    pidProfile->linear_throttle = linear_throttle;
+    pidProfile->mixer_impl = mixer_impl;
+    pidProfile->mixer_laziness = mixer_laziness;
     pidInitConfig(currentPidProfile);
     return 0;
 }
@@ -189,6 +207,12 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = {
     { "I RELAX THRESH YAW", OME_UINT8, NULL, &(OSD_UINT8_t){ &itermRelaxThresholdYaw,   0, 100, 1 }, 0 },
     { "I WINDUP",          OME_UINT8, NULL, &(OSD_UINT8_t){ &itermWindup,              0, 100, 1 }, 0 },
 
+    { "LINEAR THRUST LOW",  OME_UINT8, NULL, &(OSD_UINT8_t) { &linear_thrust_low_output, 0, 100,  1}, 0 },
+    { "LINEAR THRUST HIGH", OME_UINT8, NULL, &(OSD_UINT8_t) { &linear_thrust_high_output, 0, 100,  1}, 0 },
+    { "LINEAR THROTTLE",   OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &linear_throttle, 1, cms_offOnLabels }, 0 },
+    { "MIXER IMPL",        OME_TAB,   NULL, &(OSD_TAB_t)   { &mixer_impl, MIXER_IMPL_COUNT - 1, cms_mixerImplTypeLabels }, 0 },
+    { "MIXER LAZINESS",    OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &mixer_laziness, 1, cms_offOnLabels }, 0 },
+
     { "SAVE&EXIT",         OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT_SAVE, 0},
     { "BACK",              OME_Back, NULL, NULL, 0 },
     { NULL,                OME_END, NULL, NULL, 0 }
@@ -316,8 +340,6 @@ static OSD_Entry cmsx_menuRateProfileEntries[] = {
 
     { "VBAT COMP TYPE",  OME_TAB,   NULL, &(OSD_TAB_t)    { &rateProfile.vbat_comp_type, VBAT_COMP_TYPE_COUNT - 1, cms_throttleVbatCompTypeLabels}, 0 },
     { "VBAT COMP REF",   OME_UINT8, NULL, &(OSD_UINT8_t)  { &rateProfile.vbat_comp_ref, VBAT_CELL_VOTAGE_RANGE_MIN, VBAT_CELL_VOTAGE_RANGE_MAX,  1}, 0 },
-    { "VBAT COMP THR %", OME_UINT8, NULL, &(OSD_UINT8_t)  { &rateProfile.vbat_comp_throttle_level, 0,  100,  1}, 0 },
-    { "VBAT COMP PID %", OME_UINT8, NULL, &(OSD_UINT8_t)  { &rateProfile.vbat_comp_pid_level, 0,  100,  1}, 0 },
     { "THROTTLE LIMIT %", OME_UINT8, NULL, &(OSD_UINT8_t)  { &rateProfile.throttle_limit_percent, 25,  100,  1}, 0 },
 
     { "SAVE&EXIT",   OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT_SAVE, 0},

src/main/common/maths.h

@@ -58,6 +58,7 @@
 #define ABS(x) \
   __extension__ ({ __typeof__ (x) _x = (x); \
   _x > 0 ? _x : -_x; })
+#define SCALE_UNITARY_RANGE(x, from, to) ((1.0f - (x)) * (from) + (x) * (to))
 
 #define Q12 (1 << 12)
 

src/main/fc/config.c

@@ -576,6 +576,7 @@ void changePidProfile(uint8_t pidProfileIndex) {
         loadPidProfile();
         pidInit(currentPidProfile);
         initEscEndpoints();
+        mixerInitProfile();
     }
     beeperConfirmationBeeps(pidProfileIndex + 1);
 }

src/main/fc/controlrate_profile.c

@@ -64,8 +64,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) {
         .throttle_limit_percent = 100,
         .vbat_comp_type = VBAT_COMP_TYPE_OFF,
         .vbat_comp_ref = 37,
-        .vbat_comp_throttle_level = 75,
-        .vbat_comp_pid_level = 75,
         .addRollToYawRc = 0,
         .addYawToRollRc = 0,
         .rollPitchMagExpo = 0,

src/main/fc/controlrate_profile.h

@@ -75,8 +75,6 @@ typedef struct controlRateConfig_s {
     uint8_t throttle_limit_percent;         // Sets the maximum pilot commanded throttle limit
     uint8_t vbat_comp_type;                 // Sets the type of battery compensation: off, boost, limit or both
     uint8_t vbat_comp_ref;                  // Sets the voltage reference to calculate the battery compensation
-    uint8_t vbat_comp_throttle_level;       // Sets the level of throttle battery compensation
-    uint8_t vbat_comp_pid_level;            // Sets the level of PID battery compensation
 } controlRateConfig_t;
 
 PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);