mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 20:10:18 +03:00
address code review
This commit is contained in:
parent
293dc42710
commit
d033a8dc76
4 changed files with 30 additions and 24 deletions
|
@ -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) },
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue