Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Angle and Horizon Mode improvements (#12231)

Browse source 
* Angle and Horizon Update for 4.5

* BugFix FF noise Angle Mode on yaw and in level _race mode

* use time constant in ms for angle feedforward smoothing

* refactor to remove unnecessary definition

---------

Co-authored-by: ChrisRosser <chrisrosser91@gmail.com>
This commit is contained in:
ctzsnooze 2023-03-15 09:46:24 +11:00 • committed by GitHub
parent d6ed53ecdf
commit 241e9a9b94
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
16 changed files with 482 additions and 271 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/blackbox/blackbox.c
View file

@ -1402,7 +1402,14 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, "%d", currentPidProfile->feedforward_jitter_factor); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, "%d", currentPidProfile->feedforward_jitter_factor);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_BOOST, "%d", currentPidProfile->feedforward_boost); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_BOOST, "%d", currentPidProfile->feedforward_boost);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FEEDFORWARD, "%d", currentPidProfile->pid[PID_LEVEL].F);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FF_SMOOTHING_MS, "%d", currentPidProfile->angle_feedforward_smoothing_ms);
#endif #endif
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_ROLL_EXPO, "%d", currentControlRateProfile->levelExpo[ROLL]);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_PITCH_EXPO, "%d", currentControlRateProfile->levelExpo[PITCH]);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_LIMIT, "%d", currentPidProfile->angle_limit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_EARTH_REF, "%d", currentPidProfile->angle_earth_ref);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_LIMIT_DEGREES, "%d", currentPidProfile->horizon_limit_degrees);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit);

3
src/main/build/debug.c
View file

@ -109,4 +109,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"GPS_DOP", "GPS_DOP",
"FAILSAFE", "FAILSAFE",
"GYRO_CALIBRATION", "GYRO_CALIBRATION",
"ANGLE_MODE",
"ANGLE_TARGET",
"CURRENT_ANGLE",
}; };

3
src/main/build/debug.h
View file

@ -107,6 +107,9 @@ typedef enum {
DEBUG_GPS_DOP, DEBUG_GPS_DOP,
DEBUG_FAILSAFE, DEBUG_FAILSAFE,
DEBUG_GYRO_CALIBRATION, DEBUG_GYRO_CALIBRATION,
DEBUG_ANGLE_MODE,
DEBUG_ANGLE_TARGET,
DEBUG_CURRENT_ANGLE,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

20
src/main/cli/settings.c
View file

@ -979,8 +979,8 @@ const clivalue_t valueTable[] = {
{ "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) },
{ "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) }, { "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) },
{ "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) }, { "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) },
{ "roll_level_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_ROLL]) }, { PARAM_NAME_ANGLE_ROLL_EXPO, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_ROLL]) },
{ "pitch_level_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_PITCH]) }, { PARAM_NAME_ANGLE_PITCH_EXPO, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_PITCH]) },
// PG_SERIAL_CONFIG // PG_SERIAL_CONFIG
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },
@ -1132,14 +1132,16 @@ const clivalue_t valueTable[] = {
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
{ "f_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, F_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].F) }, { "f_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, F_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].F) },
{ "angle_level_strength", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, { PARAM_NAME_ANGLE_P_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
{ "horizon_level_strength", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, { PARAM_NAME_ANGLE_FEEDFORWARD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].F) },
{ "horizon_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, { PARAM_NAME_ANGLE_FF_SMOOTHING_MS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_feedforward_smoothing_ms) },
{ PARAM_NAME_ANGLE_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 85 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_limit) },
{ PARAM_NAME_ANGLE_EARTH_REF, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_earth_ref) },
{ "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, { PARAM_NAME_HORIZON_LEVEL_STRENGTH, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
{ PARAM_NAME_HORIZON_LIMIT_STICKS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
{ "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, { PARAM_NAME_HORIZON_LIMIT_DEGREES, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_limit_degrees) },
{ "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) }, { PARAM_NAME_HORIZON_IGNORE_STICKS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) },
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
{ PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) }, { PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },

41
src/main/cms/cms_menu_imu.c
View file

@ -516,10 +516,14 @@ static CMS_Menu cmsx_menuLaunchControl = {
}; };
#endif #endif
static uint8_t cmsx_angleStrength; static uint8_t cmsx_angleP;
static uint8_t cmsx_angleFF;
static uint8_t cmsx_angleLimit;
static uint8_t cmsx_horizonStrength; static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition; static uint8_t cmsx_horizonLimitSticks;
static uint8_t cmsx_levelAngleLimit; static uint8_t cmsx_horizonLimitDegrees;
static uint8_t cmsx_throttleBoost; static uint8_t cmsx_throttleBoost;
static uint8_t cmsx_thrustLinearization; static uint8_t cmsx_thrustLinearization;
static uint8_t cmsx_antiGravityGain; static uint8_t cmsx_antiGravityGain;
@ -560,10 +564,13 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P; cmsx_angleP = pidProfile->pid[PID_LEVEL].P;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I; cmsx_angleFF = pidProfile->pid[PID_LEVEL].F;
cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D; cmsx_angleLimit = pidProfile->angle_limit;
cmsx_levelAngleLimit = pidProfile->levelAngleLimit;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
cmsx_horizonLimitSticks = pidProfile->pid[PID_LEVEL].D;
cmsx_horizonLimitDegrees = pidProfile->horizon_limit_degrees;
cmsx_antiGravityGain = pidProfile->anti_gravity_gain; cmsx_antiGravityGain = pidProfile->anti_gravity_gain;
@ -611,10 +618,13 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidInitConfig(currentPidProfile); pidInitConfig(currentPidProfile);
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength; pidProfile->pid[PID_LEVEL].P = cmsx_angleP;
pidProfile->pid[PID_LEVEL].F = cmsx_angleFF;
pidProfile->angle_limit = cmsx_angleLimit;
pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength; pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition; pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks;
pidProfile->levelAngleLimit = cmsx_levelAngleLimit; pidProfile->horizon_limit_degrees = cmsx_horizonLimitDegrees;
pidProfile->anti_gravity_gain = cmsx_antiGravityGain; pidProfile->anti_gravity_gain = cmsx_antiGravityGain;
@ -665,10 +675,13 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_jitter_factor, 0, 20, 1 } }, { "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_jitter_factor, 0, 20, 1 } },
{ "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_boost, 0, 50, 1 } }, { "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_boost, 0, 50, 1 } },
#endif #endif
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } }, { "ANGLE P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleP, 0, 200, 1 } },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } }, { "ANGLE FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleFF, 0, 200, 1 } },
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } }, { "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleLimit, 10, 90, 1 } },
{ "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_levelAngleLimit, 10, 90, 1 } },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 100, 1 } },
{ "HORZN LIM_STK", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonLimitSticks, 10, 200, 1 } },
{ "HORZN LIM_DEG", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonLimitDegrees, 10, 250, 1 } },
{ "AG GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_antiGravityGain, ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 1, 100 } }, { "AG GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_antiGravityGain, ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 1, 100 } },
#ifdef USE_THROTTLE_BOOST #ifdef USE_THROTTLE_BOOST

