Convert mixer to float math

src/main/drivers/pwm_output.h

@@ -28,7 +28,8 @@ typedef enum {
     PWM_TYPE_MULTISHOT,
     PWM_TYPE_BRUSHED,
     PWM_TYPE_DSHOT600,
-    PWM_TYPE_DSHOT150
+    PWM_TYPE_DSHOT150,
+    PWM_TYPE_MAX
 } motorPwmProtocolTypes_e;
 
 #define PWM_TIMER_MHZ         1

src/main/drivers/pwm_output_stm32f3xx.c

@@ -58,7 +58,6 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 {
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
-    value = (value <= 1000) ? 0 : ((value - 1000) * 2);
     motor->value = value;
 
     motor->dmaBuffer[0]  = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0;

src/main/drivers/pwm_output_stm32f4xx.c

@@ -58,7 +58,6 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 {
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
-    value = (value <= 1000) ? 0 : ((value - 1000) * 2);
     motor->value = value;
 
     motor->dmaBuffer[0]  = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0;

src/main/fc/config.c

@@ -259,6 +259,7 @@ void resetMotorConfig(motorConfig_t *motorConfig)
 #endif
     motorConfig->maxthrottle = 2000;
     motorConfig->mincommand = 1000;
+    motorConfig->digitalIdleOffset = 0;
 
     uint8_t motorIndex = 0;
     for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) {

src/main/fc/fc_msp.c

@@ -48,6 +48,7 @@
 #include "drivers/buf_writer.h"
 #include "drivers/max7456.h"
 #include "drivers/vtx_soft_spi_rtc6705.h"
+#include "drivers/pwm_output.h"
 
 #include "fc/config.h"
 #include "fc/mw.h"
@@ -1866,7 +1867,11 @@ static bool processInCommand(uint8_t cmdMSP)
         masterConfig.gyro_sync_denom = read8();
         masterConfig.pid_process_denom = read8();
         masterConfig.motorConfig.useUnsyncedPwm = read8();
-        masterConfig.motorConfig.motorPwmProtocol = read8();
+#ifdef USE_DSHOT
+        masterConfig.motorConfig.motorPwmProtocol = constrain(read8(), 0, PWM_TYPE_MAX - 1);
+#else
+        masterConfig.motorConfig.motorPwmProtocol = constrain(read8(), 0, PWM_TYPE_BRUSHED);
+#endif
         masterConfig.motorConfig.motorPwmRate = read16();
         break;
     case MSP_SET_FILTER_CONFIG :

src/main/flight/mixer.c

@@ -333,6 +333,28 @@ static servoMixer_t *customServoMixers;
 
 static motorMixer_t *customMixers;
 
+static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
+static float rcCommandThrottleRange;
+
+// Add here scaled ESC outputs for digital protol
+void initEscEndpoints(void) {
+    if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) {
+        disarmMotorOutput = DSHOT_DISARM_COMMAND;
+        minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset;
+        maxMotorOutputNormal = DSHOT_MAX_THROTTLE;
+        deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH;
+        deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
+    } else {
+        disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
+        minMotorOutputNormal = motorConfig->minthrottle;
+        maxMotorOutputNormal = motorConfig->maxthrottle;
+        deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
+        deadbandMotor3dLow = flight3DConfig->deadband3d_low;
+    }
+
+    rcCommandThrottleRange = (2000 - rxConfig->mincheck);
+}
+
 void mixerUseConfigs(
         flight3DConfig_t *flight3DConfigToUse,
         motorConfig_t *motorConfigToUse,
@@ -345,6 +367,7 @@ void mixerUseConfigs(
     mixerConfig = mixerConfigToUse;
     airplaneConfig = airplaneConfigToUse;
     rxConfig = rxConfigToUse;
+    initEscEndpoints();
 }
 
 #ifdef USE_SERVOS
@@ -670,9 +693,9 @@ STATIC_UNIT_TESTED void servoMixer(void)
         input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
     } else {
         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
-        input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
+        input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL];
-        input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
+        input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH];
-        input[INPUT_STABILIZED_YAW] = axisPID[YAW];
+        input[INPUT_STABILIZED_YAW] = axisPIDf[YAW];
 
         // Reverse yaw servo when inverted in 3D mode
         if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
