mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Angle and Horizon Mode improvements (#12231)
* 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
GitHub
parent
d6ed53ecdf
commit
241e9a9b94
16 changed files with 482 additions and 271 deletions
|
|
@ -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_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_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
|
||||
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, "%d", currentPidProfile->rateAccelLimit);
|
||||
|
|
|
|||
|
|
@ -109,4 +109,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"GPS_DOP",
|
||||
"FAILSAFE",
|
||||
"GYRO_CALIBRATION",
|
||||
"ANGLE_MODE",
|
||||
"ANGLE_TARGET",
|
||||
"CURRENT_ANGLE",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -107,6 +107,9 @@ typedef enum {
|
|||
DEBUG_GPS_DOP,
|
||||
DEBUG_FAILSAFE,
|
||||
DEBUG_GYRO_CALIBRATION,
|
||||
DEBUG_ANGLE_MODE,
|
||||
DEBUG_ANGLE_TARGET,
|
||||
DEBUG_CURRENT_ANGLE,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -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]) },
|
||||
{ "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]) },
|
||||
{ "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]) },
|
||||
{ "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_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]) },
|
||||
{ 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
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "horizon_level_strength", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
|
||||
{ "horizon_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
|
||||
{ PARAM_NAME_ANGLE_P_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
|
||||
{ PARAM_NAME_ANGLE_FEEDFORWARD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].F) },
|
||||
{ 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) },
|
||||
|
||||
{ "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },
|
||||
{ "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_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) },
|
||||
{ PARAM_NAME_HORIZON_LIMIT_DEGREES, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_limit_degrees) },
|
||||
{ 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)
|
||||
{ PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
|
||||
|
|
|
|||
|
|
@ -516,10 +516,14 @@ static CMS_Menu cmsx_menuLaunchControl = {
|
|||
};
|
||||
#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_horizonTransition;
|
||||
static uint8_t cmsx_levelAngleLimit;
|
||||
static uint8_t cmsx_horizonLimitSticks;
|
||||
static uint8_t cmsx_horizonLimitDegrees;
|
||||
|
||||
static uint8_t cmsx_throttleBoost;
|
||||
static uint8_t cmsx_thrustLinearization;
|
||||
static uint8_t cmsx_antiGravityGain;
|
||||
|
|
@ -560,10 +564,13 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
|
|||
|
||||
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
|
||||
|
||||
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
|
||||
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
|
||||
cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D;
|
||||
cmsx_levelAngleLimit = pidProfile->levelAngleLimit;
|
||||
cmsx_angleP = pidProfile->pid[PID_LEVEL].P;
|
||||
cmsx_angleFF = pidProfile->pid[PID_LEVEL].F;
|
||||
cmsx_angleLimit = pidProfile->angle_limit;
|
||||
|
||||
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;
|
||||
|
||||
|
|
@ -611,10 +618,13 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
|
|||
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
|
||||
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].D = cmsx_horizonTransition;
|
||||
pidProfile->levelAngleLimit = cmsx_levelAngleLimit;
|
||||
pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks;
|
||||
pidProfile->horizon_limit_degrees = cmsx_horizonLimitDegrees;
|
||||
|
||||
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 BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_boost, 0, 50, 1 } },
|
||||
#endif
|
||||
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } },
|
||||
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 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_levelAngleLimit, 10, 90, 1 } },
|
||||
{ "ANGLE P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleP, 0, 200, 1 } },
|
||||
{ "ANGLE FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleFF, 0, 200, 1 } },
|
||||
{ "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleLimit, 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 } },
|
||||
#ifdef USE_THROTTLE_BOOST
|
||||
|
|
|
|||
|
|
@ -62,8 +62,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
|
|||
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
||||
.profileName = { 0 },
|
||||
.quickRatesRcExpo = 0,
|
||||
.levelExpo[FD_ROLL] = 0,
|
||||
.levelExpo[FD_PITCH] = 0,
|
||||
.levelExpo[FD_ROLL] = 33,
|
||||
.levelExpo[FD_PITCH] = 33,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -124,6 +124,18 @@
|
|||
#define PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO "altitude_prefer_baro"
|
||||
#define PARAM_NAME_POSITION_ALTITUDE_LPF "altitude_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
|
||||
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
|
||||
|
|
|
|||
|
|
@ -127,6 +127,11 @@ float getRcDeflection(int axis)
|
|||
#endif
|
||||
}
|
||||
|
||||
float getRcDeflectionRaw(int axis)
|
||||
{
|
||||
return rcDeflection[axis];
|
||||
}
|
||||
|
||||
float getRcDeflectionAbs(int axis)
|
||||
{
|
||||
return rcDeflectionAbs[axis];
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
void processRcCommand(void);
|
||||
float getSetpointRate(int axis);
|
||||
float getRcDeflection(int axis);
|
||||
float getRcDeflectionRaw(int axis);
|
||||
float getRcDeflectionAbs(int axis);
|
||||
void updateRcCommands(void);
|
||||
void resetYawAxis(void);
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
#include "common/maths.h"
|
||||
|
||||
#include "fc/rc.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
|
||||
|
|
@ -48,6 +49,10 @@ typedef struct laggedMovingAverageCombined_s {
|
|||
} laggedMovingAverageCombined_t;
|
||||
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
|
||||
|
||||
uint8_t getFeedforwardDuplicateCount(int axis){
|
||||
return duplicateCount[axis];
|
||||
}
|
||||
|
||||
void feedforwardInit(const pidProfile_t *pidProfile)
|
||||
{
|
||||
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 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 absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
|
||||
|
||||
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
|
||||
float rcCommandDelta = getRcCommandDelta(axis);
|
||||
|
||||
const float setpoint = getRawSetpoint(axis);
|
||||
const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
|
||||
if (axis == FD_ROLL) {
|
||||
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) {
|
||||
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 setpointAcceleration = 0.0f;
|
||||
|
||||
// calculate setpoint speed
|
||||
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
|
||||
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
|
||||
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
|
||||
rcCommandDelta = fabsf(rcCommandDelta);
|
||||
|
||||
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) {
|
||||
// we have movement and should calculate feedforward
|
||||
// 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.
|
||||
// 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
|
||||
float jitterAttenuator = 1.0f;
|
||||
if (feedforwardJitterFactor) {
|
||||
if (rcCommandDelta < feedforwardJitterFactor) {
|
||||
jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f);
|
||||
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
|
||||
// first order type smoothing for setpoint speed noise reduction
|
||||
setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
|
||||
|
||||
// calculate acceleration from smoothed setpoint speed
|
||||
setpointAcceleration = setpointSpeed - prevSetpointSpeed[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 (!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.
|
||||
// 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;
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// first order type smoothing for setpoint speed noise reduction
|
||||
setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
|
||||
prevSetpoint[axis] = setpoint;
|
||||
|
||||
// calculate acceleration from smoothed setpoint speed
|
||||
setpointAcceleration = setpointSpeed - prevSetpointSpeed[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;
|
||||
// apply averaging, if enabled - include zero values in averaging
|
||||
if (feedforwardAveraging) {
|
||||
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
// apply feedforward transition
|
||||
setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
}
|
||||
|
|
@ -221,6 +235,6 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp,
|
|||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -25,7 +25,8 @@
|
|||
#include "common/axis.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
uint8_t getFeedforwardDuplicateCount(int axis);
|
||||
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);
|
||||
bool shouldApplyFeedforwardLimits(int axis);
|
||||
|
|
|
|||
|
|
@ -128,7 +128,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
[PID_ROLL] = PID_ROLL_DEFAULT,
|
||||
[PID_PITCH] = PID_PITCH_DEFAULT,
|
||||
[PID_YAW] = PID_YAW_DEFAULT,
|
||||
[PID_LEVEL] = { 50, 50, 75, 0 },
|
||||
[PID_LEVEL] = { 50, 50, 75, 50 },
|
||||
[PID_MAG] = { 40, 0, 0, 0 },
|
||||
},
|
||||
.pidSumLimit = PIDSUM_LIMIT,
|
||||
|
|
@ -138,7 +138,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.dterm_notch_cutoff = 0,
|
||||
.itermWindupPointPercent = 85,
|
||||
.pidAtMinThrottle = PID_STABILISATION_ON,
|
||||
.levelAngleLimit = 55,
|
||||
.angle_limit = 60,
|
||||
.feedforward_transition = 0,
|
||||
.yawRateAccelLimit = 0,
|
||||
.rateAccelLimit = 0,
|
||||
|
|
@ -151,8 +151,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.crash_gthreshold = 400, // degrees/second
|
||||
.crash_setpoint_threshold = 350, // degrees/second
|
||||
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
|
||||
.horizon_tilt_effect = 75,
|
||||
.horizon_tilt_expert_mode = false,
|
||||
.horizon_limit_degrees = 135,
|
||||
.horizon_ignore_sticks = false,
|
||||
.crash_limit_yaw = 200,
|
||||
.itermLimit = 400,
|
||||
.throttle_boost = 5,
|
||||
|
|
@ -224,6 +224,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.tpa_mode = TPA_MODE_D,
|
||||
.tpa_rate = 65,
|
||||
.tpa_breakpoint = 1350,
|
||||
.angle_feedforward_smoothing_ms = 80,
|
||||
.angle_earth_ref = 100,
|
||||
);
|
||||
|
||||
#ifndef USE_D_MIN
|
||||
|
|
@ -365,9 +367,9 @@ float pidApplyThrustLinearization(float motorOutput)
|
|||
|
||||
#if defined(USE_ACC)
|
||||
// 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) {
|
||||
const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f;
|
||||
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
|
||||
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;
|
||||
// 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,
|
||||
// 1 = leveling can be totally off when inverted
|
||||
if (pidRuntime.horizonTiltExpertMode) {
|
||||
if (pidRuntime.horizonTransition > 0 && pidRuntime.horizonCutoffDegrees > 0) {
|
||||
// if d_level > 0 and horizonTiltEffect < 175
|
||||
// horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
|
||||
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
|
||||
// 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;
|
||||
}
|
||||
if (!pidRuntime.horizonIgnoreSticks) {
|
||||
// horizonIgnoreSticks: 0 = default; levelling attenuated by both attitude and sticks;
|
||||
// 1 = level attenuation only by attitude
|
||||
const float absMaxStickDeflection = MAX(fabsf(getRcDeflection(FD_ROLL)), fabsf(getRcDeflection(FD_PITCH))); // 0-1, smoothed if RC smoothing is enabled
|
||||
const float horizonStickAttenuation = MAX((pidRuntime.horizonLimitSticks - absMaxStickDeflection) / pidRuntime.horizonLimitSticks, 0.0f);
|
||||
// 1.0 at center stick, 0.0 at max stick deflection:
|
||||
horizonLevelStrength *= horizonStickAttenuation * pidRuntime.horizonGain;
|
||||
}
|
||||
|
||||
return constrainf(horizonLevelStrength, 0, 1);
|
||||
return horizonLevelStrength;
|
||||
}
|
||||
|
||||
// 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
|
||||
// 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,
|
||||
float currentPidSetpoint, float horizonLevelStrength)
|
||||
float rawSetpoint, float horizonLevelStrength, bool newRcFrame)
|
||||
{
|
||||
const float levelAngleLimit = pidProfile->levelAngleLimit;
|
||||
// calculate error angle and limit the angle to the max inclination
|
||||
// rcDeflection is in range [-1.0, 1.0]
|
||||
float angle = levelAngleLimit * getLevelModeRcDeflection(axis);
|
||||
#ifdef USE_GPS_RESCUE
|
||||
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
|
||||
#endif
|
||||
angle = constrainf(angle, -levelAngleLimit, levelAngleLimit);
|
||||
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
|
||||
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
|
||||
// ANGLE mode - control is angle based
|
||||
const float setpointCorrection = errorAngle * pidRuntime.levelGain;
|
||||
currentPidSetpoint = pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
|
||||
} else {
|
||||
// HORIZON mode - mix of ANGLE and ACRO modes
|
||||
// mix in errorAngle to currentPidSetpoint to add a little auto-level feel
|
||||
const float setpointCorrection = errorAngle * pidRuntime.horizonGain * horizonLevelStrength;
|
||||
currentPidSetpoint += pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
|
||||
const float angleLimit = pidProfile->angle_limit;
|
||||
|
||||
// ** angle loop feedforward
|
||||
float angleFeedforward = 0.0f;
|
||||
#ifdef USE_FEEDFORWARD
|
||||
const float rxRateHz = 1e6f / getCurrentRxRefreshRate();
|
||||
const float rcCommandDeltaAbs = fabsf(getRcCommandDelta(axis));
|
||||
|
||||
if (newRcFrame){
|
||||
// this runs at Rx link rate
|
||||
const float angleTargetRaw = angleLimit * getAngleModeStickInputRaw(axis);
|
||||
float angleFeedforwardInput = 0.0f;
|
||||
|
||||
if (rcCommandDeltaAbs) {
|
||||
// there is movement, so calculate a Delta value to get feedforward
|
||||
pidRuntime.angleTargetDelta[axis] = (angleTargetRaw - pidRuntime.angleTargetPrevious[axis]);
|
||||
angleFeedforwardInput = pidRuntime.angleTargetDelta[axis];
|
||||
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(
|
||||
|
|
@ -476,7 +519,7 @@ static FAST_CODE_NOINLINE void handleCrashRecovery(
|
|||
if (sensors(SENSOR_ACC)) {
|
||||
// errorAngle is deviation from horizontal
|
||||
const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
|
||||
*currentPidSetpoint = errorAngle * pidRuntime.levelGain;
|
||||
*currentPidSetpoint = errorAngle * pidRuntime.angleGain;
|
||||
*errorRate = *currentPidSetpoint - gyroRate;
|
||||
}
|
||||
}
|
||||
|
|
@ -732,7 +775,11 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
|
|||
|
||||
if (pidRuntime.itermRelax) {
|
||||
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 =
|
||||
((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
|
||||
if ((pidRuntime.itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
|
||||
|
|
@ -932,23 +979,42 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
rpmFilterUpdate();
|
||||
#endif
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
const bool newRcFrame = getShouldUpdateFeedforward();
|
||||
#endif
|
||||
const bool newRcFrame = getShouldUpdateFeedforward();
|
||||
|
||||
// ----------PID controller----------
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
|
||||
float currentPidSetpoint = getSetpointRate(axis);
|
||||
float rawSetpoint = getRawSetpoint(axis);
|
||||
bool rawSetpointIsSmoothed = false;
|
||||
if (pidRuntime.maxVelocity[axis]) {
|
||||
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
|
||||
}
|
||||
// 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
|
||||
#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)
|
||||
|| (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);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1031,7 +1097,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
// -----calculate pidSetpointDelta
|
||||
float pidSetpointDelta = 0;
|
||||
#ifdef USE_FEEDFORWARD
|
||||
pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging);
|
||||
pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging, rawSetpoint, rawSetpointIsSmoothed);
|
||||
#endif
|
||||
pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
|
||||
|
|
@ -1103,8 +1169,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
// no feedforward in launch control
|
||||
float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
|
||||
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
|
||||
float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
|
||||
|
||||
|
|
|
|||
|
|
@ -146,10 +146,10 @@ typedef struct pidProfile_s {
|
|||
uint16_t pidSumLimit;
|
||||
uint16_t pidSumLimitYaw;
|
||||
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_tilt_expert_mode; // OFF or ON
|
||||
uint8_t horizon_limit_degrees; // in Horizon mode, zero levelling when the quad's attitude exceeds this angle
|
||||
uint8_t horizon_ignore_sticks; // 0 = default, meaning both stick and attitude attenuation; 1 = only attitude attenuation
|
||||
|
||||
// Betaflight PID controller parameters
|
||||
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_rate; // Percent reduction in P or D at full throttle
|
||||
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;
|
||||
|
||||
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
|
||||
|
|
@ -294,12 +296,12 @@ typedef struct pidRuntime_s {
|
|||
uint8_t antiGravityGain;
|
||||
float antiGravityPGain;
|
||||
pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
|
||||
float levelGain;
|
||||
float angleGain;
|
||||
float angleFeedforwardGain;
|
||||
float horizonGain;
|
||||
float horizonTransition;
|
||||
float horizonCutoffDegrees;
|
||||
float horizonFactorRatio;
|
||||
uint8_t horizonTiltExpertMode;
|
||||
float horizonLimitSticks;
|
||||
float horizonLimitDegrees;
|
||||
uint8_t horizonIgnoreSticks;
|
||||
float maxVelocity[XYZ_AXIS_COUNT];
|
||||
float itermWindupPointInv;
|
||||
bool inCrashRecoveryMode;
|
||||
|
|
@ -396,10 +398,18 @@ typedef struct pidRuntime_s {
|
|||
float feedforwardSmoothFactor;
|
||||
float feedforwardJitterFactor;
|
||||
float feedforwardBoostFactor;
|
||||
float angleFeedforward[XYZ_AXIS_COUNT];
|
||||
float angleTargetPrevious[XYZ_AXIS_COUNT];
|
||||
float angleTargetDelta[XYZ_AXIS_COUNT];
|
||||
uint8_t angleDuplicateCount[XYZ_AXIS_COUNT];
|
||||
#endif
|
||||
|
||||
#ifdef USE_ACC
|
||||
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
|
||||
pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
|
||||
float angleYawSetpoint;
|
||||
float angleEarthRef;
|
||||
float angleTarget[2];
|
||||
#endif
|
||||
} 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 rotateItermAndAxisError();
|
||||
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);
|
||||
#endif
|
||||
void dynLpfDTermUpdate(float throttle);
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@
|
|||
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f
|
||||
#endif
|
||||
|
||||
#define ATTITUDE_CUTOFF_HZ 250
|
||||
#define ATTITUDE_CUTOFF_HZ 50
|
||||
|
||||
static void pidSetTargetLooptime(uint32_t pidLooptime)
|
||||
{
|
||||
|
|
@ -237,8 +237,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
|
||||
#ifdef USE_ACC
|
||||
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
|
||||
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
|
||||
pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -291,12 +295,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
{
|
||||
pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
|
||||
}
|
||||
pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
||||
pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
|
||||
pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
|
||||
pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
|
||||
pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
|
||||
pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
|
||||
pidRuntime.angleGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
||||
pidRuntime.angleFeedforwardGain = pidProfile->pid[PID_LEVEL].F / 100.0f;
|
||||
pidRuntime.angleEarthRef = pidProfile->angle_earth_ref / 100.0f;
|
||||
|
||||
pidRuntime.horizonGain = MIN(pidProfile->pid[PID_LEVEL].I / 100.0f, 1.0f);
|
||||
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_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
|
||||
pidRuntime.itermWindupPointInv = 1.0f;
|
||||
|
|
@ -426,6 +433,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
}
|
||||
pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
|
||||
pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
|
||||
|
||||
feedforwardInit(pidProfile);
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -1943,7 +1943,7 @@ case MSP_NAME:
|
|||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
|
||||
sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
|
||||
sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
|
||||
sbufWriteU8(dst, currentPidProfile->angle_limit);
|
||||
sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
|
||||
sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold
|
||||
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->yawRateAccelLimit = sbufReadU16(src);
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
currentPidProfile->levelAngleLimit = sbufReadU8(src);
|
||||
currentPidProfile->angle_limit = sbufReadU8(src);
|
||||
sbufReadU8(src); // was pidProfile.levelSensitivity
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 4) {
|
||||
|
|
|
|||
|
|
@ -28,6 +28,10 @@ bool simulatedAirmodeEnabled = true;
|
|||
float simulatedSetpointRate[3] = { 0,0,0 };
|
||||
float simulatedPrevSetpointRate[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;
|
||||
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
|
|
@ -84,6 +88,11 @@ extern "C" {
|
|||
void systemBeep(bool) { }
|
||||
bool gyroOverflowDetected(void) { return false; }
|
||||
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) { }
|
||||
bool isLaunchControlActive(void) {return unitLaunchControlActive; }
|
||||
void disarm(flightLogDisarmReason_e) { }
|
||||
|
|
@ -95,10 +104,11 @@ extern "C" {
|
|||
return value;
|
||||
}
|
||||
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(feedforwardAveraging);
|
||||
UNUSED(setpoint);
|
||||
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
|
||||
float setpointDelta = simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
|
||||
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_PITCH] = { 58, 50, 35, 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'
|
||||
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->itermWindupPointPercent = 50;
|
||||
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
|
||||
pidProfile->levelAngleLimit = 55;
|
||||
pidProfile->angle_limit = 60;
|
||||
pidProfile->feedforward_transition = 100;
|
||||
pidProfile->yawRateAccelLimit = 100;
|
||||
pidProfile->rateAccelLimit = 0;
|
||||
|
|
@ -153,8 +163,8 @@ void setDefaultTestSettings(void)
|
|||
pidProfile->crash_gthreshold = 400;
|
||||
pidProfile->crash_setpoint_threshold = 350;
|
||||
pidProfile->crash_recovery = PID_CRASH_RECOVERY_OFF;
|
||||
pidProfile->horizon_tilt_effect = 75;
|
||||
pidProfile->horizon_tilt_expert_mode = false;
|
||||
pidProfile->horizon_limit_degrees = 135;
|
||||
pidProfile->horizon_ignore_sticks = false;
|
||||
pidProfile->crash_limit_yaw = 200;
|
||||
pidProfile->itermLimit = 150;
|
||||
pidProfile->throttle_boost = 0;
|
||||
|
|
@ -194,6 +204,7 @@ void resetTest(void)
|
|||
pidData[axis].Sum = 0;
|
||||
simulatedSetpointRate[axis] = 0;
|
||||
simulatedRcDeflection[axis] = 0;
|
||||
simulatedRawSetpoint[axis] = 0;
|
||||
gyro.gyroADCf[axis] = 0;
|
||||
}
|
||||
attitude.values.roll = 0;
|
||||
|
|
@ -381,39 +392,38 @@ TEST(pidControllerTest, testPidLevel)
|
|||
float currentPidSetpoint = 30;
|
||||
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);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
|
||||
|
||||
// Test attitude response
|
||||
setStickPosition(FD_ROLL, 1.0f);
|
||||
setStickPosition(FD_PITCH, -1.0f);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(244.07211, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(-244.07211, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(174.39539, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(-174.39539, currentPidSetpoint);
|
||||
|
||||
setStickPosition(FD_ROLL, -0.5f);
|
||||
setStickPosition(FD_PITCH, 0.5f);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(-93.487915, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(93.487915, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(4.0751495, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(-4.0751495, currentPidSetpoint);
|
||||
|
||||
attitude.values.roll = -275;
|
||||
attitude.values.pitch = 275;
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(-12.047981, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(-19.130268, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(19.130268, currentPidSetpoint);
|
||||
|
||||
// Disable ANGLE_MODE
|
||||
disableFlightMode(ANGLE_MODE);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(11.07958, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(14.693689, currentPidSetpoint);
|
||||
|
||||
// Test level mode expo
|
||||
enableFlightMode(ANGLE_MODE);
|
||||
|
|
@ -423,10 +433,10 @@ TEST(pidControllerTest, testPidLevel)
|
|||
setStickPosition(FD_PITCH, -0.5f);
|
||||
currentControlRateProfile->levelExpo[FD_ROLL] = 50;
|
||||
currentControlRateProfile->levelExpo[FD_PITCH] = 26;
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(76.208672, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
|
||||
EXPECT_FLOAT_EQ(-98.175163, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(45.520832, currentPidSetpoint);
|
||||
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
|
||||
EXPECT_FLOAT_EQ(-61.216412, currentPidSetpoint);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -437,21 +447,79 @@ TEST(pidControllerTest, testPidHorizon)
|
|||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
enableFlightMode(HORIZON_MODE);
|
||||
|
||||
// Test full stick response
|
||||
setStickPosition(FD_ROLL, 1.0f);
|
||||
setStickPosition(FD_PITCH, -1.0f);
|
||||
// Test stick response greater than default limit of 0.75
|
||||
setStickPosition(FD_ROLL, 0.76f);
|
||||
setStickPosition(FD_PITCH, -0.76f);
|
||||
EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
|
||||
|
||||
// Expect full rate output on full stick
|
||||
// Test zero stick response
|
||||
setStickPosition(FD_ROLL, 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_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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue