Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Angle mode feedforward and earth referencing of yaw inputs #12067

Closed
wants to merge 1 commit into from
Closed

Angle mode feedforward and earth referencing of yaw inputs #12067

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions src/main/blackbox/blackbox.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1402,7 +1402,13 @@ 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, "%d", currentPidProfile->angle_feedforward_smoothing);
#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_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit);
Expand Down
3 changes: 3 additions & 0 deletions src/main/build/debug.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,4 +108,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"VTX_MSP",
"GPS_DOP",
"FAILSAFE",
"ANGLE_MODE",
"ANGLE_TARGET",
"CURRENT_ANGLE",
};
3 changes: 3 additions & 0 deletions src/main/build/debug.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,9 @@ typedef enum {
DEBUG_VTX_MSP,
DEBUG_GPS_DOP,
DEBUG_FAILSAFE,
DEBUG_ANGLE_MODE,
DEBUG_ANGLE_TARGET,
DEBUG_CURRENT_ANGLE,
DEBUG_COUNT
} debugType_e;

Expand Down
16 changes: 9 additions & 7 deletions src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -978,8 +978,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) },
Expand Down Expand Up @@ -1129,11 +1129,13 @@ 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) },

{ "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) },
{ 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) },
{ "horizon_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
{ 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, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_feedforward_smoothing) },
{ 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) },

{ "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) },
Expand Down
20 changes: 12 additions & 8 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -516,10 +516,11 @@ 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_horizonStrength;
static uint8_t cmsx_horizonTransition;
static uint8_t cmsx_levelAngleLimit;
static uint8_t cmsx_angleLimit;
static uint8_t cmsx_throttleBoost;
static uint8_t cmsx_thrustLinearization;
static uint8_t cmsx_antiGravityGain;
Expand Down Expand Up @@ -560,10 +561,11 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)

const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);

cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
cmsx_angleP = pidProfile->pid[PID_LEVEL].P;
cmsx_angleFF = pidProfile->pid[PID_LEVEL].F;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D;
cmsx_levelAngleLimit = pidProfile->levelAngleLimit;
cmsx_angleLimit = pidProfile->angle_limit;

cmsx_antiGravityGain = pidProfile->anti_gravity_gain;

Expand Down Expand Up @@ -611,10 +613,11 @@ 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].I = cmsx_horizonStrength;
pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition;
pidProfile->levelAngleLimit = cmsx_levelAngleLimit;
pidProfile->pid[PID_LEVEL].F = cmsx_angleFF;
pidProfile->angle_limit = cmsx_angleLimit;

pidProfile->anti_gravity_gain = cmsx_antiGravityGain;

Expand Down Expand Up @@ -665,10 +668,11 @@ 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 } },
{ "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 } },
{ "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 LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleLimit, 10, 90, 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
Expand Down
4 changes: 2 additions & 2 deletions src/main/fc/controlrate_profile.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
);
}
}
Expand Down
7 changes: 7 additions & 0 deletions src/main/fc/parameter_names.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,13 @@
#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 "angle_feedforward_smoothing"
#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"

#ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
Expand Down
5 changes: 5 additions & 0 deletions src/main/fc/rc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,11 @@ float getRcDeflection(int axis)
#endif
}

float getRcDeflectionRaw(int axis)
{
return rcDeflection[axis];
}

float getRcDeflectionAbs(int axis)
{
return rcDeflectionAbs[axis];
Expand Down
1 change: 1 addition & 0 deletions src/main/fc/rc.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading