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 "targets-group-rest: $(words $(GROUP_OTHER_TARGETS)) targets"
@echo "total in all groups $(words $(SUPPORTED_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 ## test : run the cleanflight test suite
## junittest : run the cleanflight test suite, producing Junit XML result files. ## junittest : run the cleanflight test suite, producing Junit XML result files.
test junittest: test junittest:

3
make/targets.mk
View File

@ -23,7 +23,8 @@ UNSUPPORTED_TARGETS := \
CC3D_OPBL \ CC3D_OPBL \
CJMCU \ CJMCU \
MICROSCISKY \ MICROSCISKY \
NAZE NAZE \
KISSFCV2F7
SUPPORTED_TARGETS := $(filter-out $(UNSUPPORTED_TARGETS), $(VALID_TARGETS)) 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", 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", 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)}, {"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: */ /* 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", 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)}, {"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_P[XYZ_AXIS_COUNT];
int32_t axisPID_I[XYZ_AXIS_COUNT]; int32_t axisPID_I[XYZ_AXIS_COUNT];
int32_t axisPID_D[XYZ_AXIS_COUNT]; int32_t axisPID_D[XYZ_AXIS_COUNT];
int32_t axisPID_F[XYZ_AXIS_COUNT];
int16_t rcCommand[4]; int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT]; 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: // Write roll, pitch and yaw first:
blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); 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 * 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: * 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_P[i] = pidData[i].P;
blackboxCurrent->axisPID_I[i] = pidData[i].I; blackboxCurrent->axisPID_I[i] = pidData[i].I;
blackboxCurrent->axisPID_D[i] = pidData[i].D; blackboxCurrent->axisPID_D[i] = pidData[i].D;
blackboxCurrent->axisPID_F[i] = pidData[i].F;
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]); blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
#ifdef USE_MAG #ifdef USE_MAG
@ -1253,25 +1263,10 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P, BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
currentPidProfile->pid[PID_YAW].I, currentPidProfile->pid[PID_YAW].I,
currentPidProfile->pid[PID_YAW].D); 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, BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
currentPidProfile->pid[PID_LEVEL].I, currentPidProfile->pid[PID_LEVEL].I,
currentPidProfile->pid[PID_LEVEL].D); currentPidProfile->pid[PID_LEVEL].D);
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P); 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_filter_type", "%d", currentPidProfile->dterm_filter_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz", "%d", currentPidProfile->dterm_lowpass2_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); BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
// Betaflight PID controller parameters // 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_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); 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_yaw", "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); 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", "RC_SMOOTHING",
"RX_SIGNAL_LOSS", "RX_SIGNAL_LOSS",
"RC_SMOOTHING_RATE", "RC_SMOOTHING_RATE",
"ANTI_GRAVITY",
}; };

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