4
src/main/fc/controlrate_profile.c
View file

@ -62,8 +62,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
.profileName = { 0 }, .profileName = { 0 },
.quickRatesRcExpo = 0, .quickRatesRcExpo = 0,
.levelExpo[FD_ROLL] = 0, .levelExpo[FD_ROLL] = 33,
.levelExpo[FD_PITCH] = 0, .levelExpo[FD_PITCH] = 33,
); );
} }
} }

12
src/main/fc/parameter_names.h
View file

@ -124,6 +124,18 @@
#define PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO "altitude_prefer_baro" #define PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO "altitude_prefer_baro"
#define PARAM_NAME_POSITION_ALTITUDE_LPF "altitude_lpf" #define PARAM_NAME_POSITION_ALTITUDE_LPF "altitude_lpf"
#define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf" #define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf"
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
#define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
#define PARAM_NAME_ANGLE_ROLL_EXPO "angle_roll_expo"
#define PARAM_NAME_ANGLE_PITCH_EXPO "angle_pitch_expo"
#define PARAM_NAME_ANGLE_LIMIT "angle_limit"
#define PARAM_NAME_ANGLE_P_GAIN "angle_p_gain"
#define PARAM_NAME_ANGLE_EARTH_REF "angle_earth_ref"
#define PARAM_NAME_HORIZON_LEVEL_STRENGTH "horizon_level_strength"
#define PARAM_NAME_HORIZON_LIMIT_DEGREES "horizon_limit_degrees"
#define PARAM_NAME_HORIZON_LIMIT_STICKS "horizon_limit_sticks"
#define PARAM_NAME_HORIZON_IGNORE_STICKS "horizon_ignore_sticks"
#ifdef USE_GPS #ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider" #define PARAM_NAME_GPS_PROVIDER "gps_provider"

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

