rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Minor tidy of PID code

This commit is contained in:
Martin Budden 2017-09-09 07:03:15 +01:00
parent 247a88be50
commit c1a99817f4
6 changed files with 23 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/cms/cms_menu_imu.c
View File

@ -420,7 +420,7 @@ static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
UNUSED(ptr);
if (cmsx_dstPidProfile > 0) {
copyPidProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
}
return 0;

9
src/main/common/filter.c
View File

@ -295,6 +295,7 @@ float firFilterLastInput(const firFilter_t *filter)
void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
{
memset(filter, 0, sizeof(firFilterDenoise_t));
filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE);
}
@ -303,12 +304,14 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
{
filter->state[filter->index] = input;
filter->movingSum += filter->state[filter->index++];
if (filter->index == filter->targetCount)
if (filter->index == filter->targetCount) {
filter->index = 0;
}
filter->movingSum -= filter->state[filter->index];
if (filter->targetCount >= filter->filledCount)
if (filter->targetCount >= filter->filledCount) {
return filter->movingSum / filter->targetCount;
else
} else {
return filter->movingSum / ++filter->filledCount + 1;
}
}

2
src/main/common/filter.h
View File

@ -44,7 +44,7 @@ typedef struct biquadFilter_s {
float x1, x2, y1, y2;
} biquadFilter_t;
typedef struct firFilterDenoise_s{
typedef struct firFilterDenoise_s {
int filledCount;
int targetCount;
int index;

2
src/main/fc/fc_msp.c
View File

@ -1326,7 +1326,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
uint8_t dstProfileIndex = sbufReadU8(src);
uint8_t srcProfileIndex = sbufReadU8(src);
if (value == 0) {
copyPidProfile(dstProfileIndex, srcProfileIndex);
pidCopyProfile(dstProfileIndex, srcProfileIndex);
}
else if (value == 1) {
copyControlRateProfile(dstProfileIndex, srcProfileIndex);

25
src/main/flight/pid.c
View File

@ -126,7 +126,7 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
}
}
void pidSetTargetLooptime(uint32_t pidLooptime)
static void pidSetTargetLooptime(uint32_t pidLooptime)
{
targetPidLooptime = pidLooptime;
dT = (float)targetPidLooptime * 0.000001f;
@ -199,8 +199,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
dtermLpfApplyFn = nullFilterApply;
} else {
memset(&dtermFilterLpfUnion, 0, sizeof(dtermFilterLpfUnion));
switch (pidProfile->dterm_filter_type) {
default:
dtermLpfApplyFn = nullFilterApply;
@ -294,6 +292,16 @@ void pidInit(const pidProfile_t *pidProfile)
pidInitMixer(pidProfile);
}
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
{
if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)
&& dstPidProfileIndex != srcPidProfileIndex
) {
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
}
}
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
static float calcHorizonLevelStrength(void)
{
@ -371,7 +379,8 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
return currentPidSetpoint;
}
static float accelerationLimit(int axis, float currentPidSetpoint) {
static float accelerationLimit(int axis, float currentPidSetpoint)
{
static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
@ -523,11 +532,3 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
}
}
void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex) {
if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)
&& dstPidProfileIndex != srcPidProfileIndex
) {
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
}
}

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

@ -128,11 +128,10 @@ extern uint8_t PIDweight[3];
void pidResetErrorGyroState(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetTargetLooptime(uint32_t pidLooptime);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidInit(const pidProfile_t *pidProfile);
void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex);
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
#endif