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

Merge branch 'master' of github.com:betaflight/betaflight into rescue_config_change

This commit is contained in:
s0up 2018-07-22 12:21:36 -07:00
parent 0fd24e23a4 bc22934185
commit 0f2dc6328e
46 changed files with 605 additions and 357 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Makefile
View File

@ -476,6 +476,37 @@ targets:
@echo "targets-group-rest: $(words $(GROUP_OTHER_TARGETS)) targets"
@echo "total in all groups $(words $(SUPPORTED_TARGETS)) targets"
## target-mcu : print the MCU type of the target
target-mcu:
@echo $(TARGET_MCU)
## targets-by-mcu : make all targets that have a MCU_TYPE mcu
targets-by-mcu:
@echo "Building all $(MCU_TYPE) targets..."
$(V1) for target in $(VALID_TARGETS); do \
TARGET_MCU_TYPE=$$($(MAKE) -s TARGET=$${target} target-mcu); \
if [ "$${TARGET_MCU_TYPE}" = "$${MCU_TYPE}" ]; then \
echo "Building target $${target}..."; \
$(MAKE) TARGET=$${target}; \
if [ $$? -ne 0 ]; then \
echo "Building target $${target} failed, aborting."; \
exit 1; \
fi; \
fi; \
done
## targets-f3 : make all F3 targets
targets-f3:
$(V1) $(MAKE) -s targets-by-mcu MCU_TYPE=STM32F3
## targets-f4 : make all F4 targets
targets-f4:
$(V1) $(MAKE) -s targets-by-mcu MCU_TYPE=STM32F4
## targets-f7 : make all F7 targets
targets-f7:
$(V1) $(MAKE) -s targets-by-mcu MCU_TYPE=STM32F7
## test : run the cleanflight test suite
## junittest : run the cleanflight test suite, producing Junit XML result files.
test junittest:

3
make/targets.mk
View File

@ -23,7 +23,8 @@ UNSUPPORTED_TARGETS := \
CC3D_OPBL \
CJMCU \
MICROSCISKY \
NAZE
NAZE \
KISSFCV2F7
SUPPORTED_TARGETS := $(filter-out $(UNSUPPORTED_TARGETS), $(VALID_TARGETS))

34
src/main/blackbox/blackbox.c
View File

