Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

idle

Browse source
This commit is contained in:
Thorsten Laux 2019-07-22 07:31:44 +02:00
parent 383ba1cd8e
commit d474df3149
6 changed files with 60 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -57,6 +57,7 @@
#include "flight/mixer.h"
#include "flight/mixer_tricopter.h"
#include "flight/pid.h"
#include "flight/rpm_filter.h"
#include "rx/rx.h"
@ -464,6 +465,7 @@ static FAST_RAM_ZERO_INIT float motorRangeMax;
static FAST_RAM_ZERO_INIT float motorOutputRange;
static FAST_RAM_ZERO_INIT int8_t motorOutputMixSign;
static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
{
static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions
@ -572,12 +574,23 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
pidResetIterm();
}
} else {
static float motorRangeMinIncrease = 0;
if (currentPidProfile->idle_hz) {
static float oldMinRpm;
const float minRpm = rpmMinMotorSpeed();
const float targetRpmChangeRate = (currentPidProfile->idle_hz - minRpm) * currentPidProfile->idle_adjustment_speed;
const float error = targetRpmChangeRate - (minRpm - oldMinRpm) * pidFrequency;
const float pidSum = constrainf(currentPidProfile->idle_p * 0.0001f * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit);
motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * dT, 0.0f, currentPidProfile->idle_max_increase * 0.001);
oldMinRpm = minRpm;
}
throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
currentThrottleInputRange = rcCommandThrottleRange;
motorRangeMin = motorOutputLow;
motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow);
motorRangeMax = motorOutputHigh;
motorOutputMin = motorOutputLow;
motorOutputRange = motorOutputHigh - motorOutputLow;
motorOutputMin = motorRangeMin;
motorOutputRange = motorOutputHigh - motorOutputMin;
motorOutputMixSign = 1;
}
@ -832,6 +845,8 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
throttle = pidCompensateThrustLinearization(throttle);
#endif
throttle += currentPidProfile->idle_throttle * 0.001f;
#if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) {
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);