@ -92,6 +92,7 @@ typedef enum {
DEBUG_RC_SMOOTHING, DEBUG_RC_SMOOTHING,
DEBUG_RX_SIGNAL_LOSS, DEBUG_RX_SIGNAL_LOSS,
DEBUG_RC_SMOOTHING_RATE, DEBUG_RC_SMOOTHING_RATE,
DEBUG_ANTI_GRAVITY,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } 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 uint8_t pidProfileIndex;
static char pidProfileIndexString[] = " p"; static char pidProfileIndexString[] = " p";
static uint8_t tempPid[3][3]; static uint8_t tempPid[3][3];
static uint16_t tempPidF[3];
static uint8_t tmpRateProfileIndex; static uint8_t tmpRateProfileIndex;
static uint8_t rateProfileIndex; static uint8_t rateProfileIndex;
@ -115,6 +116,7 @@ static long cmsx_PidRead(void)
tempPid[i][0] = pidProfile->pid[i].P; tempPid[i][0] = pidProfile->pid[i].P;
tempPid[i][1] = pidProfile->pid[i].I; tempPid[i][1] = pidProfile->pid[i].I;
tempPid[i][2] = pidProfile->pid[i].D; tempPid[i][2] = pidProfile->pid[i].D;
tempPidF[i] = pidProfile->pid[i].F;
} }
return 0; return 0;
@ -137,6 +139,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
pidProfile->pid[i].P = tempPid[i][0]; pidProfile->pid[i].P = tempPid[i][0];
pidProfile->pid[i].I = tempPid[i][1]; pidProfile->pid[i].I = tempPid[i][1];
pidProfile->pid[i].D = tempPid[i][2]; pidProfile->pid[i].D = tempPid[i][2];
pidProfile->pid[i].F = tempPidF[i];
} }
pidInitConfig(currentPidProfile); 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 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 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 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 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 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 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 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 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 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 }, { "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 } { NULL, OME_END, NULL, NULL, 0 }
@ -237,8 +243,7 @@ static CMS_Menu cmsx_menuRateProfile = {
.entries = cmsx_menuRateProfileEntries .entries = cmsx_menuRateProfileEntries
}; };
static uint16_t cmsx_dtermSetpointWeight; static uint8_t cmsx_feedForwardTransition;
static uint8_t cmsx_setpointRelaxRatio;
static uint8_t cmsx_angleStrength; static uint8_t cmsx_angleStrength;
static uint8_t cmsx_horizonStrength; static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition; static uint8_t cmsx_horizonTransition;
@ -250,8 +255,8 @@ static long cmsx_profileOtherOnEnter(void)
pidProfileIndexString[1] = '0' + tmpPidProfileIndex; pidProfileIndexString[1] = '0' + tmpPidProfileIndex;
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); 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_angleStrength = pidProfile->pid[PID_LEVEL].P;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I; cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
@ -268,8 +273,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
UNUSED(self); UNUSED(self);
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidInitConfig(currentPidProfile); pidInitConfig(currentPidProfile);
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength; pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
@ -285,8 +289,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
static OSD_Entry cmsx_menuProfileOtherEntries[] = { static OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
{ "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 0 }, { "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 },
{ "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 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 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 }, { "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) { if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) {
for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { 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 defined(USE_THROTTLE_BOOST)
if (!rcSmoothingIsEnabled() || if (!rcSmoothingIsEnabled() ||
!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT !(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); beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
} }
#endif #endif
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
return true; return true;
} }

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

@ -566,7 +566,7 @@ void init(void)
#if defined(USE_MAX7456) #if defined(USE_MAX7456)
// If there is a max7456 chip for the OSD then use it // If there is a max7456 chip for the OSD then use it
osdDisplayPort = max7456DisplayPortInit(vcdProfile()); 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(); osdDisplayPort = displayPortMspInit();
#endif #endif
// osdInit will register with CMS by itself. // 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]; const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
} else { pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
pidSetItermAccelerator(1.0f); } else {
pidSetItermAccelerator(1.0f);
}
} }
} }
@ -536,7 +538,7 @@ FAST_CODE void processRcCommand(void)
{ {
uint8_t updatedChannel; uint8_t updatedChannel;
if (isRXDataNew && isAntiGravityModeActive()) { if (isRXDataNew && pidAntiGravityEnabled()) {
checkForThrottleErrorResetState(currentRxRefreshRate); 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, .mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 } .data = { .step = 1 }
}, { }, {
.adjustmentFunction = ADJUSTMENT_D_SETPOINT, .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_F,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 } .data = { .step = 1 }
}, { }, {
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, .adjustmentFunction = ADJUSTMENT_FEEDFORWARD_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 } .data = { .step = 1 }
}, { }, {
@ -218,6 +218,18 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_PID_AUDIO, .adjustmentFunction = ADJUSTMENT_PID_AUDIO,
.mode = ADJUSTMENT_MODE_SELECT, .mode = ADJUSTMENT_MODE_SELECT,
.data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) } .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 I",
"ROLL D", "ROLL D",
"RC RATE YAW", "RC RATE YAW",
"D SETPOINT", "PITCH/ROLL F",
"D SETPOINT TRANSITION", "FF TRANSITION",
"HORIZON STRENGTH", "HORIZON STRENGTH",
"ROLL RC RATE", "ROLL RC RATE",
"PITCH RC RATE", "PITCH RC RATE",
"ROLL RC EXPO", "ROLL RC EXPO",
"PITCH RC EXPO", "PITCH RC EXPO",
"PID AUDIO", "PID AUDIO",
"PITCH F",
"ROLL F",
"YAW F"
}; };
static int adjustmentRangeNameIndex = 0; static int adjustmentRangeNameIndex = 0;
@ -405,15 +420,31 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
controlRateConfig->rcRates[FD_YAW] = newValue; controlRateConfig->rcRates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break; break;
case ADJUSTMENT_D_SETPOINT: case ADJUSTMENT_PITCH_ROLL_F:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 2000); // FIXME magic numbers repeated in cli.c case ADJUSTMENT_PITCH_F:
pidProfile->dtermSetpointWeight = newValue; newValue = constrain(pidProfile->pid[PID_PITCH].F + delta, 0, 2000);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); 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; break;
case ADJUSTMENT_D_SETPOINT_TRANSITION: case ADJUSTMENT_YAW_F:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c newValue = constrain(pidProfile->pid[PID_YAW].F + delta, 0, 2000);
pidProfile->setpointRelaxRatio = newValue; pidProfile->pid[PID_YAW].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, 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; break;
default: default:
newValue = -1; newValue = -1;
@ -554,15 +585,31 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
controlRateConfig->rcRates[FD_YAW] = newValue; controlRateConfig->rcRates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break; break;
case ADJUSTMENT_D_SETPOINT: case ADJUSTMENT_PITCH_ROLL_F:
newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c case ADJUSTMENT_PITCH_F:
pidProfile->dtermSetpointWeight = newValue; newValue = constrain(value, 0, 2000);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); 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; 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 newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue; pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break; break;
default: default:
newValue = -1; newValue = -1;

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

@ -47,14 +47,17 @@ typedef enum {
ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW, ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT, ADJUSTMENT_PITCH_ROLL_F,
ADJUSTMENT_D_SETPOINT_TRANSITION, ADJUSTMENT_FEEDFORWARD_TRANSITION,
ADJUSTMENT_HORIZON_STRENGTH, ADJUSTMENT_HORIZON_STRENGTH,
ADJUSTMENT_ROLL_RC_RATE, ADJUSTMENT_ROLL_RC_RATE,
ADJUSTMENT_PITCH_RC_RATE, ADJUSTMENT_PITCH_RC_RATE,
ADJUSTMENT_ROLL_RC_EXPO, ADJUSTMENT_ROLL_RC_EXPO,
ADJUSTMENT_PITCH_RC_EXPO, ADJUSTMENT_PITCH_RC_EXPO,
ADJUSTMENT_PID_AUDIO, ADJUSTMENT_PID_AUDIO,
ADJUSTMENT_PITCH_F,
ADJUSTMENT_ROLL_F,
ADJUSTMENT_YAW_F,
ADJUSTMENT_FUNCTION_COUNT ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e; } 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)); 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) { bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) { if (!IS_RANGE_USABLE(range)) {
return false; return false;

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

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

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

@ -77,9 +77,9 @@ typedef enum {
GPS_HOME_MODE = (1 << 4), GPS_HOME_MODE = (1 << 4),
GPS_HOLD_MODE = (1 << 5), GPS_HOLD_MODE = (1 << 5),
HEADFREE_MODE = (1 << 6), HEADFREE_MODE = (1 << 6),
UNUSED_MODE = (1 << 7), // old autotune // UNUSED_MODE = (1 << 7), // old autotune
PASSTHRU_MODE = (1 << 8), PASSTHRU_MODE = (1 << 8),
RANGEFINDER_MODE= (1 << 9), // RANGEFINDER_MODE= (1 << 9),
FAILSAFE_MODE = (1 << 10), FAILSAFE_MODE = (1 << 10),
GPS_RESCUE_MODE = (1 << 11) GPS_RESCUE_MODE = (1 << 11)
} flightModeFlags_e; } flightModeFlags_e;
@ -101,7 +101,6 @@ extern uint16_t flightModeFlags;
[BOXGPSHOLD] = LOG2( GPS_HOLD_MODE), \ [BOXGPSHOLD] = LOG2( GPS_HOLD_MODE), \
[BOXHEADFREE] = LOG2(HEADFREE_MODE), \ [BOXHEADFREE] = LOG2(HEADFREE_MODE), \
[BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \ [BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \
[BOXRANGEFINDER] = LOG2(RANGEFINDER_MODE), \
[BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \ [BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \
[BOXGPSRESCUE] = LOG2(GPS_RESCUE_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; motorMix[i] = mix;
} }
pidUpdateAntiGravityThrottleFilter(throttle);
#if defined(USE_THROTTLE_BOOST) #if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) { if (throttleBoost > 0.0f) {
float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle); const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f); throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
} }
#endif #endif

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

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

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