@ -182,6 +182,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
{"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
{"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
{"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
/* rcCommands are encoded together as a group in P-frames: */
{"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
@ -285,6 +288,7 @@ typedef struct blackboxMainState_s {
int32_t axisPID_P[XYZ_AXIS_COUNT];
int32_t axisPID_I[XYZ_AXIS_COUNT];
int32_t axisPID_D[XYZ_AXIS_COUNT];
int32_t axisPID_F[XYZ_AXIS_COUNT];
int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT];
@ -531,6 +535,8 @@ static void writeIntraframe(void)
}
}
blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT);
// Write roll, pitch and yaw first:
blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
@ -662,6 +668,9 @@ static void writeInterframe(void)
}
}
arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
/*
* RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
* can pack multiple values per byte:
@ -992,6 +1001,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->axisPID_P[i] = pidData[i].P;
blackboxCurrent->axisPID_I[i] = pidData[i].I;
blackboxCurrent->axisPID_D[i] = pidData[i].D;
blackboxCurrent->axisPID_F[i] = pidData[i].F;
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
#ifdef USE_MAG
@ -1253,25 +1263,10 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
currentPidProfile->pid[PID_YAW].I,
currentPidProfile->pid[PID_YAW].D);
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P,
currentPidProfile->pid[PID_ALT].I,
currentPidProfile->pid[PID_ALT].D);
BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P,
currentPidProfile->pid[PID_POS].I,
currentPidProfile->pid[PID_POS].D);
BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P,
currentPidProfile->pid[PID_POSR].I,
currentPidProfile->pid[PID_POSR].D);
BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P,
currentPidProfile->pid[PID_NAVR].I,
currentPidProfile->pid[PID_NAVR].D);
BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
currentPidProfile->pid[PID_LEVEL].I,
currentPidProfile->pid[PID_LEVEL].D);
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P,
currentPidProfile->pid[PID_VEL].I,
currentPidProfile->pid[PID_VEL].D);
BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz", "%d", currentPidProfile->dterm_lowpass2_hz);
@ -1283,10 +1278,15 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
// Betaflight PID controller parameters
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d", currentPidProfile->antiGravityMode);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight);
BLACKBOX_PRINT_HEADER_LINE("feedforward_transition", "%d", currentPidProfile->feedForwardTransition);
BLACKBOX_PRINT_HEADER_LINE("feedforward_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
currentPidProfile->pid[PID_PITCH].F,
currentPidProfile->pid[PID_YAW].F);
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);

1
src/main/build/debug.c
View File

@ -74,4 +74,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RC_SMOOTHING",
"RX_SIGNAL_LOSS",
"RC_SMOOTHING_RATE",
"ANTI_GRAVITY",
};

1
src/main/build/debug.h
View File

@ -92,6 +92,7 @@ typedef enum {
DEBUG_RC_SMOOTHING,
DEBUG_RX_SIGNAL_LOSS,
DEBUG_RC_SMOOTHING_RATE,
DEBUG_ANTI_GRAVITY,
DEBUG_COUNT
} debugType_e;

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

@ -58,6 +58,7 @@ static uint8_t tmpPidProfileIndex;
static uint8_t pidProfileIndex;
static char pidProfileIndexString[] = " p";
static uint8_t tempPid[3][3];
static uint16_t tempPidF[3];
static uint8_t tmpRateProfileIndex;
static uint8_t rateProfileIndex;
@ -115,6 +116,7 @@ static long cmsx_PidRead(void)
tempPid[i][0] = pidProfile->pid[i].P;
tempPid[i][1] = pidProfile->pid[i].I;
tempPid[i][2] = pidProfile->pid[i].D;
tempPidF[i] = pidProfile->pid[i].F;
}
return 0;
@ -137,6 +139,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
pidProfile->pid[i].P = tempPid[i][0];
pidProfile->pid[i].I = tempPid[i][1];
pidProfile->pid[i].D = tempPid[i][2];
pidProfile->pid[i].F = tempPidF[i];
}
pidInitConfig(currentPidProfile);
@ -150,14 +153,17 @@ static OSD_Entry cmsx_menuPidEntries[] =
{ "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0], 0, 200, 1 }, 0 },
{ "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1], 0, 200, 1 }, 0 },
{ "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2], 0, 200, 1 }, 0 },
{ "ROLL F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL], 0, 2000, 1 }, 0 },
{ "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }, 0 },
{ "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }, 0 },
{ "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }, 0 },
{ "PITCH F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }, 0 },
{ "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0], 0, 200, 1 }, 0 },
{ "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1], 0, 200, 1 }, 0 },
{ "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2], 0, 200, 1 }, 0 },
{ "YAW F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW], 0, 2000, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
@ -237,8 +243,7 @@ static CMS_Menu cmsx_menuRateProfile = {
.entries = cmsx_menuRateProfileEntries
};
static uint16_t cmsx_dtermSetpointWeight;
static uint8_t cmsx_setpointRelaxRatio;
static uint8_t cmsx_feedForwardTransition;
static uint8_t cmsx_angleStrength;
static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition;
@ -250,8 +255,8 @@ static long cmsx_profileOtherOnEnter(void)
pidProfileIndexString[1] = '0' + tmpPidProfileIndex;
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio;
cmsx_feedForwardTransition = pidProfile->feedForwardTransition;
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
@ -268,8 +273,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
UNUSED(self);
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
pidInitConfig(currentPidProfile);
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
@ -285,8 +289,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
static OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
{ "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 0 },
{ "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 },
{ "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 },
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },

12
src/main/fc/config.c
View File

@ -263,10 +263,20 @@ static void validateAndFixConfig(void)
if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) {
for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->dtermSetpointWeight = 0;
pidProfilesMutable(i)->pid[PID_ROLL].F = 0;
pidProfilesMutable(i)->pid[PID_PITCH].F = 0;
}
}
if (!rcSmoothingIsEnabled() ||
(rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPY &&
rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPYT)) {
for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->pid[PID_YAW].F = 0;
}
}
#if defined(USE_THROTTLE_BOOST)
if (!rcSmoothingIsEnabled() ||
!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT

3
src/main/fc/fc_core.c
View File

@ -837,6 +837,9 @@ bool processRx(timeUs_t currentTimeUs)
beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
}
#endif
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
return true;
}

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

@ -566,7 +566,7 @@ void init(void)
#if defined(USE_MAX7456)
// If there is a max7456 chip for the OSD then use it
osdDisplayPort = max7456DisplayPortInit(vcdProfile());
#elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
#elif defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
osdDisplayPort = displayPortMspInit();
#endif
// osdInit will register with CMS by itself.

12
src/main/fc/fc_rc.c
View File

@ -212,10 +212,12 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
} else {
pidSetItermAccelerator(1.0f);
if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) {
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
} else {
pidSetItermAccelerator(1.0f);
}
}
}
@ -536,7 +538,7 @@ FAST_CODE void processRcCommand(void)
{
uint8_t updatedChannel;
if (isRXDataNew && isAntiGravityModeActive()) {
if (isRXDataNew && pidAntiGravityEnabled()) {
checkForThrottleErrorResetState(currentRxRefreshRate);
}

85
src/main/fc/rc_adjustments.c
View File

@ -203,11 +203,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.adjustmentFunction = ADJUSTMENT_FEEDFORWARD_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
@ -218,6 +218,18 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_PID_AUDIO,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) }
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_YAW_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}
};
@ -244,14 +256,17 @@ static const char * const adjustmentLabels[] = {
"ROLL I",
"ROLL D",
"RC RATE YAW",
"D SETPOINT",
"D SETPOINT TRANSITION",
"PITCH/ROLL F",
"FF TRANSITION",
"HORIZON STRENGTH",
"ROLL RC RATE",
"PITCH RC RATE",
"ROLL RC EXPO",
"PITCH RC EXPO",
"PID AUDIO",
"PITCH F",
"ROLL F",
"YAW F"
};
static int adjustmentRangeNameIndex = 0;
@ -405,15 +420,31 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
controlRateConfig->rcRates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 2000); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
case ADJUSTMENT_PITCH_ROLL_F:
case ADJUSTMENT_PITCH_F:
newValue = constrain(pidProfile->pid[PID_PITCH].F + delta, 0, 2000);
pidProfile->pid[PID_PITCH].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_F
FALLTHROUGH;
case ADJUSTMENT_ROLL_F:
newValue = constrain(pidProfile->pid[PID_ROLL].F + delta, 0, 2000);
pidProfile->pid[PID_ROLL].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
case ADJUSTMENT_YAW_F:
newValue = constrain(pidProfile->pid[PID_YAW].F + delta, 0, 2000);
pidProfile->pid[PID_YAW].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(pidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break;
default:
newValue = -1;
@ -554,15 +585,31 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
controlRateConfig->rcRates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
case ADJUSTMENT_PITCH_ROLL_F:
case ADJUSTMENT_PITCH_F:
newValue = constrain(value, 0, 2000);
pidProfile->pid[PID_PITCH].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_F
FALLTHROUGH;
case ADJUSTMENT_ROLL_F:
newValue = constrain(value, 0, 2000);
pidProfile->pid[PID_ROLL].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
case ADJUSTMENT_YAW_F:
newValue = constrain(value, 0, 2000);
pidProfile->pid[PID_YAW].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break;
default:
newValue = -1;

7
src/main/fc/rc_adjustments.h
View File

@ -47,14 +47,17 @@ typedef enum {
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_PITCH_ROLL_F,
ADJUSTMENT_FEEDFORWARD_TRANSITION,
ADJUSTMENT_HORIZON_STRENGTH,
ADJUSTMENT_ROLL_RC_RATE,
ADJUSTMENT_PITCH_RC_RATE,
ADJUSTMENT_ROLL_RC_EXPO,
ADJUSTMENT_PITCH_RC_EXPO,
ADJUSTMENT_PID_AUDIO,
ADJUSTMENT_PITCH_F,
ADJUSTMENT_ROLL_F,
ADJUSTMENT_YAW_F,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;

4
src/main/fc/rc_modes.c
View File

@ -62,10 +62,6 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isAntiGravityModeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
}
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;

7
src/main/fc/rc_modes.h
View File

@ -38,7 +38,6 @@ typedef enum {
BOXGPSHOLD,
BOXHEADFREE,
BOXPASSTHRU,
BOXRANGEFINDER,
BOXFAILSAFE,
BOXGPSRESCUE,
BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE,
@ -49,16 +48,11 @@ typedef enum {
BOXANTIGRAVITY,
BOXHEADADJ,
BOXCAMSTAB,
BOXCAMTRIG,
BOXBEEPERON,
BOXLEDMAX,
BOXLEDLOW,
BOXLLIGHTS,
BOXCALIB,
BOXGOV,
BOXOSD,
BOXTELEMETRY,
BOXGTUNE,
BOXSERVO1,
BOXSERVO2,
BOXSERVO3,
@ -132,7 +126,6 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId);
void rcModeUpdate(boxBitmask_t *newState);
bool isAirmodeActive(void);
bool isAntiGravityModeActive(void);
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);

5
src/main/fc/runtime_config.h
View File

@ -77,9 +77,9 @@ typedef enum {
GPS_HOME_MODE = (1 << 4),
GPS_HOLD_MODE = (1 << 5),
HEADFREE_MODE = (1 << 6),
UNUSED_MODE = (1 << 7), // old autotune
// UNUSED_MODE = (1 << 7), // old autotune
PASSTHRU_MODE = (1 << 8),
RANGEFINDER_MODE= (1 << 9),
// RANGEFINDER_MODE= (1 << 9),
FAILSAFE_MODE = (1 << 10),
GPS_RESCUE_MODE = (1 << 11)
} flightModeFlags_e;
@ -101,7 +101,6 @@ extern uint16_t flightModeFlags;
[BOXGPSHOLD] = LOG2( GPS_HOLD_MODE), \
[BOXHEADFREE] = LOG2(HEADFREE_MODE), \
[BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \
[BOXRANGEFINDER] = LOG2(RANGEFINDER_MODE), \
[BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \
[BOXGPSRESCUE] = LOG2(GPS_RESCUE_MODE), \
} \

6
src/main/flight/mixer.c
View File

@ -806,10 +806,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
motorMix[i] = mix;
}
pidUpdateAntiGravityThrottleFilter(throttle);
#if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) {
float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f);
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
}
#endif

1
src/main/flight/mixer.h
View File

@ -136,3 +136,4 @@ uint16_t convertMotorToExternal(float motorValue);
bool mixerIsTricopter(void);
void mixerSetThrottleAngleCorrection(int correctionValue);

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

@ -58,6 +58,13 @@
#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f
const char pidNames[] =
"ROLL;"
"PITCH;"
"YAW;"
"LEVEL;"
"MAG;";
FAST_RAM_ZERO_INIT uint32_t targetPidLooptime;
FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT];
@ -68,6 +75,12 @@ static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
static FAST_RAM_ZERO_INIT float dT;
static FAST_RAM_ZERO_INIT float pidFrequency;
static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
static FAST_RAM float antiGravityOsdCutoff = 1.0f;
static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
#ifdef STM32F10X
@ -96,22 +109,19 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
#endif // USE_ACRO_TRAINER
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
void resetPidProfile(pidProfile_t *pidProfile)
{
RESET_CONFIG(pidProfile_t, pidProfile,
.pid = {
[PID_ROLL] = { 46, 45, 25 },
[PID_PITCH] = { 50, 50, 27 },
[PID_YAW] = { 65, 45, 0 },
[PID_ALT] = { 50, 0, 0 },
[PID_POS] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
[PID_POSR] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
[PID_NAVR] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
[PID_LEVEL] = { 50, 50, 75 },
[PID_MAG] = { 40, 0, 0 },
[PID_VEL] = { 55, 55, 75 }
[PID_ROLL] = { 46, 45, 25, 60 },
[PID_PITCH] = { 50, 50, 27, 60 },
[PID_YAW] = { 65, 45, 0 , 60 },
[PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 },
},
.pidSumLimit = PIDSUM_LIMIT,
@ -126,11 +136,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55,
.setpointRelaxRatio = 0,
.dtermSetpointWeight = 60,
.feedForwardTransition = 0,
.yawRateAccelLimit = 100,
.rateAccelLimit = 0,
.itermThrottleThreshold = 350,
.itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000,
.crash_time = 500, // ms
.crash_delay = 0, // ms
@ -158,6 +167,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_gain = 0,
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
);
}
@ -182,9 +192,9 @@ void pidSetItermAccelerator(float newItermAccelerator)
itermAccelerator = newItermAccelerator;
}
float pidItermAccelerator(void)
bool pidOsdAntiGravityActive(void)
{
return itermAccelerator;
return (itermAccelerator > antiGravityOsdCutoff);
}
void pidStabilisationState(pidStabilisationState_e pidControllerState)
@ -200,11 +210,11 @@ typedef union dtermLowpass_u {
} dtermLowpass_t;
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[2];
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[2];
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX)
@ -215,16 +225,18 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2];
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2];
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
void pidInitFilters(const pidProfile_t *pidProfile)
{
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
BUILD_BUG_ON(FD_YAW != 2); // ensure yaw axis is 2
if (targetPidLooptime == 0) {
// no looptime set, so set all the filters to null
@ -250,7 +262,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
}
} else {
@ -262,7 +274,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermLowpass2ApplyFn = nullFilterApply;
} else {
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
}
}
@ -276,13 +288,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
break;
case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
}
break;
case FILTER_BIQUAD:
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
}
break;
@ -306,6 +318,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
}
#ifdef USE_RC_SMOOTHING_FILTER
@ -315,7 +329,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
rcSmoothingFilterType = filterType;
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
setpointDerivativeLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
@ -331,7 +345,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
{
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
@ -349,12 +363,12 @@ typedef struct pidCoefficient_s {
float Kp;
float Ki;
float Kd;
float Kf;
} pidCoefficient_t;
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3];
static FAST_RAM_ZERO_INIT float maxVelocity[3];
static FAST_RAM_ZERO_INIT float relaxFactor;
static FAST_RAM_ZERO_INIT float dtermSetpointWeight;
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float feedForwardTransition;
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
@ -402,20 +416,27 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pi
static FAST_RAM_ZERO_INIT float acroTrainerGain;
#endif // USE_ACRO_TRAINER
void pidUpdateAntiGravityThrottleFilter(float throttle)
{
if (antiGravityMode) {
antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
}
}
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
feedForwardTransition = 0;
} else {
feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
}
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
}
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f;
if (pidProfile->setpointRelaxRatio == 0) {
relaxFactor = 0;
} else {
relaxFactor = 100.0f / pidProfile->setpointRelaxRatio;
}
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
@ -426,6 +447,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000;
crashTimeDelayUs = pidProfile->crash_delay * 1000;
crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
@ -439,6 +461,18 @@ void pidInitConfig(const pidProfile_t *pidProfile)
throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
itermRotation = pidProfile->iterm_rotation;
antiGravityMode = pidProfile->antiGravityMode;
// Calculate the anti-gravity value that will trigger the OSD display.
// For classic AG it's either 1.0 for off and > 1.0 for on.
// For the new AG it's a continuous floating value so we want to trigger the OSD
// display when it exceeds 25% of its possible range. This gives a useful indication
// of AG activity without excessive display.
antiGravityOsdCutoff = 1.0f;
if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
}
#if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward;
#endif
@ -565,7 +599,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
static float accelerationLimit(int axis, float currentPidSetpoint)
{
static float previousSetpoint[3];
static float previousSetpoint[XYZ_AXIS_COUNT];
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity[axis]) {
@ -668,7 +702,7 @@ static void rotateITermAndAxisError()
#endif
) {
const float gyroToAngle = dT * RAD;
float rotationRads[3];
float rotationRads[XYZ_AXIS_COUNT];
for (int i = FD_ROLL; i <= FD_YAW; i++) {
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
}
@ -678,7 +712,7 @@ static void rotateITermAndAxisError()
}
#endif
if (itermRotation) {
float v[3];
float v[XYZ_AXIS_COUNT];
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
v[i] = pidData[i].I;
}
@ -764,12 +798,51 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
}
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
{
float ret = pidSetpointDelta;
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if (setpointDerivativeLpfInitialized) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
}
}
return ret;
}
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_SMART_FEEDFORWARD
void FAST_CODE applySmartFeedforward(int axis)
{
if (smartFeedforward) {
if (pidData[axis].P * pidData[axis].F > 0) {
if (ABS(pidData[axis].F) > ABS(pidData[axis].P)) {
pidData[axis].P = 0;
} else {
pidData[axis].F = 0;
}
}
}
}
#endif // USE_SMART_FEEDFORWARD
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[2];
static float previousPidSetpoint[2];
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
static float previousPidSetpoint[XYZ_AXIS_COUNT];
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
@ -779,15 +852,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#endif
// Dynamic i component,
if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
}
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
// gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
// Dynamic d component, enable 2-DOF PID controller only for rate mode
const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight;
// Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[2];
for (int axis = FD_ROLL; axis < FD_YAW; ++axis) {
float gyroRateDterm[XYZ_AXIS_COUNT];
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
@ -797,12 +872,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != YAW) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != FD_YAW) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
@ -899,7 +975,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
// b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
// b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
// -----calculate P component and add Dynamic Part based on stick input
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor;
@ -916,9 +992,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate D component
if (axis != FD_YAW) {
// no transition if relaxFactor == 0
float transition = relaxFactor > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * relaxFactor) : 1;
if (pidCoefficient[axis].Kd > 0) {
// Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time
@ -931,73 +1005,52 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
} else {
pidData[axis].D = 0;
}
previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
// Only enable feedforward for rate mode
const float feedforwardGain = flightModeFlags ? 0.0f : pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
#ifdef USE_RC_SMOOTHING_FILTER
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
pidSetpointDelta = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f));
}
}
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
#endif // USE_RC_SMOOTHING_FILTER
const float pidFeedForward =
pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency;
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
#if defined(USE_SMART_FEEDFORWARD)
bool addFeedforward = true;
if (smartFeedforward) {
if (pidData[axis].P * pidFeedForward > 0) {
if (ABS(pidFeedForward) > ABS(pidData[axis].P)) {
pidData[axis].P = 0;
} else {
addFeedforward = false;
}
}
}
if (addFeedforward)
applySmartFeedforward(axis);
#endif
{
pidData[axis].D += pidFeedForward;
}
previousGyroRateDterm[axis] = gyroRateDterm[axis];
previousPidSetpoint[axis] = currentPidSetpoint;
} else {
pidData[axis].F = 0;
}
previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_YAW_SPIN_RECOVERY
if (yawSpinActive) {
if (yawSpinActive) {
pidData[axis].I = 0; // in yaw spin always disable I
if (axis <= FD_PITCH) {
// zero PIDs on pitch and roll leaving yaw P to correct spin
pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 0;
pidData[axis].F = 0;
}
#endif // USE_YAW_SPIN_RECOVERY
}
}
// calculating the PID sum
pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
#ifdef USE_YAW_SPIN_RECOVERY
if (yawSpinActive) {
// yaw P alone to correct spin
pidData[FD_YAW].I = 0;
}
#endif // USE_YAW_SPIN_RECOVERY
// YAW has no D
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I;
// calculating the PID sum
pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
}
// Disable PID control if at zero throttle or if gyro overflow detected
// This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
@ -1006,6 +1059,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 0;
pidData[axis].F = 0;
pidData[axis].Sum = 0;
}
@ -1028,3 +1082,17 @@ void pidSetAcroTrainerState(bool newState)
}
}
#endif // USE_ACRO_TRAINER
void pidSetAntiGravityState(bool newState)
{
if (newState != antiGravityEnabled) {
// reset the accelerator on state changes
itermAccelerator = 1.0f;
}
antiGravityEnabled = newState;
}
bool pidAntiGravityEnabled(void)
{
return antiGravityEnabled;
}

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

@ -39,17 +39,16 @@
#define ITERM_SCALE 0.244381f
#define DTERM_SCALE 0.000529f
// The constant scale factor to replace the Kd component of the feedforward calculation.
// This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
#define FEEDFORWARD_SCALE 0.013754f
typedef enum {
PID_ROLL,
PID_PITCH,
PID_YAW,
PID_ALT,
PID_POS,
PID_POSR,
PID_NAVR,
PID_LEVEL,
PID_MAG,
PID_VEL,
PID_ITEM_COUNT
} pidIndex_e;
@ -70,11 +69,17 @@ typedef enum {
PID_CRASH_RECOVERY_BEEP
} pidCrashRecovery_e;
typedef struct pid8_s {
typedef struct pidf_s {
uint8_t P;
uint8_t I;
uint8_t D;
} pid8_t;
uint16_t F;
} pidf_t;
typedef enum {
ANTI_GRAVITY_SMOOTH,
ANTI_GRAVITY_STEP
} antiGravityMode_e;
typedef enum {
ITERM_RELAX_OFF,
@ -95,7 +100,7 @@ typedef struct pidProfile_s {
uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
pid8_t pid[PID_ITEM_COUNT];
pidf_t pid[PID_ITEM_COUNT];
uint8_t dterm_filter_type; // Filter selection for dterm
uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
@ -108,9 +113,9 @@ typedef struct pidProfile_s {
uint8_t horizon_tilt_expert_mode; // OFF or ON
// Betaflight PID controller parameters
uint8_t antiGravityMode; // type of anti gravity method
uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit
uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative)
uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
uint16_t crash_dthreshold; // dterm crash value
@ -121,15 +126,15 @@ typedef struct pidProfile_s {
uint8_t crash_recovery_angle; // degrees
uint8_t crash_recovery_rate; // degree/second
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
uint8_t feedForwardTransition; // Feed forward weight transition
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit;
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
uint8_t crash_recovery; // off, on, on and beeps when it is in crash recovery mode
uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign.
uint8_t iterm_relax_type; // Specifies type of relax algorithm
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
uint8_t iterm_relax; // Enable iterm suppression during stick input
@ -162,10 +167,13 @@ typedef struct pidAxisData_s {
float P;
float I;
float D;
float F;
float Sum;
} pidAxisData_t;
extern const char pidNames[];
extern pidAxisData_t pidData[3];
extern uint32_t targetPidLooptime;
@ -176,7 +184,6 @@ extern pt1Filter_t throttleLpf;
void pidResetITerm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator);
float pidItermAccelerator(void);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidInit(const pidProfile_t *pidProfile);
@ -186,3 +193,8 @@ void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void);
bool pidOsdAntiGravityMode(void);
void pidSetAntiGravityState(bool newState);
bool pidAntiGravityEnabled(void);

39
src/main/interface/msp.c
View File

@ -143,18 +143,6 @@ static uint8_t rebootMode;
#ifndef USE_OSD_SLAVE
static const char pidnames[] =
"ROLL;"
"PITCH;"
"YAW;"
"ALT;"
"Pos;"
"PosR;"
"NavR;"
"LEVEL;"
"MAG;"
"VEL;";
typedef enum {
MSP_SDCARD_STATE_NOT_PRESENT = 0,
MSP_SDCARD_STATE_FATAL = 1,
@ -984,7 +972,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
break;
case MSP_PIDNAMES:
for (const char *c = pidnames; *c; c++) {
for (const char *c = pidNames; *c; c++) {
sbufWriteU8(dst, *c);
}
break;
@ -1293,8 +1281,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio);
sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255));
sbufWriteU8(dst, currentPidProfile->feedForwardTransition);
sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved
@ -1304,7 +1292,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight);
sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, currentPidProfile->iterm_rotation);
#if defined(USE_SMART_FEEDFORWARD)
sbufWriteU8(dst, currentPidProfile->smart_feedforward);
@ -1333,6 +1321,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
sbufWriteU8(dst, currentPidProfile->antiGravityMode);
break;
case MSP_SENSOR_CONFIG:
@ -1835,8 +1828,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src); // was pidProfile.yaw_p_limit
sbufReadU8(src); // reserved
currentPidProfile->vbatPidCompensation = sbufReadU8(src);
currentPidProfile->setpointRelaxRatio = sbufReadU8(src);
currentPidProfile->dtermSetpointWeight = sbufReadU8(src);
currentPidProfile->feedForwardTransition = sbufReadU8(src);
sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
sbufReadU8(src); // reserved
sbufReadU8(src); // reserved
sbufReadU8(src); // reserved
@ -1851,9 +1844,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
}
if (sbufBytesRemaining(src) >= 2) {
currentPidProfile->dtermSetpointWeight = sbufReadU16(src);
sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight
}
if (sbufBytesRemaining(src) >= 7) {
if (sbufBytesRemaining(src) >= 14) {
// Added in MSP API 1.40
currentPidProfile->iterm_rotation = sbufReadU8(src);
#if defined(USE_SMART_FEEDFORWARD)
@ -1883,6 +1876,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#else
sbufReadU8(src);
#endif
// PID controller feedforward terms
currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src);
currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src);
currentPidProfile->pid[PID_YAW].F = sbufReadU16(src);
currentPidProfile->antiGravityMode = sbufReadU8(src);
}
pidInitConfig(currentPidProfile);

18
src/main/interface/msp_box.c
View File

@ -56,20 +56,20 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOXHEADFREE, "HEADFREE", 6 },
{ BOXHEADADJ, "HEADADJ", 7 },
{ BOXCAMSTAB, "CAMSTAB", 8 },
{ BOXCAMTRIG, "CAMTRIG", 9 },
// { BOXCAMTRIG, "CAMTRIG", 9 },
{ BOXGPSHOME, "GPS HOME", 10 },
{ BOXGPSHOLD, "GPS HOLD", 11 },
{ BOXPASSTHRU, "PASSTHRU", 12 },
{ BOXBEEPERON, "BEEPER", 13 },
{ BOXLEDMAX, "LEDMAX", 14 },
// { BOXLEDMAX, "LEDMAX", 14 }, (removed)
{ BOXLEDLOW, "LEDLOW", 15 },
{ BOXLLIGHTS, "LLIGHTS", 16 },
// { BOXLLIGHTS, "LLIGHTS", 16 }, (removed)
{ BOXCALIB, "CALIB", 17 },
{ BOXGOV, "GOVERNOR", 18 },
// { BOXGOV, "GOVERNOR", 18 }, (removed)
{ BOXOSD, "OSD DISABLE SW", 19 },
{ BOXTELEMETRY, "TELEMETRY", 20 },
{ BOXGTUNE, "GTUNE", 21 },
{ BOXRANGEFINDER, "RANGEFINDER", 22 },
// { BOXGTUNE, "GTUNE", 21 }, (removed)
// { BOXRANGEFINDER, "RANGEFINDER", 22 }, (removed)
{ BOXSERVO1, "SERVO1", 23 },
{ BOXSERVO2, "SERVO2", 24 },
{ BOXSERVO3, "SERVO3", 25 },
@ -198,12 +198,6 @@ void initActiveBoxIds(void)
}
#endif
#ifdef USE_RANGEFINDER
if (feature(FEATURE_RANGEFINDER)) { // XXX && sensors(SENSOR_RANGEFINDER)?
BME(BOXRANGEFINDER);
}
#endif
BME(BOXFAILSAFE);
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {

14
src/main/interface/settings.c
View File

@ -273,6 +273,10 @@ static const char * const lookupTableDtermLowpassType[] = {
"BIQUAD",
};
static const char * const lookupTableAntiGravityMode[] = {
"SMOOTH",
"STEP",
};
static const char * const lookupTableFailsafe[] = {
"AUTO-LAND", "DROP", "GPS-RESCUE"
@ -413,6 +417,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode),
LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode),
LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery),
@ -785,10 +790,10 @@ const clivalue_t valueTable[] = {
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
{ "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
{ "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },
{ "dterm_setpoint_weight", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
{ "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) },
{ "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
{ "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
{ "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },
@ -808,7 +813,7 @@ const clivalue_t valueTable[] = {
#if defined(USE_ITERM_RELAX)
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
{ "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
#endif
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
@ -831,12 +836,15 @@ const clivalue_t valueTable[] = {
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) },
{ "f_pitch", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].F) },
{ "p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].P) },
{ "i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].I) },
{ "d_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].D) },
{ "f_roll", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].F) },
{ "p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].P) },
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
{ "f_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].F) },
{ "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },

1
src/main/interface/settings.h
View File

@ -68,6 +68,7 @@ typedef enum {
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE,
TABLE_ANTI_GRAVITY_MODE,
TABLE_FAILSAFE,
TABLE_FAILSAFE_SWITCH_MODE,
TABLE_CRASH_RECOVERY,

4
src/main/io/osd.c
View File

@ -282,7 +282,7 @@ static void osdFormatAltitudeString(char * buff, int altitude)
buff[4] = '.';
}
static void osdFormatPID(char * buff, const char * label, const pid8_t * pid)
static void osdFormatPID(char * buff, const char * label, const pidf_t * pid)
{
tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
}
@ -599,7 +599,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ANTI_GRAVITY:
{
if (pidItermAccelerator() > 1.0f) {
if (pidOsdAntiGravityActive()) {
strcpy(buff, "AG");
}

2
src/main/pg/rx.c
View File

@ -56,7 +56,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rssi_offset = 0,
.rssi_invert = 0,
.rcInterpolation = RC_SMOOTHING_AUTO,
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPT,
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT,
.rcInterpolationInterval = 19,
.fpvCamAngleDegrees = 0,
.airModeActivateThreshold = 32,

12
src/main/sensors/gyro.c
View File

@ -520,6 +520,7 @@ bool gyroInit(void)
switch (debugMode) {
case DEBUG_FFT:
case DEBUG_FFT_FREQ:
case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED:
@ -1042,12 +1043,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
return;
}
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
}
#endif
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
@ -1069,6 +1064,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
} else {
filterGyroDebug(gyroSensor, sampleDeltaUs);
}
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(gyroSensor->notchFilterDyn);
}
#endif
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)

20
src/main/sensors/gyro_filter_impl.h
View File

@ -8,22 +8,30 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
#ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter
if (isDynamicFilterActive()) {
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
}
}
#endif
// apply static notch filters and software lowpass filters
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalysePush(axis, gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
}
}
#endif
// apply static notch filters and software lowpass filters
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
// DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));

125
src/main/sensors/gyroanalyse.c
View File

@ -18,6 +18,11 @@
* If not, see <http://www.gnu.org/licenses/>.
*/
/* original work by Rav
* 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
* coding assistance and advice from DieHertz, Rav, eTracer
* test pilots icr4sh, UAV Tech, Flint723
*/
#include <stdint.h>
#include "platform.h"
@ -42,22 +47,23 @@
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz
// Eg [0,31), [31,62), [62, 93) etc
#define FFT_WINDOW_SIZE 32 // max for f3 targets
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
#define FFT_MIN_FREQ 100 // not interested in filtering frequencies below 100Hz
#define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create
#define FFT_MAX_FREQUENCY (FFT_SAMPLING_RATE / 2) // nyquist rate
#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE / FFT_WINDOW_SIZE) // hz per bin
#define DYN_NOTCH_WIDTH 100 // just an orientation and start value
#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse...
#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies
#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis
#define FFT_WINDOW_SIZE 32 // max for f3 targets
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
#define FFT_BIN_OFFSET 1 // compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz
#define FFT_SAMPLING_RATE_HZ 1333 // analyse up to 666Hz, 16 bins each 41.655z wide
#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) // centre frequency of bandpass that constrains input to FFT
#define FFT_BIQUAD_Q 0.05f // bandpass quality factor, 0.1 for steep sided bandpass
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin
#define FFT_MIN_BIN_RISE 2 // following bin must be at least 2 times previous to indicate start of peak
#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 // lowpass frequency for smoothing notch centre point
#define DYN_NOTCH_MIN_CENTRE_HZ 125 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist
#define DYN_NOTCH_WIDTH_PERCENT 25 // maybe adjustable via CLI?
#define DYN_NOTCH_CUTOFF ((float)(100 - DYN_NOTCH_WIDTH_PERCENT) / 100)
#define DYN_NOTCH_MIN_CUTOFF_HZ 105 // lowest allowed notch cutoff frequency
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis
#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth
static FAST_RAM_ZERO_INIT uint16_t fftSamplingScale;
static FAST_RAM_ZERO_INIT uint16_t fftSamplingCount;
// gyro data used for frequency analysis
static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
@ -99,7 +105,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
fftSamplingCount = samplingFrequency / FFT_SAMPLING_RATE_HZ;
arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
initGyroData();
@ -108,11 +114,11 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
const float looptime = MAX(1000000u / FFT_SAMPLING_RATE, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
const float looptime = MAX(1000000u / FFT_SAMPLING_RATE_HZ, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftResult[axis].centerFreq = 200; // any init value
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, FFT_BIQUAD_Q, FILTER_BPF);
}
}
@ -122,34 +128,36 @@ const gyroFftData_t *gyroFftData(int axis)
return &fftResult[axis];
}
static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT];
void gyroDataAnalysePush(const int axis, const float sample)
{
fftAcc[axis] += sample;
}
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
void gyroDataAnalyse(biquadFilter_t *notchFilterDyn)
{
// accumulator for oversampled data => no aliasing and less noise
static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT uint32_t fftAccCount;
static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks;
// if gyro sampling is > 1kHz, accumulate multiple samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftAcc[axis] += gyroDev->gyroADC[axis];
}
// samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples
fftAccCount++;
// this runs at 1kHz
if (fftAccCount == fftSamplingScale) {
if (fftAccCount == fftSamplingCount) {
fftAccCount = 0;
//calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = fftAcc[axis] / fftSamplingScale;
float sample = fftAcc[axis] / fftSamplingCount;
sample = biquadFilterApply(&fftGyroFilter[axis], sample);
gyroData[axis][fftIdx] = sample;
if (axis == 0)
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
fftAcc[axis] = 0;
}
@ -244,37 +252,42 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
case STEP_CALC_FREQUENCIES:
{
// 13us
// calculate FFT centreFreq
float fftSum = 0;
float fftWeightedSum = 0;
fftResult[axis].maxVal = 0;
bool fftIncreasing = false;
float cubedData;
// iterate over fft data and calculate weighted indexes
float squaredData;
for (int i = 0; i < FFT_BIN_COUNT; i++) {
squaredData = fftData[i] * fftData[i]; //more weight on higher peaks
fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData);
fftSum += squaredData;
fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0
}
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
if (fftSum > 0) {
// idx was shifted by 1 to start at 1, not 0
float fftMeanIndex = (fftWeightedSum / fftSum) - 1;
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
// fftMeanIndex += 0.5;
// don't go below the minimal cutoff frequency + 10 and don't jump around too much
float centerFreq;
centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY);
centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY);
fftResult[axis].centerFreq = centerFreq;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) {
if (!fftIncreasing && (fftData[i] < fftData[i-1] * FFT_MIN_BIN_RISE)) {
// do nothing unless has increased at some point
} else {
cubedData = fftData[i] * fftData[i] * fftData[i]; //more weight on higher peaks
if (!fftIncreasing){
cubedData += fftData[i-1] * fftData[i-1] * fftData[i-1]; //add previous bin before first rise
}
fftSum += cubedData;
fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0
fftIncreasing = true;
}
}
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
float centerFreq;
float fftMeanIndex;
if (fftSum > 0) {
// idx was shifted by 1 to start at 1, not 0
fftMeanIndex = (fftWeightedSum / fftSum) - 1;
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
} else {
centerFreq = DYN_NOTCH_MAX_CENTRE_HZ; // if no peak, go to highest point to minimise delay
}
centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
fftResult[axis].centerFreq = centerFreq;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
}
DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
@ -282,8 +295,8 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
case STEP_UPDATE_FILTERS:
{
// 7us
// calculate new filter coefficients
float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
// calculate cutoffFreq and notch Q, update notch filter
float cutoffFreq = fmax(fftResult[axis].centerFreq * DYN_NOTCH_CUTOFF, DYN_NOTCH_MIN_CUTOFF_HZ);
float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq);
biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);

