diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7bc478af79..2c437ea704 100644 --- a/src/main/drivers/pwm_output.h +++ b/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 diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 9fae6b555d..8579088d8e 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/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; @@ -215,4 +214,4 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_ITConfig(channel, DMA_IT_TC, ENABLE); } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 4f0baa5464..f2a3744825 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/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; @@ -215,4 +214,4 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); } -#endif \ No newline at end of file +#endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 100794d5cc..db7ff721f6 100755 --- a/src/main/fc/config.c +++ b/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++) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ce6e19fd64..e5529f7c75 100755 --- a/src/main/fc/fc_msp.c +++ b/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 : diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a8b462e0d1..c31f958a85 100755 --- a/src/main/flight/mixer.c +++ b/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_PITCH] = axisPID[PITCH]; - input[INPUT_STABILIZED_YAW] = axisPID[YAW]; + input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL]; + input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH]; + 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; - static fix12_t mixReduction; - bool use_vbat_compensation = false; + float vbatCompensationFactor = 1; 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; - int16_t throttleRange, throttle; - int16_t throttleMin, throttleMax; - static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions + // Initial mixer concept by bdoiron74 reused and optimized for Air Mode + float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0; + float throttleRange = 0, throttle = 0; + 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; - throttleMin = motorConfig->minthrottle; + motorOutputMax = deadbandMotor3dLow; + 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; - throttleMin = flight3DConfig->deadband3d_high; + motorOutputMax = maxMotorOutputNormal; + 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; - throttleMin = motorConfig->minthrottle; + throttle = deadbandMotor3dLow; + motorOutputMin = motorOutputMax = minMotorOutputNormal; } else { // Deadband handling from positive to negative - throttleMax = motorConfig->maxthrottle; - throttle = throttleMin = flight3DConfig->deadband3d_high; + motorOutputMax = maxMotorOutputNormal; + throttle = motorOutputMin = deadbandMotor3dHigh; } } else { throttle = rcCommand[THROTTLE]; - throttleMin = motorConfig->minthrottle; - throttleMax = motorConfig->maxthrottle; + motorOutputMin = minMotorOutputNormal; + motorOutputMax = maxMotorOutputNormal; } - throttleRange = throttleMax - throttleMin; + motorOutputRange = motorOutputMax - motorOutputMin; + throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f); + throttleRange = motorOutputMax - motorOutputMin; - if (rollPitchYawMixRange > throttleRange) { - mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); + // Limit the PIDsum + 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++) { + 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++) { - rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[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); - throttleMax = throttleMax - (rollPitchYawMixRange / 2); + float throttleLimitOffset = motorMixRange / 2.0f; + 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) { - motor[i] = constrain(motor[i], motorConfig->mincommand, motorConfig->maxthrottle); - } else if (feature(FEATURE_3D)) { - if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { - motor[i] = constrain(motor[i], motorConfig->minthrottle, flight3DConfig->deadband3d_low); - } else { - motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, motorConfig->maxthrottle); - } + if (failsafeIsActive()) { + if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) + motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range + + motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); } else { - motor[i] = constrain(motor[i], motorConfig->minthrottle, motorConfig->maxthrottle); + motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax); } // 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,7 +875,14 @@ void mixTable(void *pidProfilePtr) // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { - motor[i] = motor_disarmed[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]; + } } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4622cfd3f9..57f2ce0fc7 100644 --- a/src/main/flight/mixer.h +++ b/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 { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ecc164ca41..c73e04ddaa 100644 --- a/src/main/flight/pid.c +++ b/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)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 49a8df9563..0719f78b30 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -20,9 +20,10 @@ #include #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; diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 918c478f75..d7fedf85c5 100644 --- a/src/main/io/motors.h +++ b/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; \ No newline at end of file +} motorConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index aac2360c58..d98b5d0793 100755 --- a/src/main/io/serial_cli.c +++ b/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 } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 77a852782b..a836e4a350 100644 --- a/src/main/sensors/battery.c +++ b/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) { - fix12_t batteryScaler; +float calculateVbatPidCompensation(void) { + float batteryScaler = 1.0f; if (feature(FEATURE_VBAT) && batteryCellCount > 1) { - uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount; - batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage)); - } else { - batteryScaler = Q12; + // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference + batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); } - return batteryScaler; } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 72ff54aa2e..81867e0c18 100644 --- a/src/main/sensors/battery.h +++ b/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);