Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +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

3
src/main/cli/settings.c
View file

@ -1057,9 +1057,8 @@ const clivalue_t valueTable[] = {
#endif #endif
#ifdef USE_DYN_IDLE #ifdef USE_DYN_IDLE
{ "idle_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_hz) }, { "idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_min_rpm) },
{ "idle_adjustment_speed", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_adjustment_speed) }, { "idle_adjustment_speed", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_adjustment_speed) },
{ "idle_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_throttle) },
{ "idle_p", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_p) }, { "idle_p", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_p) },
{ "idle_pid_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_pid_limit) }, { "idle_pid_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_pid_limit) },
{ "idle_max_increase", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_max_increase) }, { "idle_max_increase", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_max_increase) },

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

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

3
src/main/flight/pid.c
View file

@ -206,9 +206,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY, .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
.transient_throttle_limit = 15, .transient_throttle_limit = 15,
.profileName = { 0 }, .profileName = { 0 },
.idle_hz = 0, .idle_min_rpm = 0,
.idle_adjustment_speed = 50, .idle_adjustment_speed = 50,
.idle_throttle = 60,
.idle_p = 50, .idle_p = 50,
.idle_pid_limit = 200, .idle_pid_limit = 200,
.idle_max_increase = 150, .idle_max_increase = 150,

3
src/main/flight/pid.h
View file

@ -172,8 +172,7 @@ typedef struct pidProfile_s {
uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
uint8_t idle_hz; // minimum motor speed enforced by integrating p controller uint8_t idle_min_rpm; // minimum motor speed enforced by integrating p controller
uint8_t idle_throttle; // added to throttle, replaces dshot_idle_value
uint8_t idle_adjustment_speed; // how quickly the integrating p controller tries to correct uint8_t idle_adjustment_speed; // how quickly the integrating p controller tries to correct
uint8_t idle_p; // kP uint8_t idle_p; // kP
uint8_t idle_pid_limit; // max P uint8_t idle_pid_limit; // max P