@ -58,6 +58,13 @@
#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f #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 uint32_t targetPidLooptime;
FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT]; 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 dT;
static FAST_RAM_ZERO_INIT float pidFrequency; 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); PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
#ifdef STM32F10X #ifdef STM32F10X
@ -96,22 +109,19 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
#endif // USE_ACRO_TRAINER #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) void resetPidProfile(pidProfile_t *pidProfile)
{ {
RESET_CONFIG(pidProfile_t, pidProfile, RESET_CONFIG(pidProfile_t, pidProfile,
.pid = { .pid = {
[PID_ROLL] = { 46, 45, 25 }, [PID_ROLL] = { 46, 45, 25, 60 },
[PID_PITCH] = { 50, 50, 27 }, [PID_PITCH] = { 50, 50, 27, 60 },
[PID_YAW] = { 65, 45, 0 }, [PID_YAW] = { 65, 45, 0 , 60 },
[PID_ALT] = { 50, 0, 0 }, [PID_LEVEL] = { 50, 50, 75, 0 },
[PID_POS] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100, [PID_MAG] = { 40, 0, 0, 0 },
[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 }
}, },
.pidSumLimit = PIDSUM_LIMIT, .pidSumLimit = PIDSUM_LIMIT,
@ -126,11 +136,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.vbatPidCompensation = 0, .vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON, .pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55, .levelAngleLimit = 55,
.setpointRelaxRatio = 0, .feedForwardTransition = 0,
.dtermSetpointWeight = 60,
.yawRateAccelLimit = 100, .yawRateAccelLimit = 100,
.rateAccelLimit = 0, .rateAccelLimit = 0,
.itermThrottleThreshold = 350, .itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000, .itermAcceleratorGain = 5000,
.crash_time = 500, // ms .crash_time = 500, // ms
.crash_delay = 0, // ms .crash_delay = 0, // ms
@ -158,6 +167,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_gain = 0, .abs_control_gain = 0,
.abs_control_limit = 90, .abs_control_limit = 90,
.abs_control_error_limit = 20, .abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
); );
} }
@ -182,9 +192,9 @@ void pidSetItermAccelerator(float newItermAccelerator)
itermAccelerator = newItermAccelerator; itermAccelerator = newItermAccelerator;
} }
float pidItermAccelerator(void) bool pidOsdAntiGravityActive(void)
{ {
return itermAccelerator; return (itermAccelerator > antiGravityOsdCutoff);
} }
void pidStabilisationState(pidStabilisationState_e pidControllerState) void pidStabilisationState(pidStabilisationState_e pidControllerState)
@ -200,11 +210,11 @@ typedef union dtermLowpass_u {
} dtermLowpass_t; } dtermLowpass_t;
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; 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 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 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 filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
@ -215,16 +225,18 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif #endif
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2]; static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2]; static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized; static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
void pidInitFilters(const pidProfile_t *pidProfile) 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) { if (targetPidLooptime == 0) {
// no looptime set, so set all the filters to null // 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) { if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); 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); biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
} }
} else { } else {
@ -262,7 +274,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermLowpass2ApplyFn = nullFilterApply; dtermLowpass2ApplyFn = nullFilterApply;
} else { } else {
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; 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)); pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
} }
} }
@ -276,13 +288,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
break; break;
case FILTER_PT1: case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; 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)); pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
} }
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; 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); biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
} }
break; break;
@ -306,6 +318,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
} }
#endif #endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
@ -315,7 +329,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
rcSmoothingFilterType = filterType; rcSmoothingFilterType = filterType;
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
setpointDerivativeLpfInitialized = true; setpointDerivativeLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (rcSmoothingFilterType) { switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1: case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
@ -331,7 +345,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff) void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
{ {
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { 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) { switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1: case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
@ -349,12 +363,12 @@ typedef struct pidCoefficient_s {
float Kp; float Kp;
float Ki; float Ki;
float Kd; float Kd;
float Kf;
} pidCoefficient_t; } pidCoefficient_t;
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3]; static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float maxVelocity[3]; static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float relaxFactor; static FAST_RAM_ZERO_INIT float feedForwardTransition;
static FAST_RAM_ZERO_INIT float dtermSetpointWeight;
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
static FAST_RAM_ZERO_INIT float ITermWindupPointInv; static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode; 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; static FAST_RAM_ZERO_INIT float acroTrainerGain;
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
void pidUpdateAntiGravityThrottleFilter(float throttle)
{
if (antiGravityMode) {
antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
}
}
void pidInitConfig(const pidProfile_t *pidProfile) 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++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P; pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I; pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D; 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; levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f; horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D; horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
@ -426,6 +447,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000; crashTimeLimitUs = pidProfile->crash_time * 1000;
crashTimeDelayUs = pidProfile->crash_delay * 1000; crashTimeDelayUs = pidProfile->crash_delay * 1000;
crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10; crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
@ -439,6 +461,18 @@ void pidInitConfig(const pidProfile_t *pidProfile)
throttleBoost = pidProfile->throttle_boost * 0.1f; throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif #endif
itermRotation = pidProfile->iterm_rotation; 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) #if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward; smartFeedforward = pidProfile->smart_feedforward;
#endif #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 accelerationLimit(int axis, float currentPidSetpoint)
{ {
static float previousSetpoint[3]; static float previousSetpoint[XYZ_AXIS_COUNT];
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis]; const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity[axis]) { if (ABS(currentVelocity) > maxVelocity[axis]) {
@ -668,7 +702,7 @@ static void rotateITermAndAxisError()
#endif #endif
) { ) {
const float gyroToAngle = dT * RAD; const float gyroToAngle = dT * RAD;
float rotationRads[3]; float rotationRads[XYZ_AXIS_COUNT];
for (int i = FD_ROLL; i <= FD_YAW; i++) { for (int i = FD_ROLL; i <= FD_YAW; i++) {
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle; rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
} }
@ -678,7 +712,7 @@ static void rotateITermAndAxisError()
} }
#endif #endif
if (itermRotation) { if (itermRotation) {
float v[3]; float v[XYZ_AXIS_COUNT];
for (int i = 0; i < XYZ_AXIS_COUNT; i++) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
v[i] = pidData[i].I; v[i] = pidData[i].I;
} }
@ -764,12 +798,51 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
} }
#endif // USE_ACRO_TRAINER #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. // 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) // Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{ {
static float previousGyroRateDterm[2]; static float previousGyroRateDterm[XYZ_AXIS_COUNT];
static float previousPidSetpoint[2]; static float previousPidSetpoint[XYZ_AXIS_COUNT];
const float tpaFactor = getThrottlePIDAttenuation(); const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
@ -779,15 +852,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#endif #endif
// Dynamic i component, // 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 // gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator; 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 // Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[2]; float gyroRateDterm[XYZ_AXIS_COUNT];
for (int axis = FD_ROLL; axis < FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]); gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]); gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[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---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) { if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // 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); 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. ---------- // --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term. // 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 // -----calculate P component and add Dynamic Part based on stick input
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor; 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 // -----calculate D component
if (axis != FD_YAW) { if (pidCoefficient[axis].Kd > 0) {
// no transition if relaxFactor == 0
float transition = relaxFactor > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * relaxFactor) : 1;
// Divide rate change by dT to get differential (ie dr/dt). // Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time // 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); detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; 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]; float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
if (axis == rcSmoothingDebugAxis) { pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
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));
}
}
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
const float pidFeedForward = pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency;
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
bool addFeedforward = true; applySmartFeedforward(axis);
if (smartFeedforward) {
if (pidData[axis].P * pidFeedForward > 0) {
if (ABS(pidFeedForward) > ABS(pidData[axis].P)) {
pidData[axis].P = 0;
} else {
addFeedforward = false;
}
}
}
if (addFeedforward)
#endif #endif
{ } else {
pidData[axis].D += pidFeedForward; pidData[axis].F = 0;
} }
previousGyroRateDterm[axis] = gyroRateDterm[axis]; previousPidSetpoint[axis] = currentPidSetpoint;
previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_YAW_SPIN_RECOVERY #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 // zero PIDs on pitch and roll leaving yaw P to correct spin
pidData[axis].P = 0; pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 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 #endif // USE_YAW_SPIN_RECOVERY
// YAW has no D // calculating the PID sum
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I; 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 // 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 // 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].P = 0;
pidData[axis].I = 0; pidData[axis].I = 0;
pidData[axis].D = 0; pidData[axis].D = 0;
pidData[axis].F = 0;
pidData[axis].Sum = 0; pidData[axis].Sum = 0;
} }
@ -1028,3 +1082,17 @@ void pidSetAcroTrainerState(bool newState)
} }
} }
#endif // USE_ACRO_TRAINER #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 ITERM_SCALE 0.244381f
#define DTERM_SCALE 0.000529f #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 { typedef enum {
PID_ROLL, PID_ROLL,
PID_PITCH, PID_PITCH,
PID_YAW, PID_YAW,
PID_ALT,
PID_POS,
PID_POSR,
PID_NAVR,
PID_LEVEL, PID_LEVEL,
PID_MAG, PID_MAG,
PID_VEL,
PID_ITEM_COUNT PID_ITEM_COUNT
} pidIndex_e; } pidIndex_e;
@ -70,11 +69,17 @@ typedef enum {
PID_CRASH_RECOVERY_BEEP PID_CRASH_RECOVERY_BEEP
} pidCrashRecovery_e; } pidCrashRecovery_e;
typedef struct pid8_s { typedef struct pidf_s {
uint8_t P; uint8_t P;
uint8_t I; uint8_t I;
uint8_t D; uint8_t D;
} pid8_t; uint16_t F;
} pidf_t;
typedef enum {
ANTI_GRAVITY_SMOOTH,
ANTI_GRAVITY_STEP
} antiGravityMode_e;
typedef enum { typedef enum {
ITERM_RELAX_OFF, ITERM_RELAX_OFF,
@ -95,7 +100,7 @@ typedef struct pidProfile_s {
uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff 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 dterm_filter_type; // Filter selection for dterm
uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation 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 uint8_t horizon_tilt_expert_mode; // OFF or ON
// Betaflight PID controller parameters // 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 itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit 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 yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
uint16_t crash_dthreshold; // dterm crash value 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_angle; // degrees
uint8_t crash_recovery_rate; // degree/second uint8_t crash_recovery_rate; // degree/second
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage 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 crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit; uint16_t itermLimit;
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz 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 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; // 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 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 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 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_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_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 uint8_t iterm_relax; // Enable iterm suppression during stick input
@ -162,10 +167,13 @@ typedef struct pidAxisData_s {
float P; float P;
float I; float I;
float D; float D;
float F;
float Sum; float Sum;
} pidAxisData_t; } pidAxisData_t;
extern const char pidNames[];
extern pidAxisData_t pidData[3]; extern pidAxisData_t pidData[3];
extern uint32_t targetPidLooptime; extern uint32_t targetPidLooptime;
@ -176,7 +184,6 @@ extern pt1Filter_t throttleLpf;
void pidResetITerm(void); void pidResetITerm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState); void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator); void pidSetItermAccelerator(float newItermAccelerator);
float pidItermAccelerator(void);
void pidInitFilters(const pidProfile_t *pidProfile); void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile);
void pidInit(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile);
@ -186,3 +193,8 @@ void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState); void pidSetAcroTrainerState(bool newState);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType); void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff); 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 #ifndef USE_OSD_SLAVE
static const char pidnames[] =
"ROLL;"
"PITCH;"
"YAW;"
"ALT;"
"Pos;"
"PosR;"
"NavR;"
"LEVEL;"
"MAG;"
"VEL;";
typedef enum { typedef enum {
MSP_SDCARD_STATE_NOT_PRESENT = 0, MSP_SDCARD_STATE_NOT_PRESENT = 0,
MSP_SDCARD_STATE_FATAL = 1, MSP_SDCARD_STATE_FATAL = 1,
@ -984,7 +972,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
break; break;
case MSP_PIDNAMES: case MSP_PIDNAMES:
for (const char *c = pidnames; *c; c++) { for (const char *c = pidNames; *c; c++) {
sbufWriteU8(dst, *c); sbufWriteU8(dst, *c);
} }
break; break;
@ -1293,8 +1281,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, currentPidProfile->vbatPidCompensation); sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio); sbufWriteU8(dst, currentPidProfile->feedForwardTransition);
sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255)); sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved 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 sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, currentPidProfile->iterm_rotation); sbufWriteU8(dst, currentPidProfile->iterm_rotation);
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
sbufWriteU8(dst, currentPidProfile->smart_feedforward); sbufWriteU8(dst, currentPidProfile->smart_feedforward);
@ -1333,6 +1321,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #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; break;
case MSP_SENSOR_CONFIG: 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 sbufReadU16(src); // was pidProfile.yaw_p_limit
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
currentPidProfile->vbatPidCompensation = sbufReadU8(src); currentPidProfile->vbatPidCompensation = sbufReadU8(src);
currentPidProfile->setpointRelaxRatio = sbufReadU8(src); currentPidProfile->feedForwardTransition = sbufReadU8(src);
currentPidProfile->dtermSetpointWeight = sbufReadU8(src); sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
sbufReadU8(src); // reserved 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); currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
} }
if (sbufBytesRemaining(src) >= 2) { 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 // Added in MSP API 1.40
currentPidProfile->iterm_rotation = sbufReadU8(src); currentPidProfile->iterm_rotation = sbufReadU8(src);
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
@ -1883,6 +1876,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#else #else
sbufReadU8(src); sbufReadU8(src);
#endif #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); 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 }, { BOXHEADFREE, "HEADFREE", 6 },
{ BOXHEADADJ, "HEADADJ", 7 }, { BOXHEADADJ, "HEADADJ", 7 },
{ BOXCAMSTAB, "CAMSTAB", 8 }, { BOXCAMSTAB, "CAMSTAB", 8 },
{ BOXCAMTRIG, "CAMTRIG", 9 }, // { BOXCAMTRIG, "CAMTRIG", 9 },
{ BOXGPSHOME, "GPS HOME", 10 }, { BOXGPSHOME, "GPS HOME", 10 },
{ BOXGPSHOLD, "GPS HOLD", 11 }, { BOXGPSHOLD, "GPS HOLD", 11 },
{ BOXPASSTHRU, "PASSTHRU", 12 }, { BOXPASSTHRU, "PASSTHRU", 12 },
{ BOXBEEPERON, "BEEPER", 13 }, { BOXBEEPERON, "BEEPER", 13 },
{ BOXLEDMAX, "LEDMAX", 14 }, // { BOXLEDMAX, "LEDMAX", 14 }, (removed)
{ BOXLEDLOW, "LEDLOW", 15 }, { BOXLEDLOW, "LEDLOW", 15 },
{ BOXLLIGHTS, "LLIGHTS", 16 }, // { BOXLLIGHTS, "LLIGHTS", 16 }, (removed)
{ BOXCALIB, "CALIB", 17 }, { BOXCALIB, "CALIB", 17 },
{ BOXGOV, "GOVERNOR", 18 }, // { BOXGOV, "GOVERNOR", 18 }, (removed)
{ BOXOSD, "OSD DISABLE SW", 19 }, { BOXOSD, "OSD DISABLE SW", 19 },
{ BOXTELEMETRY, "TELEMETRY", 20 }, { BOXTELEMETRY, "TELEMETRY", 20 },
{ BOXGTUNE, "GTUNE", 21 }, // { BOXGTUNE, "GTUNE", 21 }, (removed)
{ BOXRANGEFINDER, "RANGEFINDER", 22 }, // { BOXRANGEFINDER, "RANGEFINDER", 22 }, (removed)
{ BOXSERVO1, "SERVO1", 23 }, { BOXSERVO1, "SERVO1", 23 },
{ BOXSERVO2, "SERVO2", 24 }, { BOXSERVO2, "SERVO2", 24 },
{ BOXSERVO3, "SERVO3", 25 }, { BOXSERVO3, "SERVO3", 25 },
@ -198,12 +198,6 @@ void initActiveBoxIds(void)
} }
#endif #endif
#ifdef USE_RANGEFINDER
if (feature(FEATURE_RANGEFINDER)) { // XXX && sensors(SENSOR_RANGEFINDER)?
BME(BOXRANGEFINDER);
}
#endif
BME(BOXFAILSAFE); BME(BOXFAILSAFE);
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) { 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", "BIQUAD",
}; };
static const char * const lookupTableAntiGravityMode[] = {
"SMOOTH",
"STEP",
};
static const char * const lookupTableFailsafe[] = { static const char * const lookupTableFailsafe[] = {
"AUTO-LAND", "DROP", "GPS-RESCUE" "AUTO-LAND", "DROP", "GPS-RESCUE"
@ -413,6 +417,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels), LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
LOOKUP_TABLE_ENTRY(lookupTableLowpassType), LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType), LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode),
LOOKUP_TABLE_ENTRY(lookupTableFailsafe), LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode), LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode),
LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery), 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) }, { "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) }, { "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) }, { "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_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) }, { "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) }, { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) },
{ "dterm_setpoint_weight", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
{ "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "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) }, { "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) }, { "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) #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", 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_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 #endif
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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) }, { "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_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE, TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE, TABLE_DTERM_LOWPASS_TYPE,
TABLE_ANTI_GRAVITY_MODE,
TABLE_FAILSAFE, TABLE_FAILSAFE,
TABLE_FAILSAFE_SWITCH_MODE, TABLE_FAILSAFE_SWITCH_MODE,
TABLE_CRASH_RECOVERY, TABLE_CRASH_RECOVERY,

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

@ -282,7 +282,7 @@ static void osdFormatAltitudeString(char * buff, int altitude)
buff[4] = '.'; 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); 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: case OSD_ANTI_GRAVITY:
{ {
if (pidItermAccelerator() > 1.0f) { if (pidOsdAntiGravityActive()) {
strcpy(buff, "AG"); 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_offset = 0,
.rssi_invert = 0, .rssi_invert = 0,
.rcInterpolation = RC_SMOOTHING_AUTO, .rcInterpolation = RC_SMOOTHING_AUTO,
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPT, .rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT,
.rcInterpolationInterval = 19, .rcInterpolationInterval = 19,
.fpvCamAngleDegrees = 0, .fpvCamAngleDegrees = 0,
.airModeActivateThreshold = 32, .airModeActivateThreshold = 32,

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

@ -520,6 +520,7 @@ bool gyroInit(void)
switch (debugMode) { switch (debugMode) {
case DEBUG_FFT: case DEBUG_FFT:
case DEBUG_FFT_FREQ:
case DEBUG_GYRO_RAW: case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED: case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED: case DEBUG_GYRO_FILTERED:
@ -1042,12 +1043,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
return; return;
} }
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
}
#endif
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs; accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs; accumulatedMeasurementTimeUs += sampleDeltaUs;
@ -1069,6 +1064,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
} else { } else {
filterGyroDebug(gyroSensor, sampleDeltaUs); filterGyroDebug(gyroSensor, sampleDeltaUs);
} }
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(gyroSensor->notchFilterDyn);
}
#endif
} }
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) 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)); GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data 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); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
} }
} }
#endif #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. // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); 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/>. * 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 <stdint.h>
#include "platform.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 // 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 // Eg [0,31), [31,62), [62, 93) etc
#define FFT_WINDOW_SIZE 32 // max for f3 targets #define FFT_WINDOW_SIZE 32 // max for f3 targets
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
#define FFT_MIN_FREQ 100 // not interested in filtering frequencies below 100Hz #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 1000 // allows analysis up to 500Hz which is more than motors create #define FFT_SAMPLING_RATE_HZ 1333 // analyse up to 666Hz, 16 bins each 41.655z wide
#define FFT_MAX_FREQUENCY (FFT_SAMPLING_RATE / 2) // nyquist rate #define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) // centre frequency of bandpass that constrains input to FFT
#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies #define FFT_BIQUAD_Q 0.05f // bandpass quality factor, 0.1 for steep sided bandpass
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE / FFT_WINDOW_SIZE) // hz per bin #define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin
#define DYN_NOTCH_WIDTH 100 // just an orientation and start value #define FFT_MIN_BIN_RISE 2 // following bin must be at least 2 times previous to indicate start of peak
#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse... #define DYN_NOTCH_SMOOTH_FREQ_HZ 60 // lowpass frequency for smoothing notch centre point
#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies #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_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies) #define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis #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 fftSamplingCount;
static FAST_RAM_ZERO_INIT uint16_t fftSamplingScale;
// gyro data used for frequency analysis // gyro data used for frequency analysis
static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; 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 // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; 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); arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
initGyroData(); 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 // 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 // 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 // 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++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftResult[axis].centerFreq = 200; // any init value fftResult[axis].centerFreq = 200; // any init value
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime); biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF); 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]; 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 * 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 // 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 fftAccCount;
static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks; static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks;
// if gyro sampling is > 1kHz, accumulate multiple samples // samples should have been pushed by `gyroDataAnalysePush`
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // if gyro sampling is > 1kHz, accumulate multiple samples
fftAcc[axis] += gyroDev->gyroADC[axis];
}
fftAccCount++; fftAccCount++;
// this runs at 1kHz // this runs at 1kHz
if (fftAccCount == fftSamplingScale) { if (fftAccCount == fftSamplingCount) {
fftAccCount = 0; fftAccCount = 0;
//calculate mean value of accumulated samples //calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = fftAcc[axis] / fftSamplingScale; float sample = fftAcc[axis] / fftSamplingCount;
sample = biquadFilterApply(&fftGyroFilter[axis], sample); sample = biquadFilterApply(&fftGyroFilter[axis], sample);
gyroData[axis][fftIdx] = sample; gyroData[axis][fftIdx] = sample;
if (axis == 0) if (axis == 0)
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale)); DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
fftAcc[axis] = 0; fftAcc[axis] = 0;
} }
@ -244,37 +252,42 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
case STEP_CALC_FREQUENCIES: case STEP_CALC_FREQUENCIES:
{ {
// 13us // 13us
// calculate FFT centreFreq
float fftSum = 0; float fftSum = 0;
float fftWeightedSum = 0; float fftWeightedSum = 0;
fftResult[axis].maxVal = 0; fftResult[axis].maxVal = 0;
bool fftIncreasing = false;
float cubedData;
// iterate over fft data and calculate weighted indexes // iterate over fft data and calculate weighted indexes
float squaredData; for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) {
for (int i = 0; i < FFT_BIN_COUNT; i++) { if (!fftIncreasing && (fftData[i] < fftData[i-1] * FFT_MIN_BIN_RISE)) {
squaredData = fftData[i] * fftData[i]; //more weight on higher peaks // do nothing unless has increased at some point
fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData); } else {
fftSum += squaredData; cubedData = fftData[i] * fftData[i] * fftData[i]; //more weight on higher peaks
fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0 if (!fftIncreasing){
} cubedData += fftData[i-1] * fftData[i-1] * fftData[i-1]; //add previous bin before first rise
}
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) fftSum += cubedData;
if (fftSum > 0) { fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0
// idx was shifted by 1 to start at 1, not 0 fftIncreasing = true;
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));
} }
} }
// 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_FREQ, axis, fftResult[axis].centerFreq);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
@ -282,8 +295,8 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
case STEP_UPDATE_FILTERS: case STEP_UPDATE_FILTERS:
{ {
// 7us // 7us
// calculate new filter coefficients // calculate cutoffFreq and notch Q, update notch filter
float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF); float cutoffFreq = fmax(fftResult[axis].centerFreq * DYN_NOTCH_CUTOFF, DYN_NOTCH_MIN_CUTOFF_HZ);
float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq); float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq);
biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); 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); void gyroDataAnalyseInit(uint32_t targetLooptime);
const gyroFftData_t *gyroFftData(int axis); const gyroFftData_t *gyroFftData(int axis);
struct gyroDev_s; 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); void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);

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

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

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