@ -127,6 +127,11 @@ float getRcDeflection(int axis)
#endif #endif
} }
float getRcDeflectionRaw(int axis)
{
return rcDeflection[axis];
}
float getRcDeflectionAbs(int axis) float getRcDeflectionAbs(int axis)
{ {
return rcDeflectionAbs[axis]; return rcDeflectionAbs[axis];

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

@ -32,6 +32,7 @@
void processRcCommand(void); void processRcCommand(void);
float getSetpointRate(int axis); float getSetpointRate(int axis);
float getRcDeflection(int axis); float getRcDeflection(int axis);
float getRcDeflectionRaw(int axis);
float getRcDeflectionAbs(int axis); float getRcDeflectionAbs(int axis);
void updateRcCommands(void); void updateRcCommands(void);
void resetYawAxis(void); void resetYawAxis(void);

234
src/main/flight/feedforward.c
View file

@ -28,6 +28,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "fc/rc.h" #include "fc/rc.h"
#include "fc/runtime_config.h"
#include "flight/pid.h" #include "flight/pid.h"
@ -48,6 +49,10 @@ typedef struct laggedMovingAverageCombined_s {
} laggedMovingAverageCombined_t; } laggedMovingAverageCombined_t;
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT]; laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
uint8_t getFeedforwardDuplicateCount(int axis){
return duplicateCount[axis];
}
void feedforwardInit(const pidProfile_t *pidProfile) void feedforwardInit(const pidProfile_t *pidProfile)
{ {
const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f; const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
@ -59,136 +64,145 @@ void feedforwardInit(const pidProfile_t *pidProfile)
} }
} }
FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging) FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging, const float setpoint, bool rawSetpointIsSmoothed)
{ {
const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
if (newRcFrame) { if (rawSetpointIsSmoothed) {
// simple feedforward with boost on pre-smoothed data that changes smoothly each PID loop
const float setpointSpeed = (setpoint - prevSetpoint[axis]);
prevSetpoint[axis] = setpoint;
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
prevSetpointSpeed[axis] = setpointSpeed;
setpointAcceleration *= feedforwardBoostFactor;
setpointDelta[axis] = (setpointSpeed + setpointAcceleration);
} else {
if (newRcFrame) {
// calculate feedforward in steps, when new RC packet arrives, then upsammple the resulting feedforward to PID loop rate
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
// good values : 25 for 111hz FrSky, 30 for 150hz, 50 for 250hz, 65 for 500hz links
const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
// 7 is default, 5 for faster links with smaller steps and for racing, 10-12 for 150hz freestyle
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; // 0.0066 for 150hz RC Link.
const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor(); const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
// good values : 25 for 111hz FrSky, 30 for 150hz, 50 for 250hz, 65 for 500hz links
const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
// 7 is default, 5 for faster links with smaller steps and for racing, 10-12 for 150hz freestyle
const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; // 0.0066 for 150hz RC Link. float rcCommandDelta = getRcCommandDelta(axis);
const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link
const float setpoint = getRawSetpoint(axis); if (axis == FD_ROLL) {
const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis]; DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f));
// rcCommand packet difference = value of 100 if 1000 RC steps
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint));
// un-smoothed in blackbox
}
float rcCommandDelta = getRcCommandDelta(axis); // calculate setpoint speed
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
if (axis == FD_ROLL) { float setpointAcceleration = 0.0f;
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f));
// rcCommand packet difference = value of 100 if 1000 RC steps
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint));
// un-smoothed in blackbox
}
// calculate setpoint speed rcCommandDelta = fabsf(rcCommandDelta);
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
float setpointAcceleration = 0.0f; if (rcCommandDelta) {
// we have movement and should calculate feedforward
rcCommandDelta = fabsf(rcCommandDelta); // jitter attenuator falls below 1 when rcCommandDelta falls below jitter threshold
float jitterAttenuator = 1.0f;
if (feedforwardJitterFactor) {
if (rcCommandDelta < feedforwardJitterFactor) {
jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f);
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
}
}
if (rcCommandDelta) { // duplicateCount indicates number of prior duplicate/s, 1 means one only duplicate prior to this packet
// we have movement and should calculate feedforward // reduce setpoint speed by half after a single duplicate or a third after two. Any more are forced to zero.
// needed because while sticks are moving, the next valid step up will be proportionally bigger
// and stops excessive feedforward where steps are at intervals, eg when the OpenTx ADC filter is active
// downside is that for truly held sticks, the first feedforward step won't be as big as it should be
if (duplicateCount[axis]) {
setpointSpeed /= duplicateCount[axis] + 1;
}
// jitter attenuator falls below 1 when rcCommandDelta falls below jitter threshold // first order type smoothing for setpoint speed noise reduction
float jitterAttenuator = 1.0f; setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
if (feedforwardJitterFactor) {
if (rcCommandDelta < feedforwardJitterFactor) { // calculate acceleration from smoothed setpoint speed
jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f); setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
// use rxRate to normalise acceleration to nominal RC packet interval of 100hz
// without this, we would get less boost than we should at higher Rx rates
// note rxRate updates with every new packet (though not every time data changes), hence
// if no Rx packets are received for a period, boost amount is correctly attenuated in proportion to the delay
setpointAcceleration *= rxRate * 0.01f;
// first order acceleration smoothing (with smoothed input this is effectively second order all up)
setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
// jitter reduction to reduce acceleration spikes at low rcCommandDelta values
// no effect for rcCommandDelta values above jitter threshold (zero delay)
// does not attenuate the basic feedforward amount, but this is small anyway at centre due to expo
setpointAcceleration *= jitterAttenuator;
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (absSetpointPercent > 0.95f && absSetpointSpeed < 3.0f * absPrevSetpointSpeed) {
// approaching max stick position so zero out feedforward to minimise overshoot
setpointSpeed = 0.0f;
setpointAcceleration = 0.0f;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevAcceleration[axis] = setpointAcceleration;
setpointAcceleration *= feedforwardBoostFactor;
// add attenuated boost to base feedforward and apply jitter attenuation
setpointDelta[axis] = (setpointSpeed + setpointAcceleration) * pidGetDT() * jitterAttenuator;
//reset counter
duplicateCount[axis] = 0;
} else {
// no movement
if (duplicateCount[axis]) {
// increment duplicate count to max of 2
duplicateCount[axis] += (duplicateCount[axis] < 2) ? 1 : 0;
// second or subsequent duplicate, or duplicate when held at max stick or centre position.
// force feedforward to zero
setpointDelta[axis] = 0.0f;
// zero speed and acceleration for correct smoothing of next good packet
setpointSpeed = 0.0f;
prevSetpointSpeed[axis] = 0.0f;
prevAcceleration[axis] = 0.0f;
} else {
// first duplicate; hold feedforward and previous static values, as if we just never got anything
duplicateCount[axis] = 1;
} }
} }
// duplicateCount indicates number of prior duplicate/s, 1 means one only duplicate prior to this packet
// reduce setpoint speed by half after a single duplicate or a third after two. Any more are forced to zero. if (axis == FD_ROLL) {
// needed because while sticks are moving, the next valid step up will be proportionally bigger DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * pidGetDT() * 100.0f)); // setpoint speed after smoothing
// and stops excessive feedforward where steps are at intervals, eg when the OpenTx ADC filter is active DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpointAcceleration * pidGetDT() * 100.0f)); // boost amount after smoothing
// downside is that for truly held sticks, the first feedforward step won't be as big as it should be // debug 0 is interpolated setpoint, above
if (duplicateCount[axis]) { // debug 3 is rcCommand delta, above
setpointSpeed /= duplicateCount[axis] + 1;
} }
// first order type smoothing for setpoint speed noise reduction prevSetpoint[axis] = setpoint;
setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
// calculate acceleration from smoothed setpoint speed // apply averaging, if enabled - include zero values in averaging
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; if (feedforwardAveraging) {
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]);
// use rxRate to normalise acceleration to nominal RC packet interval of 100hz
// without this, we would get less boost than we should at higher Rx rates
// note rxRate updates with every new packet (though not every time data changes), hence
// if no Rx packets are received for a period, boost amount is correctly attenuated in proportion to the delay
setpointAcceleration *= rxRate * 0.01f;
// first order acceleration smoothing (with smoothed input this is effectively second order all up)
setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
// jitter reduction to reduce acceleration spikes at low rcCommandDelta values
// no effect for rcCommandDelta values above jitter threshold (zero delay)
// does not attenuate the basic feedforward amount, but this is small anyway at centre due to expo
setpointAcceleration *= jitterAttenuator;
if (absSetpointPercent > 0.95f && absSetpointSpeed < 3.0f * absPrevSetpointSpeed) {
// approaching max stick position so zero out feedforward to minimise overshoot
setpointSpeed = 0.0f;
setpointAcceleration = 0.0f;
} }
prevSetpointSpeed[axis] = setpointSpeed; // apply feedforward transition
prevAcceleration[axis] = setpointAcceleration; setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
setpointAcceleration *= feedforwardBoostFactor;
// add attenuated boost to base feedforward and apply jitter attenuation
setpointDelta[axis] = (setpointSpeed + setpointAcceleration) * pidGetDT() * jitterAttenuator;
//reset counter
duplicateCount[axis] = 0;
} else {
// no movement
if (duplicateCount[axis]) {
// increment duplicate count to max of 2
duplicateCount[axis] += (duplicateCount[axis] < 2) ? 1 : 0;
// second or subsequent duplicate, or duplicate when held at max stick or centre position.
// force feedforward to zero
setpointDelta[axis] = 0.0f;
// zero speed and acceleration for correct smoothing of next good packet
setpointSpeed = 0.0f;
prevSetpointSpeed[axis] = 0.0f;
prevAcceleration[axis] = 0.0f;
} else {
// first duplicate; hold feedforward and previous static values, as if we just never got anything
duplicateCount[axis] = 1;
}
} }
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * pidGetDT() * 100.0f)); // setpoint speed after smoothing
DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpointAcceleration * pidGetDT() * 100.0f)); // boost amount after smoothing
// debug 0 is interpolated setpoint, above
// debug 3 is rcCommand delta, above
}
prevSetpoint[axis] = setpoint;
// apply averaging, if enabled - include zero values in averaging
if (feedforwardAveraging) {
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]);
}
// apply feedforward transition
setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
} }
return setpointDelta[axis]; // the value used by the PID code return setpointDelta[axis]; // the value used by the PID code
} }
@ -221,6 +235,6 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp,
bool shouldApplyFeedforwardLimits(int axis) bool shouldApplyFeedforwardLimits(int axis)
{ {
return axis < FD_YAW && feedforwardMaxRateLimit[axis] != 0.0f; return axis < FD_YAW && !FLIGHT_MODE(ANGLE_MODE) && feedforwardMaxRateLimit[axis] != 0.0f;
} }
#endif #endif

3
src/main/flight/feedforward.h
View file

@ -25,7 +25,8 @@
#include "common/axis.h" #include "common/axis.h"
#include "flight/pid.h" #include "flight/pid.h"
uint8_t getFeedforwardDuplicateCount(int axis);
void feedforwardInit(const pidProfile_t *pidProfile); void feedforwardInit(const pidProfile_t *pidProfile);
float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging); float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging, const float setpoint, bool rawSetpointIsSmoothed);
float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint); float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFeedforwardLimits(int axis); bool shouldApplyFeedforwardLimits(int axis);

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