3
src/main/sensors/gyroanalyse.h
View File

@ -31,5 +31,6 @@ typedef struct gyroFftData_s {
void gyroDataAnalyseInit(uint32_t targetLooptime);
const gyroFftData_t *gyroFftData(int axis);
struct gyroDev_s;
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn);
void gyroDataAnalysePush(int axis, float sample);
void gyroDataAnalyse(biquadFilter_t *notchFilterDyn);
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);

5
src/main/target/ALIENWHOOP/config.c
View File

@ -139,8 +139,9 @@ void targetConfiguration(void)
/* Setpoints */
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_notch_hz = 0;
pidProfile->dtermSetpointWeight = 100;
pidProfile->setpointRelaxRatio = 0;
pidProfile->pid[PID_PITCH].F = 100;
pidProfile->pid[PID_ROLL].F = 100;
pidProfile->feedForwardTransition = 0;
/* Anti-Gravity */
pidProfile->itermThrottleThreshold = 500;

12
src/main/target/BETAFLIGHTF3/target.h
View File

@ -28,13 +28,13 @@
//#undef USE_GYRO_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2
//#undef USE_ITERM_RELAX
//#undef USE_RC_SMOOTHING_FILTER
#undef USE_ITERM_RELAX
#undef USE_RC_SMOOTHING_FILTER
//#undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM
//#undef USE_SERIALRX_XBUS
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS
#undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS

