diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 534cac81b..66d97cd23 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -403,7 +403,7 @@ static OSD_Entry cmsx_menuImuEntries[] = {"MISC PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, {"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, - {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, + {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}, 0}, {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 655839a7f..49cee1d84 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -309,7 +309,7 @@ typedef struct master_s { #endif profile_t profile[MAX_PROFILE_COUNT]; - controlRateConfig_t controlRateProfile[MAX_RATEPROFILES]; + controlRateConfig_t controlRateProfile[CONTROL_RATE_PROFILE_COUNT]; modeActivationProfile_t modeActivationProfile; adjustmentProfile_t adjustmentProfile; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 71b79f3ff..531075dec 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3422,7 +3422,7 @@ static void cliRateProfile(char *cmdline) return; } else { const int i = atoi(cmdline); - if (i >= 0 && i < MAX_RATEPROFILES) { + if (i >= 0 && i < CONTROL_RATE_PROFILE_COUNT) { changeControlRateProfile(i); cliRateProfile(""); } @@ -3451,7 +3451,7 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_ static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, const master_t *defaultConfig) { - if (rateProfileIndex >= MAX_RATEPROFILES) { + if (rateProfileIndex >= CONTROL_RATE_PROFILE_COUNT) { // Faulty values return; } @@ -4136,7 +4136,7 @@ static void printConfig(char *cmdline, bool doDiff) cliProfile(""); const uint8_t controlRateProfileIndexSave = getCurrentControlRateProfileIndex(); - for (uint32_t rateIndex = 0; rateIndex < MAX_RATEPROFILES; rateIndex++) { + for (uint32_t rateIndex = 0; rateIndex < CONTROL_RATE_PROFILE_COUNT; rateIndex++) { cliDumpRateProfile(rateIndex, dumpMask, &defaultConfig); } changeControlRateProfile(controlRateProfileIndexSave); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 291a7a6cd..26576f651 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -181,6 +181,7 @@ static void resetControlRateProfile(controlRateConfig_t *controlRateConfig) } #endif +#ifndef USE_PARAMETER_GROUPS static void resetPidProfile(pidProfile_t *pidProfile) { pidProfile->P8[ROLL] = 44; @@ -231,11 +232,14 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->itermThrottleThreshold = 350; pidProfile->itermAcceleratorGain = 1.0f; } +#endif +#ifndef USE_PARAMETER_GROUPS void resetProfile(profile_t *profile) { resetPidProfile(&profile->pidProfile); } +#endif #ifdef GPS void resetGpsProfile(gpsProfile_t *gpsProfile) @@ -1011,13 +1015,10 @@ void createDefaultConfig(master_t *config) resetSerialPinConfig(&config->serialPinConfig); resetSerialConfig(&config->serialConfig); -#endif - for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { resetProfile(&config->profile[ii]); } -#ifndef USE_PARAMETER_GROUPS for (int ii = 0; ii < CONTROL_RATE_PROFILE_COUNT; ++ii) { resetControlRateProfile(&config->controlRateProfile[ii]); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 164d06b4b..602536bf6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -30,12 +30,6 @@ #include "drivers/sound_beeper.h" #include "drivers/vcd.h" -#if FLASH_SIZE <= 128 -#define MAX_PROFILE_COUNT 2 -#else -#define MAX_PROFILE_COUNT 3 -#endif -#define MAX_RATEPROFILES 3 #define MAX_NAME_LENGTH 16 typedef enum { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 186bf13d5..8c73f0051 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1275,7 +1275,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } else { value = value & ~RATEPROFILE_MASK; - if (value >= MAX_RATEPROFILES) { + if (value >= CONTROL_RATE_PROFILE_COUNT) { value = 0; } changeControlRateProfile(value); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3a8b7c722..6845c1fcd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -30,6 +31,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "fc/fc_core.h" #include "fc/fc_rc.h" @@ -71,6 +73,63 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, .pid_process_denom = PID_PROCESS_DENOM_DEFAULT ); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 0); + +void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) +{ + for (int i = 0; i < MAX_PROFILE_COUNT; i++) { + RESET_CONFIG(const pidProfile_t, &pidProfiles[i], + .P8[ROLL] = 44, + .I8[ROLL] = 40, + .D8[ROLL] = 20, + .P8[PITCH] = 58, + .I8[PITCH] = 50, + .D8[PITCH] = 22, + .P8[YAW] = 70, + .I8[YAW] = 45, + .D8[YAW] = 20, + .P8[PIDALT] = 50, + .I8[PIDALT] = 0, + .D8[PIDALT] = 0, + .P8[PIDPOS] = 15, // POSHOLD_P * 100, + .I8[PIDPOS] = 0, // POSHOLD_I * 100, + .D8[PIDPOS] = 0, + .P8[PIDPOSR] = 34, // POSHOLD_RATE_P * 10, + .I8[PIDPOSR] = 14, // POSHOLD_RATE_I * 100, + .D8[PIDPOSR] = 53, // POSHOLD_RATE_D * 1000, + .P8[PIDNAVR] = 25, // NAV_P * 10, + .I8[PIDNAVR] = 33, // NAV_I * 100, + .D8[PIDNAVR] = 83, // NAV_D * 1000, + .P8[PIDLEVEL] = 50, + .I8[PIDLEVEL] = 50, + .D8[PIDLEVEL] = 100, + .P8[PIDMAG] = 40, + .P8[PIDVEL] = 55, + .I8[PIDVEL] = 55, + .D8[PIDVEL] = 75, + + .yaw_p_limit = YAW_P_LIMIT_MAX, + .pidSumLimit = PIDSUM_LIMIT, + .yaw_lpf_hz = 0, + .itermWindupPointPercent = 50, + .dterm_filter_type = FILTER_BIQUAD, + .dterm_lpf_hz = 100, // filtering ON by default + .dterm_notch_hz = 260, + .dterm_notch_cutoff = 160, + .vbatPidCompensation = 0, + .pidAtMinThrottle = PID_STABILISATION_ON, + .levelAngleLimit = 55, + .levelSensitivity = 55, + .setpointRelaxRatio = 20, + .dtermSetpointWeight = 100, + .yawRateAccelLimit = 10.0f, + .rateAccelLimit = 0.0f, + .itermThrottleThreshold = 350, + .itermAcceleratorGain = 1.0f + ); + } +} + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index dcc24be33..cd669a621 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -87,6 +87,12 @@ typedef struct pidProfile_s { } pidProfile_t; //PG_DECLARE_PROFILE(pidProfile_t, pidProfile); +#if FLASH_SIZE <= 128 +#define MAX_PROFILE_COUNT 2 +#else +#define MAX_PROFILE_COUNT 3 +#endif +PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index f4302bfbb..297c6a911 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -48,6 +48,7 @@ #include "config/parameter_group_ids.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 739938b86..83049fab8 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -26,6 +26,7 @@ #include "drivers/io.h" #include "fc/rc_controls.h" +#include "fc/controlrate_profile.h" #include "flight/failsafe.h" #include "flight/mixer.h" @@ -72,7 +73,7 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.P8[PIDLEVEL] = 30; config->profile[profileId].pidProfile.D8[PIDLEVEL] = 30; - for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { + for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { config->controlRateProfile[rateProfileId].rcRate8 = 100; config->controlRateProfile[rateProfileId].rcYawRate8 = 110; config->controlRateProfile[rateProfileId].rcExpo8 = 0;