@ -128,7 +128,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
[PID_ROLL] = PID_ROLL_DEFAULT, [PID_ROLL] = PID_ROLL_DEFAULT,
[PID_PITCH] = PID_PITCH_DEFAULT, [PID_PITCH] = PID_PITCH_DEFAULT,
[PID_YAW] = PID_YAW_DEFAULT, [PID_YAW] = PID_YAW_DEFAULT,
[PID_LEVEL] = { 50, 50, 75, 0 }, [PID_LEVEL] = { 50, 50, 75, 50 },
[PID_MAG] = { 40, 0, 0, 0 }, [PID_MAG] = { 40, 0, 0, 0 },
}, },
.pidSumLimit = PIDSUM_LIMIT, .pidSumLimit = PIDSUM_LIMIT,
@ -138,7 +138,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dterm_notch_cutoff = 0, .dterm_notch_cutoff = 0,
.itermWindupPointPercent = 85, .itermWindupPointPercent = 85,
.pidAtMinThrottle = PID_STABILISATION_ON, .pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55, .angle_limit = 60,
.feedforward_transition = 0, .feedforward_transition = 0,
.yawRateAccelLimit = 0, .yawRateAccelLimit = 0,
.rateAccelLimit = 0, .rateAccelLimit = 0,
@ -151,8 +151,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.crash_gthreshold = 400, // degrees/second .crash_gthreshold = 400, // degrees/second
.crash_setpoint_threshold = 350, // degrees/second .crash_setpoint_threshold = 350, // degrees/second
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
.horizon_tilt_effect = 75, .horizon_limit_degrees = 135,
.horizon_tilt_expert_mode = false, .horizon_ignore_sticks = false,
.crash_limit_yaw = 200, .crash_limit_yaw = 200,
.itermLimit = 400, .itermLimit = 400,
.throttle_boost = 5, .throttle_boost = 5,
@ -224,6 +224,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.tpa_mode = TPA_MODE_D, .tpa_mode = TPA_MODE_D,
.tpa_rate = 65, .tpa_rate = 65,
.tpa_breakpoint = 1350, .tpa_breakpoint = 1350,
.angle_feedforward_smoothing_ms = 80,
.angle_earth_ref = 100,
); );
#ifndef USE_D_MIN #ifndef USE_D_MIN
@ -365,9 +367,9 @@ float pidApplyThrustLinearization(float motorOutput)
#if defined(USE_ACC) #if defined(USE_ACC)
// calculate the stick deflection while applying level mode expo // calculate the stick deflection while applying level mode expo
static float getLevelModeRcDeflection(uint8_t axis) static float getAngleModeStickInputRaw(uint8_t axis)
{ {
const float stickDeflection = getRcDeflection(axis); const float stickDeflection = getRcDeflectionRaw(axis);
if (axis < FD_YAW) { if (axis < FD_YAW) {
const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f; const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f;
return power3(stickDeflection) * expof + stickDeflection * (1 - expof); return power3(stickDeflection) * expof + stickDeflection * (1 - expof);
@ -379,86 +381,127 @@ static float getLevelModeRcDeflection(uint8_t axis)
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength(void) STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength(void)
{ {
// start with 1.0 at center stick, 0.0 at max stick deflection:
float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH)));
// 0 at level, 90 at vertical, 180 at inverted (degrees):
const float currentInclination = MAX(abs(attitude.values.roll), abs(attitude.values.pitch)) / 10.0f; const float currentInclination = MAX(abs(attitude.values.roll), abs(attitude.values.pitch)) / 10.0f;
// 0 when level, 90 when vertical, 180 when inverted (degrees):
float horizonLevelStrength = MAX((pidRuntime.horizonLimitDegrees - currentInclination) / pidRuntime.horizonLimitDegrees, 0.0f);
// 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees
// horizonTiltExpertMode: 0 = leveling always active when sticks centered, if (!pidRuntime.horizonIgnoreSticks) {
// 1 = leveling can be totally off when inverted // horizonIgnoreSticks: 0 = default; levelling attenuated by both attitude and sticks;
if (pidRuntime.horizonTiltExpertMode) { // 1 = level attenuation only by attitude
if (pidRuntime.horizonTransition > 0 && pidRuntime.horizonCutoffDegrees > 0) { const float absMaxStickDeflection = MAX(fabsf(getRcDeflection(FD_ROLL)), fabsf(getRcDeflection(FD_PITCH))); // 0-1, smoothed if RC smoothing is enabled
// if d_level > 0 and horizonTiltEffect < 175 const float horizonStickAttenuation = MAX((pidRuntime.horizonLimitSticks - absMaxStickDeflection) / pidRuntime.horizonLimitSticks, 0.0f);
// horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero) // 1.0 at center stick, 0.0 at max stick deflection:
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling) horizonLevelStrength *= horizonStickAttenuation * pidRuntime.horizonGain;
// for larger inclinations; 0.0 at horizonCutoffDegrees value:
const float inclinationLevelRatio = constrainf((pidRuntime.horizonCutoffDegrees-currentInclination) / pidRuntime.horizonCutoffDegrees, 0, 1);
// apply configured horizon sensitivity:
// when stick is near center (horizonLevelStrength ~= 1.0)
// H_sensitivity value has little effect,
// when stick is deflected (horizonLevelStrength near 0.0)
// H_sensitivity value has more effect:
horizonLevelStrength = (horizonLevelStrength - 1) * 100 / pidRuntime.horizonTransition + 1;
// apply inclination ratio, which may lower leveling
// to zero regardless of stick position:
horizonLevelStrength *= inclinationLevelRatio;
} else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
horizonLevelStrength = 0;
}
} else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
float sensitFact;
if (pidRuntime.horizonFactorRatio < 1.0f) { // if horizonTiltEffect > 0
// horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
// for larger inclinations, goes to 1.0 at inclination==level:
const float inclinationLevelRatio = (180 - currentInclination) / 180 * (1.0f - pidRuntime.horizonFactorRatio) + pidRuntime.horizonFactorRatio;
// apply ratio to configured horizon sensitivity:
sensitFact = pidRuntime.horizonTransition * inclinationLevelRatio;
} else { // horizonTiltEffect=0 for "old" functionality
sensitFact = pidRuntime.horizonTransition;
}
if (sensitFact <= 0) { // zero means no leveling
horizonLevelStrength = 0;
} else {
// when stick is near center (horizonLevelStrength ~= 1.0)
// sensitFact value has little effect,
// when stick is deflected (horizonLevelStrength near 0.0)
// sensitFact value has more effect:
horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
}
} }
return horizonLevelStrength;
return constrainf(horizonLevelStrength, 0, 1);
} }
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow. // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
// The impact is possibly slightly slower performance on F7/H7 but they have more than enough // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
// processing power that it should be a non-issue. // processing power that it should be a non-issue.
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim,
float currentPidSetpoint, float horizonLevelStrength) float rawSetpoint, float horizonLevelStrength, bool newRcFrame)
{ {
const float levelAngleLimit = pidProfile->levelAngleLimit; const float angleLimit = pidProfile->angle_limit;
// calculate error angle and limit the angle to the max inclination
// rcDeflection is in range [-1.0, 1.0] // ** angle loop feedforward
float angle = levelAngleLimit * getLevelModeRcDeflection(axis); float angleFeedforward = 0.0f;
#ifdef USE_GPS_RESCUE #ifdef USE_FEEDFORWARD
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES const float rxRateHz = 1e6f / getCurrentRxRefreshRate();
#endif const float rcCommandDeltaAbs = fabsf(getRcCommandDelta(axis));
angle = constrainf(angle, -levelAngleLimit, levelAngleLimit);
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); if (newRcFrame){
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) { // this runs at Rx link rate
// ANGLE mode - control is angle based const float angleTargetRaw = angleLimit * getAngleModeStickInputRaw(axis);
const float setpointCorrection = errorAngle * pidRuntime.levelGain; float angleFeedforwardInput = 0.0f;
currentPidSetpoint = pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
} else { if (rcCommandDeltaAbs) {
// HORIZON mode - mix of ANGLE and ACRO modes // there is movement, so calculate a Delta value to get feedforward
// mix in errorAngle to currentPidSetpoint to add a little auto-level feel pidRuntime.angleTargetDelta[axis] = (angleTargetRaw - pidRuntime.angleTargetPrevious[axis]);
const float setpointCorrection = errorAngle * pidRuntime.horizonGain * horizonLevelStrength; angleFeedforwardInput = pidRuntime.angleTargetDelta[axis];
currentPidSetpoint += pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection); pidRuntime.angleDuplicateCount[axis] = 0;
pidRuntime.angleTargetPrevious[axis] = angleTargetRaw;
// no need for averaging or PT1 smoothing because of the strong PT3 smoothing of the final value
} else { // it's a duplicate
// TO DO - interpolate duplicates in rc.c for frsky so that incoming steps don't have them
pidRuntime.angleDuplicateCount[axis] += 1;
const float rcDeflectionAbs = getRcDeflectionAbs(axis);
if (pidRuntime.angleDuplicateCount[axis] == 1 && rcDeflectionAbs < 0.97f) {
// on first duplicate, unless we just hit max deflection, reduce glitch by interpolation
angleFeedforwardInput = pidRuntime.angleTargetDelta[axis];
} else {
// force feedforward to zero
pidRuntime.angleDuplicateCount[axis] = 2;
pidRuntime.angleTargetDelta[axis] = 0.0f;
angleFeedforwardInput = 0.0f;
}
}
// jitter attenuation copied from feedforward.c
// TO DO move jitter algorithm to rc.c and use same code here and in feedforward.c
const float feedforwardJitterFactor = pidRuntime.feedforwardJitterFactor;
float jitterAttenuator = 1.0f;
if (feedforwardJitterFactor && rcCommandDeltaAbs < feedforwardJitterFactor) {
jitterAttenuator = 1.0f - (rcCommandDeltaAbs / feedforwardJitterFactor);
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
}
angleFeedforwardInput *= jitterAttenuator * rxRateHz;
pidRuntime.angleFeedforward[axis] = angleFeedforwardInput; // feedforward element in degrees
} }
return currentPidSetpoint;
angleFeedforward = pidRuntime.angleFeedforward[axis] * pidRuntime.angleFeedforwardGain;
// filter angle feedforward, heavily, at the PID loop rate, providing user control over time constant
angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward);
#endif // USE_FEEDFORWARD
// calculate error angle and limit the angle to the max inclination
// stick input is from rcCommand, is smoothed, includes level expo, and is in range [-1.0, 1.0]
float angleTarget = angleLimit * getAngleModeStickInputRaw(axis);
#ifdef USE_GPS_RESCUE
angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
#endif
const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; // stepped at 500hz with some 4ms flat spots
const float errorAngle = angleTarget - currentAngle;
float angleRate = errorAngle * pidRuntime.angleGain + angleFeedforward;
// optionally, minimise cross-axis wobble due to faster yaw responses than roll or pitch, and make co-ordinated yaw turns
// by compensating for the effect of yaw on roll while pitched, and on pitch while rolled
float axisCoordination = pidRuntime.angleYawSetpoint;
if (pidRuntime.angleEarthRef) {
const float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));
pidRuntime.angleTarget[axis] = angleTarget; // store target for alternate axis to current axis, for use in preceding calculation
axisCoordination *= (axis == FD_ROLL) ? -sinAngle : sinAngle; // must be negative for Roll
angleRate += axisCoordination * pidRuntime.angleEarthRef;
}
// smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc
// this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy
angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
rawSetpoint = angleRate;
} else {
// can only be HORIZON mode - crossfade Angle rate and Acro rate
rawSetpoint = rawSetpoint * (1.0f - horizonLevelStrength) + angleRate * horizonLevelStrength;
}
//logging
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ANGLE_MODE, 0, lrintf(angleTarget * 10.0f)); // target angle
DEBUG_SET(DEBUG_ANGLE_MODE, 1, lrintf(errorAngle * pidRuntime.angleGain * 10.0f)); // un-smoothed error correction in degrees
DEBUG_SET(DEBUG_ANGLE_MODE, 2, lrintf(angleFeedforward * 10.0f)); // feedforward amount in degrees
DEBUG_SET(DEBUG_ANGLE_MODE, 3, lrintf(currentAngle * 10.0f)); // angle returned
DEBUG_SET(DEBUG_ANGLE_TARGET, 0, lrintf(angleTarget * 10.0f));
DEBUG_SET(DEBUG_ANGLE_TARGET, 1, lrintf(axisCoordination * 10.0f));
// debug ANGLE_TARGET 2 is yaw attenuation
DEBUG_SET(DEBUG_ANGLE_TARGET, 3, lrintf(currentAngle * 10.0f)); // angle returned
}
DEBUG_SET(DEBUG_CURRENT_ANGLE, axis, lrintf(currentAngle * 10.0f)); // current angle
return rawSetpoint;
} }
static FAST_CODE_NOINLINE void handleCrashRecovery( static FAST_CODE_NOINLINE void handleCrashRecovery(
@ -476,7 +519,7 @@ static FAST_CODE_NOINLINE void handleCrashRecovery(
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
// errorAngle is deviation from horizontal // errorAngle is deviation from horizontal
const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
*currentPidSetpoint = errorAngle * pidRuntime.levelGain; *currentPidSetpoint = errorAngle * pidRuntime.angleGain;
*errorRate = *currentPidSetpoint - gyroRate; *errorRate = *currentPidSetpoint - gyroRate;
} }
} }
@ -732,7 +775,11 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
if (pidRuntime.itermRelax) { if (pidRuntime.itermRelax) {
if (axis < FD_YAW || pidRuntime.itermRelax == ITERM_RELAX_RPY || pidRuntime.itermRelax == ITERM_RELAX_RPY_INC) { if (axis < FD_YAW || pidRuntime.itermRelax == ITERM_RELAX_RPY || pidRuntime.itermRelax == ITERM_RELAX_RPY_INC) {
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD); float itermRelaxThreshold = ITERM_RELAX_SETPOINT_THRESHOLD;
if (FLIGHT_MODE(ANGLE_MODE)) {
itermRelaxThreshold *= 0.2f;
}
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxThreshold);
const bool isDecreasingI = const bool isDecreasingI =
((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0)); ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
if ((pidRuntime.itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) { if ((pidRuntime.itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
@ -932,23 +979,42 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate(); rpmFilterUpdate();
#endif #endif
#ifdef USE_FEEDFORWARD const bool newRcFrame = getShouldUpdateFeedforward();
const bool newRcFrame = getShouldUpdateFeedforward();
#endif
// ----------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);
float rawSetpoint = getRawSetpoint(axis);
bool rawSetpointIsSmoothed = false;
if (pidRuntime.maxVelocity[axis]) { if (pidRuntime.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
// When Race Mode is active PITCH control is also GYRO based in level or horizon mode // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
#if defined(USE_ACC) #if defined(USE_ACC)
// earth reference yaw by attenuating yaw rate at higher angles
if (axis == FD_YAW && pidRuntime.angleEarthRef) {
pidRuntime.angleYawSetpoint = currentPidSetpoint;
if (levelMode == LEVEL_MODE_RP) {
float maxAngleTargetAbs = fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );
maxAngleTargetAbs *= pidRuntime.angleEarthRef;
if (FLIGHT_MODE(HORIZON_MODE)) {
// reduce yaw compensation when Horizon uses less levelling
maxAngleTargetAbs *= horizonLevelStrength;
}
float attenuateYawSetpoint = cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs));
currentPidSetpoint *= attenuateYawSetpoint;
rawSetpoint *= attenuateYawSetpoint;
DEBUG_SET(DEBUG_ANGLE_TARGET, 2, lrintf(attenuateYawSetpoint * 100.0f)); // yaw attenuation
}
}
if ((levelMode == LEVEL_MODE_R && axis == FD_ROLL) if ((levelMode == LEVEL_MODE_R && axis == FD_ROLL)
|| (levelMode == LEVEL_MODE_RP && (axis == FD_ROLL || axis == FD_PITCH)) ) { || (levelMode == LEVEL_MODE_RP && (axis == FD_ROLL || axis == FD_PITCH)) ) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength); rawSetpoint = pidLevel(axis, pidProfile, angleTrim, rawSetpoint, horizonLevelStrength, newRcFrame);
currentPidSetpoint = rawSetpoint;
rawSetpointIsSmoothed = true;
// levelled axes are already smoothed; feedforward should be calculated each PID loop
DEBUG_SET(DEBUG_ATTITUDE, axis - FD_ROLL + 2, currentPidSetpoint); DEBUG_SET(DEBUG_ATTITUDE, axis - FD_ROLL + 2, currentPidSetpoint);
} }
#endif #endif
@ -1031,7 +1097,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate pidSetpointDelta // -----calculate pidSetpointDelta
float pidSetpointDelta = 0; float pidSetpointDelta = 0;
#ifdef USE_FEEDFORWARD #ifdef USE_FEEDFORWARD
pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging); pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging, rawSetpoint, rawSetpointIsSmoothed);
#endif #endif
pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint; pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
@ -1103,8 +1169,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// no feedforward in launch control // no feedforward in launch control
float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf; float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
if (feedforwardGain > 0) { if (feedforwardGain > 0) {
// halve feedforward in Level mode since stick sensitivity is weaker by about half
feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f;
// transition now calculated in feedforward.c when new RC data arrives // transition now calculated in feedforward.c when new RC data arrives
float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency; float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;

28
src/main/flight/pid.h
View file

@ -146,10 +146,10 @@ typedef struct pidProfile_s {
uint16_t pidSumLimit; uint16_t pidSumLimit;
uint16_t pidSumLimitYaw; uint16_t pidSumLimitYaw;
uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
uint8_t levelAngleLimit; // Max angle in degrees in level mode uint8_t angle_limit; // Max angle in degrees in Angle mode
uint8_t horizon_tilt_effect; // inclination factor for Horizon mode uint8_t horizon_limit_degrees; // in Horizon mode, zero levelling when the quad's attitude exceeds this angle
uint8_t horizon_tilt_expert_mode; // OFF or ON uint8_t horizon_ignore_sticks; // 0 = default, meaning both stick and attitude attenuation; 1 = only attitude attenuation
// Betaflight PID controller parameters // Betaflight PID controller parameters
uint8_t anti_gravity_gain; // AntiGravity Gain (was itermAcceleratorGain) uint8_t anti_gravity_gain; // AntiGravity Gain (was itermAcceleratorGain)
@ -234,6 +234,8 @@ typedef struct pidProfile_s {
uint8_t tpa_mode; // Controls which PID terms TPA effects uint8_t tpa_mode; // Controls which PID terms TPA effects
uint8_t tpa_rate; // Percent reduction in P or D at full throttle uint8_t tpa_rate; // Percent reduction in P or D at full throttle
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds
uint8_t angle_earth_ref; // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode
} pidProfile_t; } pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -294,12 +296,12 @@ typedef struct pidRuntime_s {
uint8_t antiGravityGain; uint8_t antiGravityGain;
float antiGravityPGain; float antiGravityPGain;
pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT]; pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
float levelGain; float angleGain;
float angleFeedforwardGain;
float horizonGain; float horizonGain;
float horizonTransition; float horizonLimitSticks;
float horizonCutoffDegrees; float horizonLimitDegrees;
float horizonFactorRatio; uint8_t horizonIgnoreSticks;
uint8_t horizonTiltExpertMode;
float maxVelocity[XYZ_AXIS_COUNT]; float maxVelocity[XYZ_AXIS_COUNT];
float itermWindupPointInv; float itermWindupPointInv;
bool inCrashRecoveryMode; bool inCrashRecoveryMode;
@ -396,10 +398,18 @@ typedef struct pidRuntime_s {
float feedforwardSmoothFactor; float feedforwardSmoothFactor;
float feedforwardJitterFactor; float feedforwardJitterFactor;
float feedforwardBoostFactor; float feedforwardBoostFactor;
float angleFeedforward[XYZ_AXIS_COUNT];
float angleTargetPrevious[XYZ_AXIS_COUNT];
float angleTargetDelta[XYZ_AXIS_COUNT];
uint8_t angleDuplicateCount[XYZ_AXIS_COUNT];
#endif #endif
#ifdef USE_ACC #ifdef USE_ACC
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
float angleYawSetpoint;
float angleEarthRef;
float angleTarget[2];
#endif #endif
} pidRuntime_t; } pidRuntime_t;
@ -446,7 +456,7 @@ void applyItermRelax(const int axis, const float iterm,
void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate); void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
void rotateItermAndAxisError(); void rotateItermAndAxisError();
float pidLevel(int axis, const pidProfile_t *pidProfile, float pidLevel(int axis, const pidProfile_t *pidProfile,
const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint, float horizonLevelStrength); const rollAndPitchTrims_t *angleTrim, float rawSetpoint, float horizonLevelStrength, bool newRcFrame);
float calcHorizonLevelStrength(void); float calcHorizonLevelStrength(void);
#endif #endif
void dynLpfDTermUpdate(float throttle); void dynLpfDTermUpdate(float throttle);

22
src/main/flight/pid_init.c
View file

@ -52,7 +52,7 @@
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f #define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f
#endif #endif
#define ATTITUDE_CUTOFF_HZ 250 #define ATTITUDE_CUTOFF_HZ 50
static void pidSetTargetLooptime(uint32_t pidLooptime) static void pidSetTargetLooptime(uint32_t pidLooptime)
{ {
@ -237,8 +237,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
#ifdef USE_ACC #ifdef USE_ACC
const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT); const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT);
const float angleCutoffHz = 1000.0f / (2.0f * M_PIf * pidProfile->angle_feedforward_smoothing_ms); // default of 80ms -> 2.0Hz, 160ms -> 1.0Hz, approximately
const float k2 = pt3FilterGain(angleCutoffHz, pidRuntime.dT);
for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k); pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
} }
#endif #endif
@ -291,12 +295,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
{ {
pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f; pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
} }
pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f; pidRuntime.angleGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f; pidRuntime.angleFeedforwardGain = pidProfile->pid[PID_LEVEL].F / 100.0f;
pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D; pidRuntime.angleEarthRef = pidProfile->angle_earth_ref / 100.0f;
pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f; pidRuntime.horizonGain = MIN(pidProfile->pid[PID_LEVEL].I / 100.0f, 1.0f);
pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; pidRuntime.horizonLimitSticks = pidProfile->pid[PID_LEVEL].D / 100.0f;
pidRuntime.horizonIgnoreSticks = pidProfile->horizon_ignore_sticks;
pidRuntime.horizonLimitDegrees = (float)pidProfile->horizon_limit_degrees;
pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT; pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT; pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.itermWindupPointInv = 1.0f; pidRuntime.itermWindupPointInv = 1.0f;
@ -426,6 +433,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
} }
pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor; pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f; pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
feedforwardInit(pidProfile); feedforwardInit(pidProfile);
#endif #endif