13
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -159,20 +159,14 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXCAMTRIG, "CAMTRIG;", 9 },
{ BOXGPSHOME, "GPS HOME;", 10 },
{ BOXGPSHOLD, "GPS HOLD;", 11 },
{ BOXPASSTHRU, "PASSTHRU;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDMAX, "LEDMAX;", 14 },
{ BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLLIGHTS, "LLIGHTS;", 16 },
{ BOXCALIB, "CALIB;", 17 },
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD DISABLE SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXGTUNE, "GTUNE;", 21 },
{ BOXRANGEFINDER, "RANGEFINDER;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
@ -305,20 +299,14 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << BOXRANGEFINDER |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
@ -878,7 +866,6 @@ bool writeFCModeToBST(void)
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 |
IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << 6 |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
bstMasterStartBuffer(PUBLIC_ADDRESS);

10
src/main/target/FRSKYF3/target.h
View File

@ -27,12 +27,12 @@
//#undef USE_GYRO_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2
//#undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM
//#undef USE_SERIALRX_XBUS
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS
//#undef USE_BOARD_INFO
#undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#undef USE_RX_MSP

8
src/main/target/FURYF3/target.h
View File

@ -35,16 +35,16 @@
//#undef USE_ITERM_RELAX
//#undef USE_RC_SMOOTHING_FILTER
//#undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM
#ifdef FURYF3OSD
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS
#undef USE_BOARD_INFO
#endif
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#endif
#undef USE_RX_MSP
#undef USE_ESC_SENSOR_INFO

19
src/main/target/IMPULSERCF3/target.h
View File

@ -23,6 +23,25 @@
#define TARGET_BOARD_IDENTIFIER "IMF3"
// Removed to make the firmware fit into flash (in descending order of priority):
//#undef USE_GYRO_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2
//#undef USE_ITERM_RELAX
//#undef USE_RC_SMOOTHING_FILTER
//#undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM
//#undef USE_SERIALRX_XBUS
//#undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#undef USE_RX_MSP
#undef USE_ESC_SENSOR_INFO
#define ENABLE_DSHOT_DMAR true
#define LED0_PIN PB7

5
src/main/target/NAZE/config.c
View File

@ -85,8 +85,9 @@ void targetConfiguration(void)
pidProfile->pid[PID_LEVEL].P = 30;
pidProfile->pid[PID_LEVEL].D = 30;
pidProfile->dtermSetpointWeight = 200;
pidProfile->setpointRelaxRatio = 50;
pidProfile->pid[PID_PITCH].F = 200;
pidProfile->pid[PID_ROLL].F = 200;
pidProfile->feedForwardTransition = 50;
}
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {

7
src/main/target/OMNIBUS/target.h
View File

@ -29,6 +29,13 @@
#undef USE_ITERM_RELAX
#undef USE_RC_SMOOTHING_FILTER
#undef USE_MSP_DISPLAYPORT
#undef USE_MSP_OVER_TELEMETRY
#undef USE_HUFFMAN
#undef USE_PINIO
#undef USE_PINIOBOX
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM

18
src/main/target/SIRINFPV/target.h
View File

@ -23,7 +23,25 @@
#define TARGET_BOARD_IDENTIFIER "SIRF"
// Removed to make the firmware fit into flash (in descending order of priority):
//#undef USE_GYRO_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2
//#undef USE_ITERM_RELAX
//#undef USE_RC_SMOOTHING_FILTER
//#undef USE_MSP_DISPLAYPORT
//#undef USE_MSP_OVER_TELEMETRY
//#undef USE_HUFFMAN
//#undef USE_PINIO
//#undef USE_PINIOBOX
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS
#undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#undef USE_RX_MSP

12
src/main/target/SPRACINGF3EVO/target.h
View File

@ -36,14 +36,22 @@
#undef USE_RC_SMOOTHING_FILTER
#undef ITERM_RELAX
#undef USE_MSP_DISPLAYPORT
#undef USE_MSP_OVER_TELEMETRY
#undef USE_HUFFMAN
#undef USE_PINIO
#undef USE_PINIOBOX
#undef USE_VIRTUAL_CURRENT_METER
#endif
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS
#undef USE_BOARD_INFO
#endif
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#undef USE_RX_MSP

10
src/main/target/SPRACINGF3MINI/target.h
View File

@ -36,12 +36,12 @@
//#undef USE_GYRO_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2
//#undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM
//#undef USE_SERIALRX_XBUS
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS
//#undef USE_BOARD_INFO
#undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#undef USE_RX_MSP

9
src/main/telemetry/mavlink.c
View File

@ -491,12 +491,15 @@ void mavlinkSendHUDAndHeartbeat(void)
mavCustomMode = 0; //Stabilize
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE))
if (FLIGHT_MODE(BARO_MODE)) {
mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(GPS_HOME_MODE))
}
if (FLIGHT_MODE(GPS_HOME_MODE)) {
mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(GPS_HOLD_MODE))
}
if (FLIGHT_MODE(GPS_HOLD_MODE)) {
mavCustomMode = 16; //Position Hold (Earlier called Hybrid)
}
uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) {

31
src/main/telemetry/smartport.c
View File

@ -675,31 +675,36 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
} else {
tmpi += 2;
}
if (ARMING_FLAG(ARMED))
if (ARMING_FLAG(ARMED)) {
tmpi += 4;
}
if (FLIGHT_MODE(ANGLE_MODE))
if (FLIGHT_MODE(ANGLE_MODE)) {
tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE))
}
if (FLIGHT_MODE(HORIZON_MODE)) {
tmpi += 20;
if (FLIGHT_MODE(UNUSED_MODE))
tmpi += 40;
if (FLIGHT_MODE(PASSTHRU_MODE))
}
if (FLIGHT_MODE(PASSTHRU_MODE)) {
tmpi += 40;
}
if (FLIGHT_MODE(MAG_MODE))
if (FLIGHT_MODE(MAG_MODE)) {
tmpi += 100;
if (FLIGHT_MODE(BARO_MODE))
}
if (FLIGHT_MODE(BARO_MODE)) {
tmpi += 200;
if (FLIGHT_MODE(RANGEFINDER_MODE))
tmpi += 400;
}
if (FLIGHT_MODE(GPS_HOLD_MODE))
if (FLIGHT_MODE(GPS_HOLD_MODE)) {
tmpi += 1000;
if (FLIGHT_MODE(GPS_HOME_MODE))
}
if (FLIGHT_MODE(GPS_HOME_MODE)) {
tmpi += 2000;
if (FLIGHT_MODE(HEADFREE_MODE))
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
tmpi += 4000;
}
smartPortSendPackage(id, (uint32_t)tmpi);
*clearToSend = false;