@@ -739,108 +762,99 @@ STATIC_UNIT_TESTED void servoMixer(void)
 void mixTable(void *pidProfilePtr)
 {
     uint32_t i = 0;
-    fix12_t vbatCompensationFactor = 0;
+    float vbatCompensationFactor = 1;
-    static fix12_t mixReduction;
-    bool use_vbat_compensation = false;
     pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
-    if (batteryConfig && pidProfile->vbatPidCompensation) {
-        use_vbat_compensation = true;
-        vbatCompensationFactor = calculateVbatPidCompensation();
-    }
-
-    bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
-
-    // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
-    int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
-    int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
-    int16_t rollPitchYawMixMin = 0;
-
-    if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]);  // Add voltage compensation
-
-    // Find roll/pitch/yaw desired output
-    for (i = 0; i < motorCount; i++) {
-        rollPitchYawMix[i] =
-            axisPID[PITCH] * currentMixer[i].pitch +
-            axisPID[ROLL] * currentMixer[i].roll +
-            -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
-
-        if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]);  // Add voltage compensation
-
-        if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
-        if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
-
-        if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i];
-    }
 
     // Scale roll/pitch/yaw uniformly to fit within throttle range
-    int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
+    // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
-    int16_t throttleRange, throttle;
+    float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0;
-    int16_t throttleMin, throttleMax;
+    float throttleRange = 0, throttle = 0;
-    static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
+    float motorOutputRange, motorMixRange;
+    uint16_t motorOutputMin, motorOutputMax;
+    static uint16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 
     // Find min and max throttle based on condition.
     if (feature(FEATURE_3D)) {
         if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 
         if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
-            throttleMax = flight3DConfig->deadband3d_low;
+            motorOutputMax = deadbandMotor3dLow;
-            throttleMin = motorConfig->minthrottle;
+            motorOutputMin = minMotorOutputNormal;
             throttlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle;
         } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
-            throttleMax = motorConfig->maxthrottle;
+            motorOutputMax = maxMotorOutputNormal;
-            throttleMin = flight3DConfig->deadband3d_high;
+            motorOutputMin = deadbandMotor3dHigh;
             throttlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle;
         } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)))  { // Deadband handling from negative to positive
-            throttle = throttleMax = flight3DConfig->deadband3d_low;
+            throttle = deadbandMotor3dLow;
-            throttleMin = motorConfig->minthrottle;
+            motorOutputMin = motorOutputMax = minMotorOutputNormal;
         } else {  // Deadband handling from positive to negative
-            throttleMax = motorConfig->maxthrottle;
+            motorOutputMax = maxMotorOutputNormal;
-            throttle = throttleMin = flight3DConfig->deadband3d_high;
+            throttle = motorOutputMin = deadbandMotor3dHigh;
         }
     } else {
         throttle = rcCommand[THROTTLE];
-        throttleMin = motorConfig->minthrottle;
+        motorOutputMin = minMotorOutputNormal;
-        throttleMax = motorConfig->maxthrottle;
+        motorOutputMax = maxMotorOutputNormal;
     }
 
-    throttleRange = throttleMax - throttleMin;
+    motorOutputRange = motorOutputMax - motorOutputMin;
+    throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f);
+    throttleRange = motorOutputMax - motorOutputMin;
 
