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

remove non-required parameters, simplify calling FF recalculation

Browse source
This commit is contained in:
ctzsnooze 2021-03-22 18:24:56 +11:00
parent 9cb2fdbf47
commit 34096c50f0
3 changed files with 12 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

29
src/main/fc/rc.c
View file

@ -61,9 +61,6 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
#ifdef USE_INTERPOLATED_SP #ifdef USE_INTERPOLATED_SP
// Setpoint in degrees/sec before RC-Smoothing is applied // Setpoint in degrees/sec before RC-Smoothing is applied
static float rawSetpoint[XYZ_AXIS_COUNT]; static float rawSetpoint[XYZ_AXIS_COUNT];
// Stick deflection [-1.0, 1.0] before RC-Smoothing is applied
static float rawDeflection[XYZ_AXIS_COUNT];
static float oldRcCommand[XYZ_AXIS_COUNT];
#endif #endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation; static float throttlePIDAttenuation;
@ -75,7 +72,7 @@ static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f; static float rcCommandYawDivider = 500.0f;
FAST_DATA_ZERO_INIT uint8_t interpolationChannels; FAST_DATA_ZERO_INIT uint8_t interpolationChannels;
static FAST_DATA_ZERO_INIT uint32_t rcFrameNumber; static FAST_DATA_ZERO_INIT bool newRxDataForFF;
enum { enum {
ROLL_FLAG = 1 << ROLL, ROLL_FLAG = 1 << ROLL,
@ -99,12 +96,18 @@ enum {
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
uint32_t getRcFrameNumber() bool getShouldUpdateFf()
// only used in pid.c when interpolated_sp is active to initiate a new FF value
{ {
return rcFrameNumber; const bool updateFf = newRxDataForFF;
if (newRxDataForFF == true){
newRxDataForFF = false;
}
return updateFf;
} }
float getSetpointRate(int axis) float getSetpointRate(int axis)
// only used in pid.c to provide setpointRate for the crash recovery function
{ {
return setpointRate[axis]; return setpointRate[axis];
} }
@ -130,11 +133,6 @@ float getRawSetpoint(int axis)
return rawSetpoint[axis]; return rawSetpoint[axis];
} }
float getRawDeflection(int axis)
{
return rawDeflection[axis];
}
#endif #endif
#define THROTTLE_LOOKUP_LENGTH 12 #define THROTTLE_LOOKUP_LENGTH 12
@ -235,11 +233,6 @@ float applyCurve(int axis, float deflection)
return applyRates(axis, deflection, fabsf(deflection)); return applyRates(axis, deflection, fabsf(deflection));
} }
float getRcCurveSlope(int axis, float deflection)
{
return (applyCurve(axis, deflection + 0.01f) - applyCurve(axis, deflection)) * 100.0f;
}
static void calculateSetpointRate(int axis) static void calculateSetpointRate(int axis)
{ {
float angleRate; float angleRate;
@ -691,7 +684,7 @@ FAST_CODE void processRcCommand(void)
uint8_t updatedChannel; uint8_t updatedChannel;
if (isRxDataNew) { if (isRxDataNew) {
rcFrameNumber++; newRxDataForFF = true;
} }
if (isRxDataNew && pidAntiGravityEnabled()) { if (isRxDataNew && pidAntiGravityEnabled()) {
@ -701,7 +694,6 @@ FAST_CODE void processRcCommand(void)
#ifdef USE_INTERPOLATED_SP #ifdef USE_INTERPOLATED_SP
if (isRxDataNew) { if (isRxDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) { for (int i = FD_ROLL; i <= FD_YAW; i++) {
oldRcCommand[i] = rcCommand[i];
float rcCommandf; float rcCommandf;
if (i == FD_YAW) { if (i == FD_YAW) {
rcCommandf = rcCommand[i] / rcCommandYawDivider; rcCommandf = rcCommand[i] / rcCommandYawDivider;
@ -710,7 +702,6 @@ FAST_CODE void processRcCommand(void)
} }
const float rcCommandfAbs = fabsf(rcCommandf); const float rcCommandfAbs = fabsf(rcCommandf);
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs); rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
rawDeflection[i] = rcCommandf;
} }
} }
#endif #endif

4
src/main/fc/rc.h
View file

@ -51,9 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void); bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void); bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis); float getRawSetpoint(int axis);
float getRawDeflection(int axis);
float applyCurve(int axis, float deflection); float applyCurve(int axis, float deflection);
uint32_t getRcFrameNumber(); bool getShouldUpdateFf();
float getRcCurveSlope(int axis, float deflection);
void updateRcRefreshRate(timeUs_t currentTimeUs); void updateRcRefreshRate(timeUs_t currentTimeUs);
uint16_t getCurrentRxRefreshRate(void); uint16_t getCurrentRxRefreshRate(void);

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

@ -803,9 +803,6 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{ {
static float previousGyroRateDterm[XYZ_AXIS_COUNT]; static float previousGyroRateDterm[XYZ_AXIS_COUNT];
#ifdef USE_INTERPOLATED_SP
static FAST_DATA_ZERO_INIT uint32_t lastFrameNumber;
#endif
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT]; static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
#if defined(USE_ACC) #if defined(USE_ACC)
@ -919,8 +916,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#ifdef USE_INTERPOLATED_SP #ifdef USE_INTERPOLATED_SP
bool newRcFrame = false; bool newRcFrame = false;
if (lastFrameNumber != getRcFrameNumber()) { if (getShouldUpdateFf()) {
lastFrameNumber = getRcFrameNumber();
newRcFrame = true; newRcFrame = true;
} }
#endif #endif