@ -28,13 +28,13 @@
//#undef USE_GYRO_OVERFLOW_CHECK //#undef USE_GYRO_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2 //#undef USE_GYRO_LPF2
//#undef USE_ITERM_RELAX #undef USE_ITERM_RELAX
//#undef USE_RC_SMOOTHING_FILTER #undef USE_RC_SMOOTHING_FILTER
//#undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM #undef USE_TELEMETRY_LTM
//#undef USE_SERIALRX_XBUS #undef USE_SERIALRX_XBUS
#undef USE_BOARD_INFO #undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS #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 }, { BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 }, { BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 }, { BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXCAMTRIG, "CAMTRIG;", 9 },
{ BOXGPSHOME, "GPS HOME;", 10 }, { BOXGPSHOME, "GPS HOME;", 10 },
{ BOXGPSHOLD, "GPS HOLD;", 11 }, { BOXGPSHOLD, "GPS HOLD;", 11 },
{ BOXPASSTHRU, "PASSTHRU;", 12 }, { BOXPASSTHRU, "PASSTHRU;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 }, { BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDMAX, "LEDMAX;", 14 },
{ BOXLEDLOW, "LEDLOW;", 15 }, { BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLLIGHTS, "LLIGHTS;", 16 },
{ BOXCALIB, "CALIB;", 17 }, { BOXCALIB, "CALIB;", 17 },
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD DISABLE SW;", 19 }, { BOXOSD, "OSD DISABLE SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 }, { BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXGTUNE, "GTUNE;", 21 },
{ BOXRANGEFINDER, "RANGEFINDER;", 22 },
{ BOXSERVO1, "SERVO1;", 23 }, { BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 }, { BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 }, { BOXSERVO3, "SERVO3;", 25 },
@ -305,20 +299,14 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | 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_HOME_MODE)) << BOXGPSHOME |
IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | 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(BOXLEDLOW)) << BOXLEDLOW |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | 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(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | 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(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; 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(BARO_MODE)) << 3 |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 |
IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << 6 |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7; IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterStartBuffer(PUBLIC_ADDRESS);

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

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

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

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

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

@ -23,6 +23,25 @@
#define TARGET_BOARD_IDENTIFIER "IMF3" #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 ENABLE_DSHOT_DMAR true
#define LED0_PIN PB7 #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].P = 30;
pidProfile->pid[PID_LEVEL].D = 30; pidProfile->pid[PID_LEVEL].D = 30;
pidProfile->dtermSetpointWeight = 200; pidProfile->pid[PID_PITCH].F = 200;
pidProfile->setpointRelaxRatio = 50; pidProfile->pid[PID_ROLL].F = 200;
pidProfile->feedForwardTransition = 50;
} }
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { 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_ITERM_RELAX
#undef USE_RC_SMOOTHING_FILTER #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_HOTT
#undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM #undef USE_TELEMETRY_LTM

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

@ -23,7 +23,25 @@
#define TARGET_BOARD_IDENTIFIER "SIRF" #define TARGET_BOARD_IDENTIFIER "SIRF"
// Removed to make the firmware fit into flash (in descending order of priority): // 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_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME #undef USE_RTC_TIME
#undef USE_RX_MSP #undef USE_RX_MSP

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

@ -36,14 +36,22 @@
#undef USE_RC_SMOOTHING_FILTER #undef USE_RC_SMOOTHING_FILTER
#undef ITERM_RELAX #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_HOTT
#undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM #undef USE_TELEMETRY_LTM
#undef USE_SERIALRX_XBUS #undef USE_SERIALRX_XBUS
#undef USE_BOARD_INFO #undef USE_BOARD_INFO
#endif
#undef USE_EXTENDED_CMS_MENUS #undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME #undef USE_RTC_TIME
#undef USE_RX_MSP #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_OVERFLOW_CHECK
//#undef USE_GYRO_LPF2 //#undef USE_GYRO_LPF2
//#undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_HOTT
//#undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_MAVLINK
//#undef USE_TELEMETRY_LTM #undef USE_TELEMETRY_LTM
//#undef USE_SERIALRX_XBUS #undef USE_SERIALRX_XBUS
//#undef USE_BOARD_INFO #undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS #undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME #undef USE_RTC_TIME
#undef USE_RX_MSP #undef USE_RX_MSP

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

@ -491,12 +491,15 @@ void mavlinkSendHUDAndHeartbeat(void)
mavCustomMode = 0; //Stabilize mavCustomMode = 0; //Stabilize
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
} }
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) if (FLIGHT_MODE(BARO_MODE)) {
mavCustomMode = 2; //Alt Hold mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(GPS_HOME_MODE)) }
if (FLIGHT_MODE(GPS_HOME_MODE)) {
mavCustomMode = 6; //Return to Launch mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(GPS_HOLD_MODE)) }
if (FLIGHT_MODE(GPS_HOLD_MODE)) {
mavCustomMode = 16; //Position Hold (Earlier called Hybrid) mavCustomMode = 16; //Position Hold (Earlier called Hybrid)
}
uint8_t mavSystemState = 0; uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) { 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 { } else {
tmpi += 2; tmpi += 2;
} }
if (ARMING_FLAG(ARMED)) if (ARMING_FLAG(ARMED)) {
tmpi += 4; tmpi += 4;
}
if (FLIGHT_MODE(ANGLE_MODE)) if (FLIGHT_MODE(ANGLE_MODE)) {
tmpi += 10; tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE)) }
if (FLIGHT_MODE(HORIZON_MODE)) {
tmpi += 20; tmpi += 20;
if (FLIGHT_MODE(UNUSED_MODE)) }
tmpi += 40; if (FLIGHT_MODE(PASSTHRU_MODE)) {
if (FLIGHT_MODE(PASSTHRU_MODE))
tmpi += 40; tmpi += 40;
}
if (FLIGHT_MODE(MAG_MODE)) if (FLIGHT_MODE(MAG_MODE)) {
tmpi += 100; tmpi += 100;
if (FLIGHT_MODE(BARO_MODE)) }
if (FLIGHT_MODE(BARO_MODE)) {
tmpi += 200; tmpi += 200;
if (FLIGHT_MODE(RANGEFINDER_MODE)) }
tmpi += 400;
if (FLIGHT_MODE(GPS_HOLD_MODE)) if (FLIGHT_MODE(GPS_HOLD_MODE)) {
tmpi += 1000; tmpi += 1000;
if (FLIGHT_MODE(GPS_HOME_MODE)) }
if (FLIGHT_MODE(GPS_HOME_MODE)) {
tmpi += 2000; tmpi += 2000;
if (FLIGHT_MODE(HEADFREE_MODE)) }
if (FLIGHT_MODE(HEADFREE_MODE)) {
tmpi += 4000; tmpi += 4000;
}
smartPortSendPackage(id, (uint32_t)tmpi); smartPortSendPackage(id, (uint32_t)tmpi);
*clearToSend = false; *clearToSend = false;

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