-    if (rollPitchYawMixRange > throttleRange) {
+    // Limit the PIDsum
-        mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
+    for (int axis = 0; axis < 3; axis++)
+        axisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
 
+    // Calculate voltage compensation
+    if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation();
+
+    // Find roll/pitch/yaw desired output
     for (i = 0; i < motorCount; i++) {
-            rollPitchYawMix[i] =  qMultiply(mixReduction,rollPitchYawMix[i]);
+        motorMix[i] =
+            axisPIDf[PITCH] * currentMixer[i].pitch +
+            axisPIDf[ROLL] * currentMixer[i].roll +
+            -mixerConfig->yaw_motor_direction * axisPIDf[YAW] * currentMixer[i].yaw;
+
+        if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor;  // Add voltage compensation
+
+        if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i];
+        if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i];
+    }
+
+    motorMixRange = motorMixMax - motorMixMin;
+
+    if (motorMixRange > 1.0f) {
+        for (i = 0; i < motorCount; i++) {
+            motorMix[i] /= motorMixRange;
         }
         // Get the maximum correction by setting offset to center
-        throttleMin = throttleMax = throttleMin + (throttleRange / 2);
+        throttle = 0.5f;
     } else {
-        throttleMin = throttleMin + (rollPitchYawMixRange / 2);
+        float throttleLimitOffset = motorMixRange / 2.0f;
-        throttleMax = throttleMax - (rollPitchYawMixRange / 2);
+        throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
     }
 
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     for (i = 0; i < motorCount; i++) {
-        motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
+        motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) );
 
-        if (isFailsafeActive) {
+        if (failsafeIsActive()) {
-            motor[i] = constrain(motor[i], motorConfig->mincommand, motorConfig->maxthrottle);
+            if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600)
-        } else if (feature(FEATURE_3D)) {
+                motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range
-            if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
+
-                motor[i] = constrain(motor[i], motorConfig->minthrottle, flight3DConfig->deadband3d_low);
+            motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax);
         } else {
-                motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, motorConfig->maxthrottle);
+            motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax);
-            }
-        } else {
-            motor[i] = constrain(motor[i], motorConfig->minthrottle, motorConfig->maxthrottle);
         }
 
         // Motor stop handling
         if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
             if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
-                motor[i] = motorConfig->mincommand;
+                motor[i] = disarmMotorOutput;
             }
         }
     }
@@ -853,7 +867,7 @@ void mixTable(void *pidProfilePtr)
         if (maxThrottleStep < throttleRange) {
             static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
 
-            motor[i] = constrain(motor[i], motorConfig->minthrottle, motorPrevious[i] + maxThrottleStep);  // Only limit accelerating situation
+            motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep);  // Only limit accelerating situation
             motorPrevious[i] = motor[i];
         }
     }
@@ -861,9 +875,16 @@ void mixTable(void *pidProfilePtr)
     // Disarmed mode
     if (!ARMING_FLAG(ARMED)) {
         for (i = 0; i < motorCount; i++) {
+            if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) {
+                motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range
+
+                if (motor_disarmed[i] != disarmMotorOutput)
+                    motor[i] = (motor_disarmed[i] - 1000) * 2; // TODO - Perhaps needs rescaling as it will never reach 2047 during motor tests
+            } else {
                 motor[i] = motor_disarmed[i];
             }
         }
+    }
 
     // motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
 

src/main/flight/mixer.h

@@ -22,6 +22,13 @@
 
 #define QUAD_MOTOR_COUNT 4
 
+// Digital protocol has fixed values
+#define DSHOT_DISARM_COMMAND      0
+#define DSHOT_MIN_THROTTLE       48
+#define DSHOT_MAX_THROTTLE     2047
+#define DSHOT_3D_DEADBAND_LOW   900  // TODO - not agreed yet
+#define DSHOT_3D_DEADBAND_HIGH 1100  // TODO - not agreed yet
+
 // Note: this is called MultiType/MULTITYPE_* in baseflight.
 typedef enum mixerMode
 {

src/main/flight/pid.c

@@ -54,7 +54,7 @@ uint32_t targetPidLooptime;
 bool pidStabilisationEnabled;
 uint8_t PIDweight[3];
 
-int16_t axisPID[3];
+float axisPIDf[3];
 
 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
 uint8_t PIDweight[3];
@@ -290,17 +290,17 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             DTerm = Kd[axis] * delta * tpaFactor;
 
             // -----calculate total PID output
-            axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
+            axisPIDf[axis] = PTerm + ITerm + DTerm;
         } else {
             if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
 
-            axisPID[axis] = lrintf(PTerm + ITerm);
+            axisPIDf[axis] = PTerm + ITerm;
 
             DTerm = 0.0f; // needed for blackbox
         }
 
         // Disable PID control at zero throttle
