mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 12:55:19 +03:00
Merge pull request #11779 from haslinghuis/update-pidprofile
Move TPA to PID profile
This commit is contained in:
commit
cbbc80ee90
11 changed files with 59 additions and 42 deletions
|
@ -1327,8 +1327,8 @@ static bool blackboxWriteSysinfo(void)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d", activePidLoopDenom);
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d", activePidLoopDenom);
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d", currentControlRateProfile->thrMid8);
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d", currentControlRateProfile->thrMid8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d", currentControlRateProfile->thrExpo8);
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d", currentControlRateProfile->thrExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentControlRateProfile->tpa_rate);
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentPidProfile->tpa_rate);
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentControlRateProfile->tpa_breakpoint);
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentPidProfile->tpa_breakpoint);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
|
BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
|
||||||
currentControlRateProfile->rcRates[PITCH],
|
currentControlRateProfile->rcRates[PITCH],
|
||||||
currentControlRateProfile->rcRates[YAW]);
|
currentControlRateProfile->rcRates[YAW]);
|
||||||
|
|
|
@ -975,11 +975,6 @@ const clivalue_t valueTable[] = {
|
||||||
{ "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_ROLL]) },
|
{ "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_ROLL]) },
|
||||||
{ "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_PITCH]) },
|
{ "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_PITCH]) },
|
||||||
{ "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_YAW]) },
|
{ "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_YAW]) },
|
||||||
{ PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_TPA_MAX}, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_rate) },
|
|
||||||
{ PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_breakpoint) },
|
|
||||||
#ifdef USE_TPA_MODE
|
|
||||||
{ "tpa_mode", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpaMode) },
|
|
||||||
#endif
|
|
||||||
{ PARAM_NAME_THROTTLE_LIMIT_TYPE, VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_THROTTLE_LIMIT_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_type) },
|
{ PARAM_NAME_THROTTLE_LIMIT_TYPE, VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_THROTTLE_LIMIT_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_type) },
|
||||||
{ PARAM_NAME_THROTTLE_LIMIT_PERCENT, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) },
|
{ PARAM_NAME_THROTTLE_LIMIT_PERCENT, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) },
|
||||||
{ "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]) },
|
||||||
|
@ -1065,6 +1060,9 @@ const clivalue_t valueTable[] = {
|
||||||
{ "runaway_takeoff_deactivate_delay", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 1000 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_delay) }, // deactivate time in ms
|
{ "runaway_takeoff_deactivate_delay", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 1000 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_delay) }, // deactivate time in ms
|
||||||
{ "runaway_takeoff_deactivate_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
|
{ "runaway_takeoff_deactivate_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef USE_TPA_MODE
|
||||||
|
{ PARAM_NAME_TPA_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_CONFIG, offsetof(pidConfig_t, tpaMode) },
|
||||||
|
#endif
|
||||||
|
|
||||||
// PG_PID_PROFILE
|
// PG_PID_PROFILE
|
||||||
#ifdef USE_PROFILE_NAMES
|
#ifdef USE_PROFILE_NAMES
|
||||||
|
@ -1220,6 +1218,8 @@ const clivalue_t valueTable[] = {
|
||||||
{ PARAM_NAME_SIMPLIFIED_GYRO_FILTER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter) },
|
{ PARAM_NAME_SIMPLIFIED_GYRO_FILTER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter) },
|
||||||
{ PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_FILTERS_MIN, SIMPLIFIED_TUNING_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter_multiplier) },
|
{ PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_FILTERS_MIN, SIMPLIFIED_TUNING_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter_multiplier) },
|
||||||
#endif
|
#endif
|
||||||
|
{ PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_rate) },
|
||||||
|
{ PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) },
|
||||||
|
|
||||||
// PG_TELEMETRY_CONFIG
|
// PG_TELEMETRY_CONFIG
|
||||||
#ifdef USE_TELEMETRY
|
#ifdef USE_TELEMETRY
|
||||||
|
|
|
@ -246,6 +246,8 @@ static uint8_t cmsx_simplified_dterm_filter;
|
||||||
static uint8_t cmsx_simplified_dterm_filter_multiplier;
|
static uint8_t cmsx_simplified_dterm_filter_multiplier;
|
||||||
static uint8_t cmsx_simplified_gyro_filter;
|
static uint8_t cmsx_simplified_gyro_filter;
|
||||||
static uint8_t cmsx_simplified_gyro_filter_multiplier;
|
static uint8_t cmsx_simplified_gyro_filter_multiplier;
|
||||||
|
static uint8_t cmsx_tpa_rate;
|
||||||
|
static uint16_t cmsx_tpa_breakpoint;
|
||||||
|
|
||||||
static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
|
static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
|
||||||
{
|
{
|
||||||
|
@ -410,8 +412,6 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] =
|
||||||
|
|
||||||
{ "THR MID", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8, 0, 100, 1} },
|
{ "THR MID", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8, 0, 100, 1} },
|
||||||
{ "THR EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8, 0, 100, 1} },
|
{ "THR EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8, 0, 100, 1} },
|
||||||
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.tpa_rate, 0, 100, 1, 10} },
|
|
||||||
{ "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10} },
|
|
||||||
|
|
||||||
{ "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} },
|
{ "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} },
|
||||||
{ "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1} },
|
{ "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1} },
|
||||||
|
@ -625,6 +625,9 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
|
||||||
pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
|
pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
pidProfile->tpa_rate = cmsx_tpa_rate;
|
||||||
|
pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
|
||||||
|
|
||||||
initEscEndpoints();
|
initEscEndpoints();
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
@ -675,6 +678,9 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
|
||||||
{ "VBAT_SAG_COMP", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
|
{ "VBAT_SAG_COMP", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate, 0, 100, 1, 10} },
|
||||||
|
{ "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint, 1000, 2000, 10} },
|
||||||
|
|
||||||
{ "BACK", OME_Back, NULL, NULL },
|
{ "BACK", OME_Back, NULL, NULL },
|
||||||
{ NULL, OME_END, NULL, NULL}
|
{ NULL, OME_END, NULL, NULL}
|
||||||
};
|
};
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
|
|
||||||
controlRateConfig_t *currentControlRateProfile;
|
controlRateConfig_t *currentControlRateProfile;
|
||||||
|
|
||||||
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4);
|
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 5);
|
||||||
|
|
||||||
void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
|
void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
|
||||||
{
|
{
|
||||||
|
@ -45,8 +45,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
|
||||||
RESET_CONFIG(controlRateConfig_t, &controlRateConfig[i],
|
RESET_CONFIG(controlRateConfig_t, &controlRateConfig[i],
|
||||||
.thrMid8 = 50,
|
.thrMid8 = 50,
|
||||||
.thrExpo8 = 0,
|
.thrExpo8 = 0,
|
||||||
.tpa_rate = 65,
|
|
||||||
.tpa_breakpoint = 1350,
|
|
||||||
.rates_type = RATES_TYPE_ACTUAL,
|
.rates_type = RATES_TYPE_ACTUAL,
|
||||||
.rcRates[FD_ROLL] = 7,
|
.rcRates[FD_ROLL] = 7,
|
||||||
.rcRates[FD_PITCH] = 7,
|
.rcRates[FD_PITCH] = 7,
|
||||||
|
@ -62,7 +60,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
|
||||||
.rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
.rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
||||||
.rate_limit[FD_PITCH] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
.rate_limit[FD_PITCH] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
||||||
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
||||||
.tpaMode = TPA_MODE_D,
|
|
||||||
.profileName = { 0 },
|
.profileName = { 0 },
|
||||||
.quickRatesRcExpo = 0,
|
.quickRatesRcExpo = 0,
|
||||||
.levelExpo[FD_ROLL] = 0,
|
.levelExpo[FD_ROLL] = 0,
|
||||||
|
|
|
@ -46,11 +46,6 @@ typedef enum {
|
||||||
THROTTLE_LIMIT_TYPE_COUNT // must be the last entry
|
THROTTLE_LIMIT_TYPE_COUNT // must be the last entry
|
||||||
} throttleLimitType_e;
|
} throttleLimitType_e;
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
TPA_MODE_PD,
|
|
||||||
TPA_MODE_D
|
|
||||||
} tpaMode_e;
|
|
||||||
|
|
||||||
#define MAX_RATE_PROFILE_NAME_LENGTH 8u
|
#define MAX_RATE_PROFILE_NAME_LENGTH 8u
|
||||||
|
|
||||||
typedef struct controlRateConfig_s {
|
typedef struct controlRateConfig_s {
|
||||||
|
@ -60,12 +55,9 @@ typedef struct controlRateConfig_s {
|
||||||
uint8_t rcRates[3];
|
uint8_t rcRates[3];
|
||||||
uint8_t rcExpo[3];
|
uint8_t rcExpo[3];
|
||||||
uint8_t rates[3];
|
uint8_t rates[3];
|
||||||
uint8_t tpa_rate; // Percent reduction in P or D at full throttle
|
|
||||||
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
|
|
||||||
uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip
|
uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip
|
||||||
uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit
|
uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit
|
||||||
uint16_t rate_limit[3]; // Sets the maximum rate for the axes
|
uint16_t rate_limit[3]; // Sets the maximum rate for the axes
|
||||||
uint8_t tpaMode; // Controls which PID terms TPA effects
|
|
||||||
char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile
|
char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile
|
||||||
uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates
|
uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates
|
||||||
uint8_t levelExpo[2]; // roll/pitch level mode expo
|
uint8_t levelExpo[2]; // roll/pitch level mode expo
|
||||||
|
|
|
@ -54,6 +54,7 @@
|
||||||
#define PARAM_NAME_RATES_TYPE "rates_type"
|
#define PARAM_NAME_RATES_TYPE "rates_type"
|
||||||
#define PARAM_NAME_TPA_RATE "tpa_rate"
|
#define PARAM_NAME_TPA_RATE "tpa_rate"
|
||||||
#define PARAM_NAME_TPA_BREAKPOINT "tpa_breakpoint"
|
#define PARAM_NAME_TPA_BREAKPOINT "tpa_breakpoint"
|
||||||
|
#define PARAM_NAME_TPA_MODE "tpa_mode"
|
||||||
#define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type"
|
#define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type"
|
||||||
#define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"
|
#define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"
|
||||||
#define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm"
|
#define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm"
|
||||||
|
|
|
@ -82,8 +82,6 @@ typedef enum {
|
||||||
// (Super) rates are constrained to [0, 100] for Betaflight rates, so values higher than 100 won't make a difference. Range extended for RaceFlight rates.
|
// (Super) rates are constrained to [0, 100] for Betaflight rates, so values higher than 100 won't make a difference. Range extended for RaceFlight rates.
|
||||||
#define CONTROL_RATE_CONFIG_RATE_MAX 255
|
#define CONTROL_RATE_CONFIG_RATE_MAX 255
|
||||||
|
|
||||||
#define CONTROL_RATE_CONFIG_TPA_MAX 100
|
|
||||||
|
|
||||||
extern float rcCommand[4];
|
extern float rcCommand[4];
|
||||||
|
|
||||||
typedef struct rcSmoothingFilterTraining_s {
|
typedef struct rcSmoothingFilterTraining_s {
|
||||||
|
|
|
@ -31,6 +31,7 @@
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
|
||||||
|
#include "config/config.h"
|
||||||
#include "config/config_reset.h"
|
#include "config/config_reset.h"
|
||||||
#include "config/simplified_tuning.h"
|
#include "config/simplified_tuning.h"
|
||||||
|
|
||||||
|
@ -87,7 +88,7 @@ FAST_DATA_ZERO_INIT float throttleBoost;
|
||||||
pt1Filter_t throttleLpf;
|
pt1Filter_t throttleLpf;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
|
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 4);
|
||||||
|
|
||||||
#if defined(STM32F411xE)
|
#if defined(STM32F411xE)
|
||||||
#define PID_PROCESS_DENOM_DEFAULT 2
|
#define PID_PROCESS_DENOM_DEFAULT 2
|
||||||
|
@ -100,11 +101,13 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
||||||
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
|
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
|
||||||
.runaway_takeoff_prevention = true,
|
.runaway_takeoff_prevention = true,
|
||||||
.runaway_takeoff_deactivate_throttle = 20, // throttle level % needed to accumulate deactivation time
|
.runaway_takeoff_deactivate_throttle = 20, // throttle level % needed to accumulate deactivation time
|
||||||
.runaway_takeoff_deactivate_delay = 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
|
.runaway_takeoff_deactivate_delay = 500, // Accumulated time (in milliseconds) before deactivation in successful takeoff
|
||||||
|
.tpaMode = TPA_MODE_D
|
||||||
);
|
);
|
||||||
#else
|
#else
|
||||||
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
||||||
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT
|
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
|
||||||
|
.tpaMode = TPA_MODE_D
|
||||||
);
|
);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -117,7 +120,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
||||||
|
|
||||||
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
|
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
|
||||||
|
|
||||||
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
|
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
|
||||||
|
|
||||||
void resetPidProfile(pidProfile_t *pidProfile)
|
void resetPidProfile(pidProfile_t *pidProfile)
|
||||||
{
|
{
|
||||||
|
@ -218,6 +221,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
||||||
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
|
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
|
||||||
.anti_gravity_cutoff_hz = 5,
|
.anti_gravity_cutoff_hz = 5,
|
||||||
.anti_gravity_p_gain = 100,
|
.anti_gravity_p_gain = 100,
|
||||||
|
.tpa_rate = 65,
|
||||||
|
.tpa_breakpoint = 1350,
|
||||||
);
|
);
|
||||||
|
|
||||||
#ifndef USE_D_MIN
|
#ifndef USE_D_MIN
|
||||||
|
@ -289,8 +294,12 @@ void pidResetIterm(void)
|
||||||
|
|
||||||
void pidUpdateTpaFactor(float throttle)
|
void pidUpdateTpaFactor(float throttle)
|
||||||
{
|
{
|
||||||
const float tpaBreakpoint = (currentControlRateProfile->tpa_breakpoint - 1000) / 1000.0f;
|
pidProfile_t *currentPidProfile;
|
||||||
float tpaRate = currentControlRateProfile->tpa_rate / 100.0f;
|
|
||||||
|
currentPidProfile = pidProfilesMutable(systemConfig()->pidProfileIndex);
|
||||||
|
const float tpaBreakpoint = (currentPidProfile->tpa_breakpoint - 1000) / 1000.0f;
|
||||||
|
float tpaRate = currentPidProfile->tpa_rate / 100.0f;
|
||||||
|
|
||||||
if (throttle > tpaBreakpoint) {
|
if (throttle > tpaBreakpoint) {
|
||||||
if (throttle < 1.0f) {
|
if (throttle < 1.0f) {
|
||||||
tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint);
|
tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint);
|
||||||
|
@ -825,7 +834,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
||||||
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
|
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
#ifdef USE_TPA_MODE
|
#ifdef USE_TPA_MODE
|
||||||
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
|
const float tpaFactorKp = (pidConfig()->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
|
||||||
#else
|
#else
|
||||||
const float tpaFactorKp = pidRuntime.tpaFactor;
|
const float tpaFactorKp = pidRuntime.tpaFactor;
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -68,6 +68,13 @@
|
||||||
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
|
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
|
||||||
#define DTERM_LPF2_HZ_DEFAULT 150
|
#define DTERM_LPF2_HZ_DEFAULT 150
|
||||||
|
|
||||||
|
#define TPA_MAX 100
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
TPA_MODE_PD,
|
||||||
|
TPA_MODE_D
|
||||||
|
} tpaMode_e;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
PID_ROLL,
|
PID_ROLL,
|
||||||
PID_PITCH,
|
PID_PITCH,
|
||||||
|
@ -223,15 +230,18 @@ typedef struct pidProfile_s {
|
||||||
|
|
||||||
uint8_t anti_gravity_cutoff_hz;
|
uint8_t anti_gravity_cutoff_hz;
|
||||||
uint8_t anti_gravity_p_gain;
|
uint8_t anti_gravity_p_gain;
|
||||||
|
uint8_t tpa_rate; // Percent reduction in P or D at full throttle
|
||||||
|
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
|
||||||
} pidProfile_t;
|
} pidProfile_t;
|
||||||
|
|
||||||
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
|
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
|
||||||
|
|
||||||
typedef struct pidConfig_s {
|
typedef struct pidConfig_s {
|
||||||
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
|
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
|
||||||
uint8_t runaway_takeoff_prevention; // off, on - enables pidsum runaway disarm logic
|
uint8_t runaway_takeoff_prevention; // off, on - enables pidsum runaway disarm logic
|
||||||
uint16_t runaway_takeoff_deactivate_delay; // delay in ms for "in-flight" conditions before deactivation (successful flight)
|
uint16_t runaway_takeoff_deactivate_delay; // delay in ms for "in-flight" conditions before deactivation (successful flight)
|
||||||
uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
|
uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
|
||||||
|
uint8_t tpaMode; // Controls which PID terms TPA effects
|
||||||
} pidConfig_t;
|
} pidConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(pidConfig_t, pidConfig);
|
PG_DECLARE(pidConfig_t, pidConfig);
|
||||||
|
@ -390,6 +400,8 @@ typedef struct pidRuntime_s {
|
||||||
#ifdef USE_ACC
|
#ifdef USE_ACC
|
||||||
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
|
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
|
||||||
#endif
|
#endif
|
||||||
|
uint8_t tpa_rate;
|
||||||
|
uint16_t tpa_breakpoint;
|
||||||
} pidRuntime_t;
|
} pidRuntime_t;
|
||||||
|
|
||||||
extern pidRuntime_t pidRuntime;
|
extern pidRuntime_t pidRuntime;
|
||||||
|
|
|
@ -1353,10 +1353,10 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
|
||||||
for (int i = 0 ; i < 3; i++) {
|
for (int i = 0 ; i < 3; i++) {
|
||||||
sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
|
sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
|
||||||
}
|
}
|
||||||
sbufWriteU8(dst, currentControlRateProfile->tpa_rate);
|
sbufWriteU8(dst, 0); // was currentControlRateProfile->tpa_rate
|
||||||
sbufWriteU8(dst, currentControlRateProfile->thrMid8);
|
sbufWriteU8(dst, currentControlRateProfile->thrMid8);
|
||||||
sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
|
sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
|
||||||
sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
|
sbufWriteU16(dst, 0); // was currentControlRateProfile->tpa_breakpoint
|
||||||
sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
|
sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
|
||||||
sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
|
sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
|
||||||
sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
|
sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
|
||||||
|
@ -2014,6 +2014,8 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
|
||||||
#else
|
#else
|
||||||
sbufWriteU8(dst, 0);
|
sbufWriteU8(dst, 0);
|
||||||
#endif
|
#endif
|
||||||
|
sbufWriteU8(dst, currentPidProfile->tpa_rate);
|
||||||
|
sbufWriteU16(dst, currentPidProfile->tpa_breakpoint); // was currentControlRateProfile->tpa_breakpoint
|
||||||
break;
|
break;
|
||||||
case MSP_SENSOR_CONFIG:
|
case MSP_SENSOR_CONFIG:
|
||||||
#if defined(USE_ACC)
|
#if defined(USE_ACC)
|
||||||
|
@ -2666,11 +2668,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
||||||
currentControlRateProfile->rates[i] = sbufReadU8(src);
|
currentControlRateProfile->rates[i] = sbufReadU8(src);
|
||||||
}
|
}
|
||||||
|
|
||||||
value = sbufReadU8(src);
|
sbufReadU8(src); // tpa_rate is moved to PID profile
|
||||||
currentControlRateProfile->tpa_rate = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
|
|
||||||
currentControlRateProfile->thrMid8 = sbufReadU8(src);
|
currentControlRateProfile->thrMid8 = sbufReadU8(src);
|
||||||
currentControlRateProfile->thrExpo8 = sbufReadU8(src);
|
currentControlRateProfile->thrExpo8 = sbufReadU8(src);
|
||||||
currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
|
sbufReadU16(src); // tpa_breakpoint is moved to PID profile
|
||||||
|
|
||||||
if (sbufBytesRemaining(src) >= 1) {
|
if (sbufBytesRemaining(src) >= 1) {
|
||||||
currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
|
currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
|
||||||
|
@ -3135,6 +3136,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
||||||
sbufReadU8(src);
|
sbufReadU8(src);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
if (sbufBytesRemaining(src) >= 3) {
|
||||||
|
// Added in API 1.45
|
||||||
|
value = sbufReadU8(src);
|
||||||
|
currentPidProfile->tpa_rate = MIN(value, TPA_MAX);
|
||||||
|
currentPidProfile->tpa_breakpoint = sbufReadU16(src);
|
||||||
|
}
|
||||||
|
|
||||||
pidInitConfig(currentPidProfile);
|
pidInitConfig(currentPidProfile);
|
||||||
initEscEndpoints();
|
initEscEndpoints();
|
||||||
mixerInitProfile();
|
mixerInitProfile();
|
||||||
|
|
|
@ -267,9 +267,7 @@ protected:
|
||||||
.thrMid8 = 0,
|
.thrMid8 = 0,
|
||||||
.thrExpo8 = 0,
|
.thrExpo8 = 0,
|
||||||
.rates = {0, 0, 0},
|
.rates = {0, 0, 0},
|
||||||
.tpa_rate = 0,
|
|
||||||
.rcExpo[FD_YAW] = 0,
|
.rcExpo[FD_YAW] = 0,
|
||||||
.tpa_breakpoint = 0
|
|
||||||
};
|
};
|
||||||
|
|
||||||
channelRange_t fullRange = {
|
channelRange_t fullRange = {
|
||||||
|
@ -295,8 +293,6 @@ protected:
|
||||||
controlRateConfig.rates[0] = 0;
|
controlRateConfig.rates[0] = 0;
|
||||||
controlRateConfig.rates[1] = 0;
|
controlRateConfig.rates[1] = 0;
|
||||||
controlRateConfig.rates[2] = 0;
|
controlRateConfig.rates[2] = 0;
|
||||||
controlRateConfig.tpa_rate = 0;
|
|
||||||
controlRateConfig.tpa_breakpoint = 0;
|
|
||||||
|
|
||||||
PG_RESET(adjustmentRanges);
|
PG_RESET(adjustmentRanges);
|
||||||
adjustmentRangesIndex = 0;
|
adjustmentRangesIndex = 0;
|
||||||
|
@ -373,9 +369,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
|
||||||
.thrMid8 = 0,
|
.thrMid8 = 0,
|
||||||
.thrExpo8 = 0,
|
.thrExpo8 = 0,
|
||||||
.rates = {0,0,0},
|
.rates = {0,0,0},
|
||||||
.tpa_rate = 0,
|
|
||||||
.rcExpo[FD_YAW] = 0,
|
.rcExpo[FD_YAW] = 0,
|
||||||
.tpa_breakpoint = 0
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// and
|
// and
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue