Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00
Code Issues Wiki Activity

address code review

Browse source
This commit is contained in:
Thorsten Laux 2019-08-02 14:20:37 +02:00
parent 293dc42710
commit d033a8dc76
4 changed files with 30 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/main/flight/mixer.c
View file

@ -37,6 +37,7 @@
#include "pg/motor.h"
#include "pg/rx.h"
#include "drivers/dshot.h"
#include "drivers/motor.h"
#include "drivers/time.h"
#include "drivers/io.h"
@ -288,6 +289,13 @@ FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow;
static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
static FAST_RAM_ZERO_INIT float rcCommandThrottleRange;
#ifdef USE_DYN_IDLE
static FAST_RAM_ZERO_INIT float idleMaxIncrease;
static FAST_RAM_ZERO_INIT float idleThrottleOffset;
static FAST_RAM_ZERO_INIT float idleMinMotorRps;
static FAST_RAM_ZERO_INIT float idleP;
#endif
uint8_t getMotorCount(void)
{
@ -348,6 +356,12 @@ void mixerInit(mixerMode_e mixerMode)
mixerTricopterInit();
}
#endif
#ifdef USE_DYN_IDLE
idleMinMotorRps = currentPidProfile->idle_min_rpm * 0.01f * 60.0f;
idleMaxIncrease = currentPidProfile->idle_max_increase * 0.001f;
idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f;
idleP = currentPidProfile->idle_p * 0.0001f;
#endif
}
#ifdef USE_LAUNCH_CONTROL
@ -471,6 +485,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions
static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode
static float motorRangeMinIncrease = 0;
static float oldMinRps;
float currentThrottleInputRange = 0;
if (featureIsEnabled(FEATURE_3D)) {
@ -575,26 +590,26 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
pidResetIterm();
}
} else {
throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
#ifdef USE_DYN_IDLE
if (currentPidProfile->idle_hz) {
static float oldMinRpm;
const float maxIncrease = isAirmodeActivated() ? currentPidProfile->idle_max_increase * 0.001f : 0.04f;
const float minRpm = rpmMinMotorFrequency();
const float targetRpmChangeRate = (currentPidProfile->idle_hz - minRpm) * currentPidProfile->idle_adjustment_speed;
const float error = targetRpmChangeRate - (minRpm - oldMinRpm) * pidGetPidFrequency();
const float pidSum = constrainf(currentPidProfile->idle_p * 0.0001f * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit);
if (idleMinMotorRps > 0.0f) {
motorOutputLow = DSHOT_MIN_THROTTLE;
const float maxIncrease = isAirmodeActivated() ? idleMaxIncrease : 0.04f;
const float minRps = rpmMinMotorFrequency();
const float targetRpsChangeRate = (idleMinMotorRps - minRps) * currentPidProfile->idle_adjustment_speed;
const float error = targetRpsChangeRate - (minRps - oldMinRps) * pidGetPidFrequency();
const float pidSum = constrainf(idleP * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit);
motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * pidGetDT(), 0.0f, maxIncrease);
oldMinRpm = minRpm;
oldMinRps = minRps;
throttle += idleThrottleOffset;
DEBUG_SET(DEBUG_DYN_IDLE, 0, motorRangeMinIncrease * 1000);
DEBUG_SET(DEBUG_DYN_IDLE, 1, targetRpmChangeRate);
DEBUG_SET(DEBUG_DYN_IDLE, 1, targetRpsChangeRate);
DEBUG_SET(DEBUG_DYN_IDLE, 2, error);
DEBUG_SET(DEBUG_DYN_IDLE, 3, minRpm);
DEBUG_SET(DEBUG_DYN_IDLE, 3, minRps);
}
#endif
throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
currentThrottleInputRange = rcCommandThrottleRange;
motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow);
motorRangeMax = motorOutputHigh;
@ -854,12 +869,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
throttle = pidCompensateThrustLinearization(throttle);
#endif
#ifdef USE_DYN_IDLE
if (currentPidProfile->idle_hz) {
throttle += currentPidProfile->idle_throttle * 0.001f;
}
#endif
#if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) {
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);