-        if (!pidStabilisationEnabled) axisPID[axis] = 0;
+        if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
 
 #ifdef GTUNE
         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {

src/main/flight/pid.h

@@ -20,9 +20,10 @@
 #include <stdbool.h>
 
 #define PID_CONTROLLER_BETAFLIGHT 1
+#define PID_MIXER_SCALING 1000.0f
 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
-#define PIDSUM_LIMIT 700
+#define PIDSUM_LIMIT 0.5f
 
 // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
 #define PTERM_SCALE 0.032029f
@@ -67,7 +68,7 @@ typedef struct pidProfile_s {
     uint16_t rollPitchItermIgnoreRate;      // Experimental threshold for resetting iterm for pitch and roll on certain rates
     uint16_t yawItermIgnoreRate;            // Experimental threshold for resetting iterm for yaw on certain rates
     uint16_t yaw_p_limit;
-    uint16_t pidSumLimit;
+    float pidSumLimit;
     uint8_t dterm_average_count;            // Configurable delta count for dterm
     uint8_t vbatPidCompensation;            // Scale PIDsum to battery voltage
     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
@@ -98,7 +99,7 @@ typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t ma
 void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
         const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);
 
-extern int16_t axisPID[3];
+extern float axisPIDf[3];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 bool airmodeWasActivated;
 extern uint32_t targetPidLooptime;

src/main/io/motors.h

@@ -28,5 +28,6 @@ typedef struct motorConfig_s {
     uint16_t motorPwmRate;                  // The update rate of motor outputs (50-498Hz)
     uint8_t  motorPwmProtocol;              // Pwm Protocol
     uint8_t  useUnsyncedPwm;
+    uint8_t  digitalIdleOffset;
     ioTag_t  ioTags[MAX_SUPPORTED_MOTORS];
 } motorConfig_t;

src/main/io/serial_cli.c

@@ -683,6 +683,9 @@ const clivalue_t valueTable[] = {
     { "min_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.motorConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
     { "max_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.motorConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
     { "min_command",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.motorConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
+#ifdef USE_DSHOT
+    { "digital_idle_offset",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.motorConfig.digitalIdleOffset, .config.minmax = { 0,  255} },
+#endif
     { "max_esc_throttle_jump",      VAR_UINT16 | MASTER_VALUE,  &masterConfig.motorConfig.maxEscThrottleJumpMs, .config.minmax = { 0,  1000 } },
 
     { "3d_deadband_low",            VAR_UINT16 | MASTER_VALUE,  &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently
@@ -810,7 +813,7 @@ const clivalue_t valueTable[] = {
 
     { "yaw_motor_direction",        VAR_INT8   | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1,  1 } },
     { "yaw_p_limit",                VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
-    { "pidsum_limit",               VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 100, 1000 } },
+    { "pidsum_limit",               VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
 #ifdef USE_SERVOS
     { "servo_center_pulse",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
     { "tri_unarmed_servo",          VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },

src/main/sensors/battery.c

@@ -214,15 +214,12 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
     mAhDrawn = mAhdrawnRaw / (3600 * 100);
 }
 
-fix12_t calculateVbatPidCompensation(void) {
+float calculateVbatPidCompensation(void) {
-    fix12_t batteryScaler;
+    float batteryScaler =  1.0f;
     if (feature(FEATURE_VBAT) && batteryCellCount > 1) {
-        uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount;
+        // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
-        batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage));
+        batteryScaler =  constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f);
-    } else {
-        batteryScaler = Q12;
     }
-
     return batteryScaler;
 }
 

src/main/sensors/battery.h

@@ -79,6 +79,6 @@ struct rxConfig_s;
 void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
 int32_t currentMeterToCentiamps(uint16_t src);
 
-fix12_t calculateVbatPidCompensation(void);
+float calculateVbatPidCompensation(void);
 uint8_t calculateBatteryPercentage(void);
 uint8_t calculateBatteryCapacityRemainingPercentage(void);