@ -850,4 +850,5 @@ extern "C" {
void rescheduleTask(cfTaskId_e, uint32_t) {} void rescheduleTask(cfTaskId_e, uint32_t) {}
bool usbCableIsInserted(void) { return false; } bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(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; 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) { void setDefaultTestSettings(void) {
pgResetAll(); pgResetAll();
pidProfile = pidProfilesMutable(1); pidProfile = pidProfilesMutable(1);
pidProfile->pid[PID_ROLL] = { 40, 40, 30 }; pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 };
pidProfile->pid[PID_PITCH] = { 58, 50, 35 }; pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 };
pidProfile->pid[PID_YAW] = { 70, 45, 0 }; pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75 }; pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 };
pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW; pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
@ -101,11 +101,11 @@ void setDefaultTestSettings(void) {
pidProfile->vbatPidCompensation = 0; pidProfile->vbatPidCompensation = 0;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55; pidProfile->levelAngleLimit = 55;
pidProfile->setpointRelaxRatio = 100; pidProfile->feedForwardTransition = 100;
pidProfile->dtermSetpointWeight = 0;
pidProfile->yawRateAccelLimit = 100; pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0; pidProfile->rateAccelLimit = 0;
pidProfile->itermThrottleThreshold = 350; pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
pidProfile->itermThrottleThreshold = 250;
pidProfile->itermAcceleratorGain = 1000; pidProfile->itermAcceleratorGain = 1000;
pidProfile->crash_time = 500; pidProfile->crash_time = 500;
pidProfile->crash_delay = 0; pidProfile->crash_delay = 0;
@ -266,7 +266,7 @@ TEST(pidControllerTest, testPidLoop) {
ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7)); 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_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].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 // Match the stick to gyro to stop error
simulatedSetpointRate[FD_ROLL] = 100; 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) {} void rescheduleTask(cfTaskId_e, uint32_t) {}
bool usbCableIsInserted(void) { return false; } bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; } bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
} }