1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00

Merge pull request #11779 from haslinghuis/update-pidprofile

Move TPA to PID profile
This commit is contained in:
haslinghuis 2022-10-13 21:09:49 +02:00 committed by GitHub
commit cbbc80ee90
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
11 changed files with 59 additions and 42 deletions

View file

@ -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]);

View file

@ -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

View file

@ -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}
}; };

View file

@ -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,

View file

@ -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

View file

@ -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"

View file

@ -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 {

View file

@ -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

View file

@ -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;

View file

@ -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();

View file

@ -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