1
src/test/unit/arming_prevention_unittest.cc
View File

@ -850,4 +850,5 @@ extern "C" {
void rescheduleTask(cfTaskId_e, uint32_t) {}
bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool) {}
}

2
src/test/unit/osd_unittest.cc
View File

@ -1036,5 +1036,5 @@ extern "C" {
return false;
}
float pidItermAccelerator(void) { return 1.0; }
bool pidOsdAntiGravityActive(void) { return false; }
}

16
src/test/unit/pid_unittest.cc
View File

@ -84,10 +84,10 @@ int loopIter = 0;
void setDefaultTestSettings(void) {
pgResetAll();
pidProfile = pidProfilesMutable(1);
pidProfile->pid[PID_ROLL] = { 40, 40, 30 };
pidProfile->pid[PID_PITCH] = { 58, 50, 35 };
pidProfile->pid[PID_YAW] = { 70, 45, 0 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75 };
pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 };
pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 };
pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 };
pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
@ -101,11 +101,11 @@ void setDefaultTestSettings(void) {
pidProfile->vbatPidCompensation = 0;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55;
pidProfile->setpointRelaxRatio = 100;
pidProfile->dtermSetpointWeight = 0;
pidProfile->feedForwardTransition = 100;
pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0;
pidProfile->itermThrottleThreshold = 350;
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
pidProfile->itermThrottleThreshold = 250;
pidProfile->itermAcceleratorGain = 1000;
pidProfile->crash_time = 500;
pidProfile->crash_delay = 0;
@ -266,7 +266,7 @@ TEST(pidControllerTest, testPidLoop) {
ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7));
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
EXPECT_FLOAT_EQ(-132.25, pidData[FD_YAW].D);
// Match the stick to gyro to stop error
simulatedSetpointRate[FD_ROLL] = 100;

1
src/test/unit/vtx_unittest.cc
View File

@ -172,4 +172,5 @@ extern "C" {
void rescheduleTask(cfTaskId_e, uint32_t) {}
bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
}