4
src/main/msp/msp.c
View file

@ -1943,7 +1943,7 @@ case MSP_NAME:
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU16(dst, currentPidProfile->rateAccelLimit); sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit); sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
sbufWriteU8(dst, currentPidProfile->levelAngleLimit); sbufWriteU8(dst, currentPidProfile->angle_limit);
sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold
sbufWriteU16(dst, currentPidProfile->anti_gravity_gain); sbufWriteU16(dst, currentPidProfile->anti_gravity_gain);
@ -3074,7 +3074,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
currentPidProfile->rateAccelLimit = sbufReadU16(src); currentPidProfile->rateAccelLimit = sbufReadU16(src);
currentPidProfile->yawRateAccelLimit = sbufReadU16(src); currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 2) { if (sbufBytesRemaining(src) >= 2) {
currentPidProfile->levelAngleLimit = sbufReadU8(src); currentPidProfile->angle_limit = sbufReadU8(src);
sbufReadU8(src); // was pidProfile.levelSensitivity sbufReadU8(src); // was pidProfile.levelSensitivity
} }
if (sbufBytesRemaining(src) >= 4) { if (sbufBytesRemaining(src) >= 4) {

134
src/test/unit/pid_unittest.cc
View file

@ -28,6 +28,10 @@ bool simulatedAirmodeEnabled = true;
float simulatedSetpointRate[3] = { 0,0,0 }; float simulatedSetpointRate[3] = { 0,0,0 };
float simulatedPrevSetpointRate[3] = { 0,0,0 }; float simulatedPrevSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedRcCommandDelta[3] = { 1,1,1 };
float simulatedRawSetpoint[3] = { 0,0,0 };
uint16_t simulatedCurrentRxRefreshRate = 10000;
uint8_t simulatedDuplicateCount = 0;
float simulatedMotorMixRange = 0.0f; float simulatedMotorMixRange = 0.0f;
int16_t debug[DEBUG16_VALUE_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT];
@ -84,6 +88,11 @@ extern "C" {
void systemBeep(bool) { } void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; } bool gyroOverflowDetected(void) { return false; }
float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; } float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
float getRcCommandDelta(int axis) { return simulatedRcCommandDelta[axis]; }
float getRcDeflectionRaw(int axis) { return simulatedRcDeflection[axis]; }
float getRawSetpoint(int axis) { return simulatedRawSetpoint[axis]; }
uint16_t getCurrentRxRefreshRate(void) { return simulatedCurrentRxRefreshRate; }
uint8_t getFeedforwardDuplicateCount(void) { return simulatedDuplicateCount; }
void beeperConfirmationBeeps(uint8_t) { } void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; } bool isLaunchControlActive(void) {return unitLaunchControlActive; }
void disarm(flightLogDisarmReason_e) { } void disarm(flightLogDisarmReason_e) { }
@ -95,10 +104,11 @@ extern "C" {
return value; return value;
} }
void feedforwardInit(const pidProfile_t) { } void feedforwardInit(const pidProfile_t) { }
float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging) float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging, const float setpoint)
{ {
UNUSED(newRcFrame); UNUSED(newRcFrame);
UNUSED(feedforwardAveraging); UNUSED(feedforwardAveraging);
UNUSED(setpoint);
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor(); const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
float setpointDelta = simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis]; float setpointDelta = simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
setpointDelta *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1; setpointDelta *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1;
@ -125,7 +135,7 @@ void setDefaultTestSettings(void)
pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 }; pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 };
pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 }; pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 };
pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 }; pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 }; pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 50 };
// Compensate for the upscaling done without 'use_integrated_yaw' // Compensate for the upscaling done without 'use_integrated_yaw'
pidProfile->pid[PID_YAW].I = pidProfile->pid[PID_YAW].I / 2.5f; pidProfile->pid[PID_YAW].I = pidProfile->pid[PID_YAW].I / 2.5f;
@ -140,7 +150,7 @@ void setDefaultTestSettings(void)
pidProfile->dterm_lpf1_type = FILTER_BIQUAD; pidProfile->dterm_lpf1_type = FILTER_BIQUAD;
pidProfile->itermWindupPointPercent = 50; pidProfile->itermWindupPointPercent = 50;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55; pidProfile->angle_limit = 60;
pidProfile->feedforward_transition = 100; pidProfile->feedforward_transition = 100;
pidProfile->yawRateAccelLimit = 100; pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0; pidProfile->rateAccelLimit = 0;
@ -153,8 +163,8 @@ void setDefaultTestSettings(void)
pidProfile->crash_gthreshold = 400; pidProfile->crash_gthreshold = 400;
pidProfile->crash_setpoint_threshold = 350; pidProfile->crash_setpoint_threshold = 350;
pidProfile->crash_recovery = PID_CRASH_RECOVERY_OFF; pidProfile->crash_recovery = PID_CRASH_RECOVERY_OFF;
pidProfile->horizon_tilt_effect = 75; pidProfile->horizon_limit_degrees = 135;
pidProfile->horizon_tilt_expert_mode = false; pidProfile->horizon_ignore_sticks = false;
pidProfile->crash_limit_yaw = 200; pidProfile->crash_limit_yaw = 200;
pidProfile->itermLimit = 150; pidProfile->itermLimit = 150;
pidProfile->throttle_boost = 0; pidProfile->throttle_boost = 0;
@ -194,6 +204,7 @@ void resetTest(void)
pidData[axis].Sum = 0; pidData[axis].Sum = 0;
simulatedSetpointRate[axis] = 0; simulatedSetpointRate[axis] = 0;
simulatedRcDeflection[axis] = 0; simulatedRcDeflection[axis] = 0;
simulatedRawSetpoint[axis] = 0;
gyro.gyroADCf[axis] = 0; gyro.gyroADCf[axis] = 0;
} }
attitude.values.roll = 0; attitude.values.roll = 0;
@ -381,39 +392,38 @@ TEST(pidControllerTest, testPidLevel)
float currentPidSetpoint = 30; float currentPidSetpoint = 30;
rollAndPitchTrims_t angleTrim = { { 0, 0 } }; rollAndPitchTrims_t angleTrim = { { 0, 0 } };
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(0, currentPidSetpoint); EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(0, currentPidSetpoint); EXPECT_FLOAT_EQ(0, currentPidSetpoint);
// Test attitude response // Test attitude response
setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f); setStickPosition(FD_PITCH, -1.0f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(244.07211, currentPidSetpoint); EXPECT_FLOAT_EQ(174.39539, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(-244.07211, currentPidSetpoint); EXPECT_FLOAT_EQ(-174.39539, currentPidSetpoint);
setStickPosition(FD_ROLL, -0.5f); setStickPosition(FD_ROLL, -0.5f);
setStickPosition(FD_PITCH, 0.5f); setStickPosition(FD_PITCH, 0.5f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(-93.487915, currentPidSetpoint); EXPECT_FLOAT_EQ(4.0751495, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(93.487915, currentPidSetpoint); EXPECT_FLOAT_EQ(-4.0751495, currentPidSetpoint);
attitude.values.roll = -275; attitude.values.roll = -275;
attitude.values.pitch = 275; attitude.values.pitch = 275;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(-12.047981, currentPidSetpoint); EXPECT_FLOAT_EQ(-19.130268, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint); EXPECT_FLOAT_EQ(19.130268, currentPidSetpoint);
// Disable ANGLE_MODE // Disable ANGLE_MODE
disableFlightMode(ANGLE_MODE); disableFlightMode(ANGLE_MODE);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(11.07958, currentPidSetpoint); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); EXPECT_FLOAT_EQ(14.693689, currentPidSetpoint);
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
// Test level mode expo // Test level mode expo
enableFlightMode(ANGLE_MODE); enableFlightMode(ANGLE_MODE);
@ -423,10 +433,10 @@ TEST(pidControllerTest, testPidLevel)
setStickPosition(FD_PITCH, -0.5f); setStickPosition(FD_PITCH, -0.5f);
currentControlRateProfile->levelExpo[FD_ROLL] = 50; currentControlRateProfile->levelExpo[FD_ROLL] = 50;
currentControlRateProfile->levelExpo[FD_PITCH] = 26; currentControlRateProfile->levelExpo[FD_PITCH] = 26;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(76.208672, currentPidSetpoint); EXPECT_FLOAT_EQ(45.520832, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength()); currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(-98.175163, currentPidSetpoint); EXPECT_FLOAT_EQ(-61.216412, currentPidSetpoint);
} }
@ -437,21 +447,79 @@ TEST(pidControllerTest, testPidHorizon)
pidStabilisationState(PID_STABILISATION_ON); pidStabilisationState(PID_STABILISATION_ON);
enableFlightMode(HORIZON_MODE); enableFlightMode(HORIZON_MODE);
// Test full stick response // Test stick response greater than default limit of 0.75
setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_ROLL, 0.76f);
setStickPosition(FD_PITCH, -1.0f); setStickPosition(FD_PITCH, -0.76f);
EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength()); EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
// Expect full rate output on full stick // Expect full rate output on full stick
// Test zero stick response // Test zero stick response
setStickPosition(FD_ROLL, 0); setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0); setStickPosition(FD_PITCH, 0);
EXPECT_FLOAT_EQ(1.0f, calcHorizonLevelStrength()); EXPECT_FLOAT_EQ(0.5f, calcHorizonLevelStrength());
// Test small stick response // Test small stick response when flat
setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f); setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.811f, calcHorizonLevelStrength(), calculateTolerance(0.82)); EXPECT_NEAR(0.4333f, calcHorizonLevelStrength(), calculateTolerance(0.434));
// Test larger stick response when flat
setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.166f, calcHorizonLevelStrength(), calculateTolerance(0.166));
// set attitude of craft to 90 degrees
attitude.values.roll = 900;
attitude.values.pitch = 900;
// Test centered sticks at 90 degrees
setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0);
// with gain of 50, and max angle of 135 deg, strength = 0.5 * (135-90) / 90 ie 0.5 * 45/136 or 0.5 * 0.333 = 0.166
EXPECT_NEAR(0.166f, calcHorizonLevelStrength(), calculateTolerance(0.166));
// Test small stick response at 90 degrees
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.144f, calcHorizonLevelStrength(), calculateTolerance(0.144));
// Test larger stick response at 90 degrees
setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.055f, calcHorizonLevelStrength(), calculateTolerance(0.055));
// set attitude of craft to 120 degrees, inside limit of 135
attitude.values.roll = 1200;
attitude.values.pitch = 1200;
// Test centered sticks at 120 degrees
setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0);
EXPECT_NEAR(0.055f, calcHorizonLevelStrength(), calculateTolerance(0.055));
// Test small stick response at 120 degrees
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.048f, calcHorizonLevelStrength(), calculateTolerance(0.048));
// Test larger stick response at 120 degrees
setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.018f, calcHorizonLevelStrength(), calculateTolerance(0.018));
// set attitude of craft to 1500 degrees, outside limit of 135
attitude.values.roll = 1500;
attitude.values.pitch = 1500;
// Test centered sticks at 150 degrees - should be zero at any stick angle
setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0);
EXPECT_NEAR(0.0f, calcHorizonLevelStrength(), calculateTolerance(0.0));
setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.0f, calcHorizonLevelStrength(), calculateTolerance(0.0));
} }
TEST(pidControllerTest, testMixerSaturation) TEST(pidControllerTest, testMixerSaturation)