From aa561d542b23a414e69b8ec2f4335bdc722d8486 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 5 Mar 2017 06:33:25 +0000 Subject: [PATCH] Updates to support parameter groups --- src/main/blackbox/blackbox.c | 90 ++++++------ src/main/cms/cms_menu_imu.c | 44 +++--- src/main/common/utils.h | 2 + src/main/config/config_eeprom.c | 8 + src/main/config/config_master.h | 5 +- src/main/fc/cli.c | 196 +++++++++++++------------ src/main/fc/config.c | 73 ++++----- src/main/fc/config.h | 19 ++- src/main/fc/fc_core.c | 8 +- src/main/fc/fc_init.c | 6 +- src/main/fc/fc_msp.c | 76 +++++----- src/main/fc/fc_rc.c | 6 +- src/main/fc/rc_controls.c | 2 +- src/main/flight/navigation.c | 2 +- src/main/flight/pid.c | 15 +- src/main/flight/pid.h | 1 + src/main/flight/servos.c | 27 ++-- src/main/flight/servos.h | 8 +- src/main/io/dashboard.c | 4 +- src/main/io/osd.c | 10 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 25 ++-- src/main/target/NAZE/config.c | 13 +- src/main/telemetry/smartport.c | 20 ++- 23 files changed, 338 insertions(+), 322 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 77ea5c272..710524af2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -408,7 +408,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2: - return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; + return currentPidProfile->D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; case FLIGHT_LOG_FIELD_CONDITION_MAG: #ifdef MAG @@ -1246,52 +1246,52 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", currentControlRateProfile->rates[ROLL], currentControlRateProfile->rates[PITCH], currentControlRateProfile->rates[YAW]); - BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", currentProfile->pidProfile.P8[ROLL], - currentProfile->pidProfile.I8[ROLL], - currentProfile->pidProfile.D8[ROLL]); - BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", currentProfile->pidProfile.P8[PITCH], - currentProfile->pidProfile.I8[PITCH], - currentProfile->pidProfile.D8[PITCH]); - BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", currentProfile->pidProfile.P8[YAW], - currentProfile->pidProfile.I8[YAW], - currentProfile->pidProfile.D8[YAW]); - BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDALT], - currentProfile->pidProfile.I8[PIDALT], - currentProfile->pidProfile.D8[PIDALT]); - BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPOS], - currentProfile->pidProfile.I8[PIDPOS], - currentProfile->pidProfile.D8[PIDPOS]); - BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPOSR], - currentProfile->pidProfile.I8[PIDPOSR], - currentProfile->pidProfile.D8[PIDPOSR]); - BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDNAVR], - currentProfile->pidProfile.I8[PIDNAVR], - currentProfile->pidProfile.D8[PIDNAVR]); - BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDLEVEL], - currentProfile->pidProfile.I8[PIDLEVEL], - currentProfile->pidProfile.D8[PIDLEVEL]); - BLACKBOX_PRINT_HEADER_LINE("magPID:%d", currentProfile->pidProfile.P8[PIDMAG]); - BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDVEL], - currentProfile->pidProfile.I8[PIDVEL], - currentProfile->pidProfile.D8[PIDVEL]); - BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", currentProfile->pidProfile.dterm_filter_type); - BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", currentProfile->pidProfile.dterm_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz); - BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentProfile->pidProfile.itermWindupPointPercent); - BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit); - BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation); - BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentProfile->pidProfile.pidAtMinThrottle); + BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", currentPidProfile->P8[ROLL], + currentPidProfile->I8[ROLL], + currentPidProfile->D8[ROLL]); + BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", currentPidProfile->P8[PITCH], + currentPidProfile->I8[PITCH], + currentPidProfile->D8[PITCH]); + BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", currentPidProfile->P8[YAW], + currentPidProfile->I8[YAW], + currentPidProfile->D8[YAW]); + BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", currentPidProfile->P8[PIDALT], + currentPidProfile->I8[PIDALT], + currentPidProfile->D8[PIDALT]); + BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", currentPidProfile->P8[PIDPOS], + currentPidProfile->I8[PIDPOS], + currentPidProfile->D8[PIDPOS]); + BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", currentPidProfile->P8[PIDPOSR], + currentPidProfile->I8[PIDPOSR], + currentPidProfile->D8[PIDPOSR]); + BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", currentPidProfile->P8[PIDNAVR], + currentPidProfile->I8[PIDNAVR], + currentPidProfile->D8[PIDNAVR]); + BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", currentPidProfile->P8[PIDLEVEL], + currentPidProfile->I8[PIDLEVEL], + currentPidProfile->D8[PIDLEVEL]); + BLACKBOX_PRINT_HEADER_LINE("magPID:%d", currentPidProfile->P8[PIDMAG]); + BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", currentPidProfile->P8[PIDVEL], + currentPidProfile->I8[PIDVEL], + currentPidProfile->D8[PIDVEL]); + BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", currentPidProfile->dterm_filter_type); + BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", currentPidProfile->dterm_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentPidProfile->yaw_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentPidProfile->dterm_notch_hz); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentPidProfile->dterm_notch_cutoff); + BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentPidProfile->itermWindupPointPercent); + BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentPidProfile->yaw_p_limit); + BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentPidProfile->dterm_average_count); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentPidProfile->vbatPidCompensation); + BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentPidProfile->pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold:%d", currentProfile->pidProfile.itermThrottleThreshold); - BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain:%d", castFloatBytesToInt(currentProfile->pidProfile.itermAcceleratorGain)); - BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentProfile->pidProfile.setpointRelaxRatio); - BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentProfile->pidProfile.dtermSetpointWeight); - BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.yawRateAccelLimit)); - BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.rateAccelLimit)); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold:%d", currentPidProfile->itermThrottleThreshold); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain:%d", castFloatBytesToInt(currentPidProfile->itermAcceleratorGain)); + BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentPidProfile->setpointRelaxRatio); + BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentPidProfile->dtermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", castFloatBytesToInt(currentPidProfile->yawRateAccelLimit)); + BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", castFloatBytesToInt(currentPidProfile->rateAccelLimit)); // End of Betaflight controller parameters BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 66d97cd23..ccbf646a4 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -52,9 +52,9 @@ // // PID // -static uint8_t tmpProfileIndex; -static uint8_t profileIndex; -static char profileIndexString[] = " p"; +static uint8_t tmpPidProfileIndex; +static uint8_t pidProfileIndex; +static char pidProfileIndexString[] = " p"; static uint8_t tempPid[3][3]; static uint8_t tmpRateProfileIndex; @@ -64,10 +64,10 @@ static controlRateConfig_t rateProfile; static long cmsx_menuImu_onEnter(void) { - profileIndex = getCurrentProfileIndex(); - tmpProfileIndex = profileIndex + 1; + pidProfileIndex = getCurrentPidProfileIndex(); + tmpPidProfileIndex = pidProfileIndex + 1; - rateProfileIndex = systemConfig()->activeRateProfile; + rateProfileIndex = getCurrentControlRateProfileIndex(); tmpRateProfileIndex = rateProfileIndex + 1; return 0; @@ -77,7 +77,7 @@ static long cmsx_menuImu_onExit(const OSD_Entry *self) { UNUSED(self); - changeProfile(profileIndex); + changePidProfile(pidProfileIndex); changeControlRateProfile(rateProfileIndex); return 0; @@ -88,7 +88,7 @@ static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *pt UNUSED(displayPort); UNUSED(ptr); - profileIndex = tmpProfileIndex - 1; + pidProfileIndex = tmpPidProfileIndex - 1; return 0; } @@ -106,7 +106,7 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void static long cmsx_PidRead(void) { - const pidProfile_t *pidProfile = pidProfiles(profileIndex); + const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); for (uint8_t i = 0; i < 3; i++) { tempPid[i][0] = pidProfile->P8[i]; tempPid[i][1] = pidProfile->I8[i]; @@ -118,7 +118,7 @@ static long cmsx_PidRead(void) static long cmsx_PidOnEnter(void) { - profileIndexString[1] = '0' + tmpProfileIndex; + pidProfileIndexString[1] = '0' + tmpPidProfileIndex; cmsx_PidRead(); return 0; @@ -128,20 +128,20 @@ static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); - pidProfile_t *pidProfile = pidProfilesMutable(profileIndex); + pidProfile_t *pidProfile = currentPidProfile; for (uint8_t i = 0; i < 3; i++) { pidProfile->P8[i] = tempPid[i][0]; pidProfile->I8[i] = tempPid[i][1]; pidProfile->D8[i] = tempPid[i][2]; } - pidInitConfig(¤tProfile->pidProfile); + pidInitConfig(currentPidProfile); return 0; } static OSD_Entry cmsx_menuPidEntries[] = { - { "-- PID --", OME_Label, NULL, profileIndexString, 0}, + { "-- PID --", OME_Label, NULL, pidProfileIndexString, 0}, { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 0, 200, 1 }, 0 }, { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 0, 200, 1 }, 0 }, @@ -190,7 +190,7 @@ static long cmsx_RateProfileWriteback(const OSD_Entry *self) static long cmsx_RateProfileOnEnter(void) { - rateProfileIndexString[1] = '0' + tmpProfileIndex; + rateProfileIndexString[1] = '0' + tmpPidProfileIndex; rateProfileIndexString[3] = '0' + tmpRateProfileIndex; cmsx_RateProfileRead(); @@ -235,9 +235,9 @@ static uint8_t cmsx_horizonTransition; static long cmsx_profileOtherOnEnter(void) { - profileIndexString[1] = '0' + tmpProfileIndex; + pidProfileIndexString[1] = '0' + tmpPidProfileIndex; - const pidProfile_t *pidProfile = pidProfiles(profileIndex); + const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight; cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio; @@ -252,10 +252,10 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) { UNUSED(self); - pidProfile_t *pidProfile = pidProfilesMutable(profileIndex); + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio; - pidInitConfig(¤tProfile->pidProfile); + pidInitConfig(currentPidProfile); pidProfile->P8[PIDLEVEL] = cmsx_angleStrength; pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength; @@ -265,7 +265,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) } static OSD_Entry cmsx_menuProfileOtherEntries[] = { - { "-- OTHER PP --", OME_Label, NULL, profileIndexString, 0 }, + { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, @@ -347,7 +347,7 @@ static uint16_t cmsx_yaw_p_limit; static long cmsx_FilterPerProfileRead(void) { - const pidProfile_t *pidProfile = pidProfiles(profileIndex); + const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz; cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; @@ -361,7 +361,7 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) { UNUSED(self); - pidProfile_t *pidProfile = pidProfilesMutable(profileIndex); + pidProfile_t *pidProfile = currentPidProfile; pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz; pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; @@ -398,7 +398,7 @@ static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpPidProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, {"MISC PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, {"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, diff --git a/src/main/common/utils.h b/src/main/common/utils.h index cb35a757b..8a2040b55 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -23,6 +23,8 @@ #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0])) #define ARRAYEND(x) (&(x)[ARRAYLEN(x)]) +#define CONST_CAST(type, value) ((type)(value)) + #define CONCAT_HELPER(x,y) x ## y #define CONCAT(x,y) CONCAT_HELPER(x, y) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 4a88a985e..aae9917e7 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -130,9 +130,11 @@ bool isEEPROMContentValid(void) chk = updateChecksum(chk, header, sizeof(*header)); p += sizeof(*header); +#ifndef USE_PARAMETER_GROUPS // include the transitional masterConfig record chk = updateChecksum(chk, p, sizeof(masterConfig)); p += sizeof(masterConfig); +#endif for (;;) { const configRecord_t *record = (const configRecord_t *)p; @@ -174,7 +176,9 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla { const uint8_t *p = &__config_start; p += sizeof(configHeader_t); // skip header +#ifndef USE_PARAMETER_GROUPS p += sizeof(master_t); // skip the transitional master_t record +#endif while (true) { const configRecord_t *record = (const configRecord_t *)p; if (record->size == 0 @@ -195,10 +199,12 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla // but each PG is loaded/initialized exactly once and in defined order. bool loadEEPROM(void) { +#ifndef USE_PARAMETER_GROUPS // read in the transitional masterConfig record const uint8_t *p = &__config_start; p += sizeof(configHeader_t); // skip header masterConfig = *(master_t*)p; +#endif PG_FOREACH(reg) { configRecordFlags_e cls_start, cls_end; @@ -238,9 +244,11 @@ static bool writeSettingsToEEPROM(void) config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)); chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header)); +#ifndef USE_PARAMETER_GROUPS // write the transitional masterConfig record config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig)); chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig)); +#endif PG_FOREACH(reg) { const uint16_t regSize = pgSize(reg); configRecord_t record = { diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 93aa7c57a..998770a89 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -81,7 +81,6 @@ #define servoConfig(x) (&masterConfig.servoConfig) #define servoMixerConfig(x) (&masterConfig.servoMixerConfig) #define gimbalConfig(x) (&masterConfig.gimbalConfig) -#define channelForwardingConfig(x) (&masterConfig.channelForwardingConfig) #define boardAlignment(x) (&masterConfig.boardAlignment) #define imuConfig(x) (&masterConfig.imuConfig) #define gyroConfig(x) (&masterConfig.gyroConfig) @@ -191,7 +190,6 @@ #define modeActivationConditionsMutable(i) (&masterConfig.modeActivationProfile.modeActivationConditions[i]) #define controlRateProfilesMutable(i) (&masterConfig.controlRateProfile[i]) #define pidProfilesMutable(i) (&masterConfig.profile[i].pidProfile) -#endif // System-wide typedef struct master_s { @@ -215,8 +213,6 @@ typedef struct master_s { servoProfile_t servoProfile; // gimbal-related configuration gimbalConfig_t gimbalConfig; - // Channel forwarding start channel - channelForwardingConfig_t channelForwardingConfig; #endif boardAlignment_t boardAlignment; @@ -334,3 +330,4 @@ typedef struct master_s { extern master_t masterConfig; void createDefaultConfig(master_t *config); +#endif // USE_PARAMETER_GROUPS diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c75886fb7..60a81aedc 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -28,6 +28,8 @@ // FIXME remove this for targets that don't need a CLI. Perhaps use a no-op macro when USE_CLI is not enabled // signal that we're in cli mode uint8_t cliMode = 0; +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; #ifdef USE_CLI @@ -44,6 +46,7 @@ uint8_t cliMode = 0; #include "common/maths.h" #include "common/printf.h" #include "common/typeconversion.h" +#include "common/utils.h" #include "config/config_master.h" #include "config/config_eeprom.h" @@ -954,7 +957,7 @@ static const clivalue_t valueTable[] = { { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servo_lowpass_freq, .config.minmax = { 0, 400} }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->dev.servoPwmRate, .config.minmax = { 50, 498 } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } }, - { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &channelForwardingConfig()->startChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, + { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &servoConfig()->channelForwardingStartChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, #endif { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, @@ -1533,8 +1536,9 @@ static uint16_t getValueOffset(const clivalue_t *value) { switch (value->type & VALUE_SECTION_MASK) { case MASTER_VALUE: - case PROFILE_VALUE: return value->offset; + case PROFILE_VALUE: + return value->offset + sizeof(pidProfile_t) * getCurrentPidProfileIndex(); case PROFILE_RATE_VALUE: return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); } @@ -1544,15 +1548,7 @@ static uint16_t getValueOffset(const clivalue_t *value) static void *getValuePointer(const clivalue_t *value) { const pgRegistry_t* rec = pgFind(value->pgn); - - switch (value->type & VALUE_SECTION_MASK) { - case MASTER_VALUE: - case PROFILE_VALUE: - return rec->address + value->offset; - case PROFILE_RATE_VALUE: - return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); - } - return NULL; + return CONST_CAST(void *, rec + getValueOffset(value)); } static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) @@ -1595,19 +1591,16 @@ static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) void *getValuePointer(const clivalue_t *value) { - void *ptr = value->ptr; + uint8_t *ptr = value->ptr; if ((value->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * systemConfig()->current_profile_index); - } - - if ((value->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * systemConfig()->current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex()); + ptr += sizeof(pidProfile_t) * getCurrentPidProfileIndex(); + } else if ((value->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { + ptr += sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); } return ptr; } -#endif // USE_PARAMETER_GROUPS static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig) { @@ -1650,13 +1643,6 @@ static bool valueEqualsDefault(const clivalue_t *value, const master_t *defaultC return result; } -static void cliPrintVar(const clivalue_t *var, uint32_t full) -{ - const void *ptr = getValuePointer(var); - - printValuePointer(var, ptr, full); -} - static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const master_t *defaultConfig) { void *ptr = getValuePointer(var); @@ -1665,7 +1651,16 @@ static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const maste printValuePointer(var, defaultPtr, full); } +#endif // USE_PARAMETER_GROUPS +static void cliPrintVar(const clivalue_t *var, uint32_t full) +{ + const void *ptr = getValuePointer(var); + + printValuePointer(var, ptr, full); +} + +#ifndef USE_PARAMETER_GROUPS static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig) { const clivalue_t *value; @@ -1687,6 +1682,7 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t * } } } +#endif static void cliPrintVarRange(const clivalue_t *var) { @@ -1714,6 +1710,7 @@ typedef union { float float_value; } int_float_value_t; + static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { void *ptr = getValuePointer(var); @@ -2635,6 +2632,30 @@ static void printServo(uint8_t dumpMask, const servoParam_t *servoParams, const servoConf->forwardFromChannel ); } + // print servo directions + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + const char *format = "smix reverse %d %d r\r\n"; + const servoParam_t *servoConf = &servoParams[i]; + const servoParam_t *servoConfDefault = &defaultServoParams[i]; + if (defaultServoParams) { + bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; + for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); + if (servoConfDefault->reversedSources & (1 << channel)) { + cliDefaultPrintf(dumpMask, equalsDefault, format, i , channel); + } + if (servoConf->reversedSources & (1 << channel)) { + cliDumpPrintf(dumpMask, equalsDefault, format, i , channel); + } + } + } else { + for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + if (servoConf->reversedSources & (1 << channel)) { + cliDumpPrintf(dumpMask, true, format, i , channel); + } + } + } + } } static void cliServo(char *cmdline) @@ -2715,18 +2736,18 @@ static void cliServo(char *cmdline) #endif #ifdef USE_SERVOS -static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) +static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixers, const servoMixer_t *defaultCustomServoMixers) { const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - const servoMixer_t customServoMixer = *customServoMixers(i); + const servoMixer_t customServoMixer = customServoMixers[i]; if (customServoMixer.rate == 0) { break; } bool equalsDefault = false; - if (defaultConfig) { - servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; + if (defaultCustomServoMixers) { + servoMixer_t customServoMixerDefault = defaultCustomServoMixers[i]; equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel && customServoMixer.inputSource == customServoMixerDefault.inputSource && customServoMixer.rate == customServoMixerDefault.rate @@ -2759,31 +2780,6 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) } cliPrint("\r\n"); - - // print servo directions - for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - const char *format = "smix reverse %d %d r\r\n"; - const servoParam_t *servoConf = servoParams(i); - if (defaultConfig) { - const servoParam_t *servoConfDefault = &defaultConfig->servoProfile.servoConf[i]; - bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; - for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); - if (servoConfDefault->reversedSources & (1 << channel)) { - cliDefaultPrintf(dumpMask, equalsDefault, format, i , channel); - } - if (servoConf->reversedSources & (1 << channel)) { - cliDumpPrintf(dumpMask, equalsDefault, format, i , channel); - } - } - } else { - for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoConf->reversedSources & (1 << channel)) { - cliDumpPrintf(dumpMask, true, format, i , channel); - } - } - } - } } static void cliServoMix(char *cmdline) @@ -2792,10 +2788,14 @@ static void cliServoMix(char *cmdline) int len = strlen(cmdline); if (len == 0) { - printServoMix(DUMP_MASTER, NULL); + printServoMix(DUMP_MASTER, customServoMixers(0), NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer +#ifdef USE_PARAMETER_GROUPS + memset(customServoMixers_array(), 0, sizeof(*customServoMixers_array())); +#else memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer)); +#endif for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servoParamsMutable(i)->reversedSources = 0; } @@ -2809,7 +2809,7 @@ static void cliServoMix(char *cmdline) break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { - servoMixerLoadMix(i, masterConfig.customServoMixer); + servoMixerLoadMix(i); cliPrintf("Loaded %s\r\n", mixerNames[i]); cliServoMix(""); break; @@ -3067,15 +3067,15 @@ static void cliFlashRead(char *cmdline) #endif #ifdef VTX -static void printVtx(uint8_t dumpMask, const master_t *defaultConfig) +static void printVtx(uint8_t dumpMask, const vtxConfig_t *vtxConfig, const vtxConfig_t *vtxConfigDefault) { // print out vtx channel settings const char *format = "vtx %u %u %u %u %u %u\r\n"; bool equalsDefault = false; for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { - const vtxChannelActivationCondition_t *cac = &vtxConfig()->vtxChannelActivationConditions[i]; - if (defaultConfig) { - const vtxChannelActivationCondition_t *cacDefault = &defaultConfig->vtxConfig.vtxChannelActivationConditions[i]; + const vtxChannelActivationCondition_t *cac = &vtxConfig->vtxChannelActivationConditions[i]; + if (vtxConfigDefault) { + const vtxChannelActivationCondition_t *cacDefault = &vtxConfigDefault->vtxChannelActivationConditions[i]; equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex && cac->band == cacDefault->band && cac->channel == cacDefault->channel @@ -3107,7 +3107,7 @@ static void cliVtx(char *cmdline) const char *ptr; if (isEmpty(cmdline)) { - printVtx(DUMP_MASTER, NULL); + printVtx(DUMP_MASTER, vtxConfig(), NULL); } else { ptr = cmdline; i = atoi(ptr++); @@ -3168,20 +3168,16 @@ static void cliName(char *cmdline) printName(DUMP_MASTER); } -static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfigDefault) +static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { - const uint32_t mask = featureMask(); + const uint32_t mask = featureConfig->enabledFeatures; const uint32_t defaultMask = featureConfigDefault->enabledFeatures; - for (uint32_t i = 0; ; i++) { // disable all feature first - if (featureNames[i] == NULL) - break; + for (uint32_t i = 0; featureNames[i]; i++) { // disable all feature first const char *format = "feature -%s\r\n"; cliDefaultPrintf(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); cliDumpPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); } - for (uint32_t i = 0; ; i++) { // reenable what we want. - if (featureNames[i] == NULL) - break; + for (uint32_t i = 0; featureNames[i]; i++) { // reenable what we want. const char *format = "feature %s\r\n"; if (defaultMask & (1 << i)) { cliDefaultPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); @@ -3260,11 +3256,11 @@ static void cliFeature(char *cmdline) } #ifdef BEEPER -static void printBeeper(uint8_t dumpMask, const master_t *defaultConfig) +static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) { const uint8_t beeperCount = beeperTableEntryCount(); - const uint32_t mask = getBeeperOffMask(); - const uint32_t defaultMask = defaultConfig->beeperConfig.beeper_off_flags; + const uint32_t mask = beeperConfig->beeper_off_flags; + const uint32_t defaultMask = beeperConfigDefault->beeper_off_flags; for (int32_t i = 0; i < beeperCount - 2; i++) { const char *formatOff = "beeper -%s\r\n"; const char *formatOn = "beeper %s\r\n"; @@ -3701,12 +3697,12 @@ static void cliPlaySound(char *cmdline) static void cliProfile(char *cmdline) { if (isEmpty(cmdline)) { - cliPrintf("profile %d\r\n", getCurrentProfileIndex()); + cliPrintf("profile %d\r\n", getCurrentPidProfileIndex()); return; } else { const int i = atoi(cmdline); if (i >= 0 && i < MAX_PROFILE_COUNT) { - systemConfigMutable()->current_profile_index = i; + systemConfigMutable()->pidProfileIndex = i; writeEEPROM(); readEEPROM(); cliProfile(""); @@ -3728,22 +3724,18 @@ static void cliRateProfile(char *cmdline) } } -static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_t *defaultConfig) +#ifndef USE_PARAMETER_GROUPS +static void cliDumpPidProfile(uint8_t pidProfileIndex, uint8_t dumpMask, const master_t *defaultConfig) { - if (profileIndex >= MAX_PROFILE_COUNT) { + if (pidProfileIndex >= MAX_PROFILE_COUNT) { // Faulty values return; } - changeProfile(profileIndex); + changePidProfile(pidProfileIndex); cliPrintHashLine("profile"); cliProfile(""); cliPrint("\r\n"); -#ifdef USE_PARAMETER_GROUPS - (void)(defaultConfig); - dumpAllValues(PROFILE_VALUE, dumpMask); -#else dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); -#endif } static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, const master_t *defaultConfig) @@ -3758,6 +3750,7 @@ static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, const cliPrint("\r\n"); dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } +#endif static void cliSave(char *cmdline) { @@ -3938,7 +3931,11 @@ static void cliStatus(char *cmdline) #endif cliPrintf("Stack size: %d, Stack address: 0x%x\r\n", stackTotalSize(), stackHighMem()); +#ifdef USE_PARAMETER_GROUPS + cliPrintf("I2C Errors: %d, config size: %d, max available config: %d\r\n", i2cErrorCounter, getEEPROMConfigSize(), &__config_end - &__config_start); +#else cliPrintf("I2C Errors: %d, config size: %d\r\n", i2cErrorCounter, sizeof(master_t)); +#endif const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID))); const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX))); @@ -4059,6 +4056,7 @@ const cliResourceValue_t resourceTable[] = { #endif }; +#ifndef USE_PARAMETER_GROUPS static void printResource(uint8_t dumpMask, const master_t *defaultConfig) { for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { @@ -4087,6 +4085,7 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig) } } } +#endif static void printResourceOwner(uint8_t owner, uint8_t index) { @@ -4137,7 +4136,9 @@ static void cliResource(char *cmdline) int len = strlen(cmdline); if (len == 0) { +#ifndef USE_PARAMETER_GROUPS printResource(DUMP_MASTER | HIDE_UNUSED, NULL); +#endif return; } else if (strncasecmp(cmdline, "list", len) == 0) { @@ -4324,16 +4325,18 @@ static void printConfig(char *cmdline, bool doDiff) dumpMask = dumpMask | DO_DIFF; } - static master_t defaultConfig; - createDefaultConfig(&defaultConfig); - #ifdef USE_PARAMETER_GROUPS backupConfigs(); // reset all configs to defaults to do differencing resetConfigs(); #if defined(TARGET_CONFIG) - targetConfiguration(&defaultConfig); + targetConfiguration(); #endif +#else + static master_t defaultConfig; + createDefaultConfig(&defaultConfig); +#if defined(TARGET_CONFIG) + targetConfiguration(&defaultConfig); #endif if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values @@ -4374,16 +4377,16 @@ static void printConfig(char *cmdline, bool doDiff) cliPrintHashLine("servo mix"); // print custom servo mixer if exists cliDumpPrintf(dumpMask, masterConfig.customServoMixer[0].rate == 0, "smix reset\r\n\r\n"); - printServoMix(dumpMask, &defaultConfig); + printServoMix(dumpMask, customServoMixers(0), defaultConfig.customServoMixer); #endif #endif cliPrintHashLine("feature"); - printFeature(dumpMask, &defaultConfig.featureConfig); + printFeature(dumpMask, featureConfig(), &defaultConfig.featureConfig); #ifdef BEEPER cliPrintHashLine("beeper"); - printBeeper(dumpMask, &defaultConfig); + printBeeper(dumpMask, beeperConfig(), &defaultConfig.beeperConfig); #endif cliPrintHashLine("map"); @@ -4414,7 +4417,7 @@ static void printConfig(char *cmdline, bool doDiff) #ifdef VTX cliPrintHashLine("vtx"); - printVtx(dumpMask, &defaultConfig); + printVtx(dumpMask, vtxConfig(), &defaultConfig.vtxConfig); #endif cliPrintHashLine("rxfail"); @@ -4424,11 +4427,11 @@ static void printConfig(char *cmdline, bool doDiff) dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { - const uint8_t profileIndexSave = getCurrentProfileIndex(); - for (uint32_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) { - cliDumpProfile(profileIndex, dumpMask, &defaultConfig); + const uint8_t pidProfileIndexSave = getCurrentPidProfileIndex(); + for (uint32_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + cliDumpPidProfile(pidProfileIndex, dumpMask, &defaultConfig); } - changeProfile(profileIndexSave); + changePidProfile(pidProfileIndexSave); cliPrintHashLine("restore original profile selection"); cliProfile(""); @@ -4443,19 +4446,20 @@ static void printConfig(char *cmdline, bool doDiff) cliPrintHashLine("save configuration"); cliPrint("save"); } else { - cliDumpProfile(getCurrentProfileIndex(), dumpMask, &defaultConfig); + cliDumpPidProfile(getCurrentPidProfileIndex(), dumpMask, &defaultConfig); cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask, &defaultConfig); } } if (dumpMask & DUMP_PROFILE) { - cliDumpProfile(getCurrentProfileIndex(), dumpMask, &defaultConfig); + cliDumpPidProfile(getCurrentPidProfileIndex(), dumpMask, &defaultConfig); } if (dumpMask & DUMP_RATES) { cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask, &defaultConfig); } +#endif #ifdef USE_PARAMETER_GROUPS // restore configs from copies restoreConfigs(); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 29951eef1..825715c94 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -94,8 +94,10 @@ #include "telemetry/telemetry.h" +#ifndef USE_PARAMETER_GROUPS master_t masterConfig; // master config struct with data independent from profiles -profile_t *currentProfile; +#endif +pidProfile_t *currentPidProfile; #ifndef DEFAULT_FEATURES #define DEFAULT_FEATURES 0 @@ -116,8 +118,8 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, - .current_profile_index = 0, - //!!TODO.activeRateProfile = 0, + .pidProfileIndex = 0, + .activeRateProfile = 0, .debug_mode = DEBUG_MODE, .task_statistics = true, .name = { 0 } @@ -197,7 +199,7 @@ static void resetControlRateProfile(controlRateConfig_t *controlRateConfig) #endif #ifndef USE_PARAMETER_GROUPS -static void resetPidProfile(pidProfile_t *pidProfile) +void resetPidProfile(pidProfile_t *pidProfile) { pidProfile->P8[ROLL] = 44; pidProfile->I8[ROLL] = 40; @@ -807,16 +809,17 @@ void resetFlashConfig(flashConfig_t *flashConfig) #endif #endif -uint8_t getCurrentProfileIndex(void) +uint8_t getCurrentPidProfileIndex(void) { - return systemConfig()->current_profile_index; + return systemConfig()->pidProfileIndex; } -static void setProfile(uint8_t profileIndex) +static void setPidProfile(uint8_t pidProfileIndex) { - if (profileIndex < MAX_PROFILE_COUNT) { - systemConfigMutable()->current_profile_index = profileIndex; - currentProfile = &masterConfig.profile[profileIndex]; + if (pidProfileIndex < MAX_PROFILE_COUNT) { + systemConfigMutable()->pidProfileIndex = pidProfileIndex; + currentPidProfile = pidProfilesMutable(pidProfileIndex); + pidInit(); // re-initialise pid controller to re-initialise filters and config } } @@ -830,7 +833,7 @@ uint16_t getCurrentMinthrottle(void) return motorConfig()->minthrottle; } - +#ifndef USE_PARAMETER_GROUPS void createDefaultConfig(master_t *config) { // Clear all configuration @@ -872,7 +875,7 @@ void createDefaultConfig(master_t *config) // global settings #ifndef USE_PARAMETER_GROUPS - config->systemConfig.current_profile_index = 0; // default profile + config->systemConfig.pidProfileIndex = 0; // default profile config->systemConfig.debug_mode = DEBUG_MODE; config->systemConfig.task_statistics = true; #endif @@ -1089,7 +1092,7 @@ void createDefaultConfig(master_t *config) #endif // Channel forwarding; - config->channelForwardingConfig.startChannel = AUX1; + config->servoConfig.channelForwardingStartChannel = AUX1; #endif #ifndef USE_PARAMETER_GROUPS @@ -1154,14 +1157,17 @@ void createDefaultConfig(master_t *config) targetConfiguration(config); #endif } +#endif void resetConfigs(void) { +#ifndef USE_PARAMETER_GROUPS createDefaultConfig(&masterConfig); +#endif pgResetAll(MAX_PROFILE_COUNT); pgActivateProfile(0); - setProfile(0); + setPidProfile(0); setControlRateProfile(0); #ifdef LED_STRIP @@ -1175,24 +1181,20 @@ void activateConfig(void) resetAdjustmentStates(); - useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile); - useAdjustmentConfig(¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useAdjustmentConfig(currentPidProfile); #ifdef GPS - gpsUsePIDs(¤tProfile->pidProfile); + gpsUsePIDs(currentPidProfile); #endif failsafeReset(); setAccelerationTrims(&accelerometerConfigMutable()->accZero); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); -#ifdef USE_SERVOS - servoUseConfigs(&masterConfig.channelForwardingConfig); -#endif - imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); - configureAltitudeHold(¤tProfile->pidProfile); + configureAltitudeHold(currentPidProfile); } void validateAndFixConfig(void) @@ -1262,15 +1264,19 @@ void validateAndFixConfig(void) #endif // Prevent invalid notch cutoff - if (currentProfile->pidProfile.dterm_notch_cutoff >= currentProfile->pidProfile.dterm_notch_hz) { - currentProfile->pidProfile.dterm_notch_hz = 0; + if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) { + currentPidProfile->dterm_notch_hz = 0; } validateAndFixGyroConfig(); #if defined(TARGET_VALIDATECONFIG) +#ifdef USE_PARAMETER_GROUPS + targetValidateConfiguration(); +#else targetValidateConfiguration(&masterConfig); #endif +#endif } void validateAndFixGyroConfig(void) @@ -1358,14 +1364,14 @@ void readEEPROM(void) failureMode(FAILURE_INVALID_EEPROM_CONTENTS); } -// pgActivateProfile(getCurrentProfileIndex()); +// pgActivateProfile(getCurrentPidProfileIndex()); // setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); - if (systemConfig()->current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check - systemConfigMutable()->current_profile_index = 0; + if (systemConfig()->pidProfileIndex > MAX_PROFILE_COUNT - 1) {// sanity check + systemConfigMutable()->pidProfileIndex = 0; } - setProfile(systemConfig()->current_profile_index); + setPidProfile(systemConfig()->pidProfileIndex); validateAndFixConfig(); activateConfig(); @@ -1403,15 +1409,16 @@ void saveConfigAndNotify(void) beeperConfirmationBeeps(1); } -void changeProfile(uint8_t profileIndex) +void changePidProfile(uint8_t pidProfileIndex) { - if (profileIndex >= MAX_PROFILE_COUNT) { - profileIndex = MAX_PROFILE_COUNT - 1; + if (pidProfileIndex >= MAX_PROFILE_COUNT) { + pidProfileIndex = MAX_PROFILE_COUNT - 1; } - systemConfigMutable()->current_profile_index = profileIndex; + systemConfigMutable()->pidProfileIndex = pidProfileIndex; + currentPidProfile = pidProfilesMutable(pidProfileIndex); writeEEPROM(); readEEPROM(); - beeperConfirmationBeeps(profileIndex + 1); + beeperConfirmationBeeps(pidProfileIndex + 1); } void beeperOffSet(uint32_t mask) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 602536bf6..0f120fee1 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -64,7 +64,7 @@ typedef enum { } features_e; typedef struct systemConfig_s { - uint8_t current_profile_index; + uint8_t pidProfileIndex; uint8_t activeRateProfile; uint8_t debug_mode; uint8_t task_statistics; @@ -81,8 +81,8 @@ PG_DECLARE(vcdProfile_t, vcdProfile); PG_DECLARE(sdcardConfig_t, sdcardConfig); PG_DECLARE(serialPinConfig_t, serialPinConfig); -struct profile_s; -extern struct profile_s *currentProfile; +struct pidProfile_s; +extern struct pidProfile_s *currentPidProfile; void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); @@ -104,10 +104,10 @@ void validateAndFixConfig(void); void validateAndFixGyroConfig(void); void activateConfig(void); -uint8_t getCurrentProfileIndex(void); -void changeProfile(uint8_t profileIndex); -struct profile_s; -void resetProfile(struct profile_s *profile); +uint8_t getCurrentPidProfileIndex(void); +void changePidProfile(uint8_t pidProfileIndex); +struct pidProfile_s; +void resetPidProfile(struct pidProfile_s *profile); uint8_t getCurrentControlRateProfileIndex(void); void changeControlRateProfile(uint8_t profileIndex); @@ -117,6 +117,11 @@ bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); void resetConfigs(void); +#ifdef USE_PARAMETER_GROUPS +void targetConfiguration(void); +void targetValidateConfiguration(void); +#else struct master_s; void targetConfiguration(struct master_s *config); void targetValidateConfiguration(struct master_s *config); +#endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 70f80353a..3e8c09ec4 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -270,7 +270,7 @@ void updateMagHold(void) dif -= 360; dif *= -rcControlsConfig()->yaw_control_direction; if (STATE(SMALL_ANGLE)) - rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg + rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30; // 18 deg } else magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } @@ -311,7 +311,7 @@ void processRx(timeUs_t currentTimeUs) This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); - if (currentProfile->pidProfile.pidAtMinThrottle) + if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); @@ -471,7 +471,7 @@ static void subTaskPidController(void) uint32_t startTime; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // PID - note this is function pointer set by setPIDController() - pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims); + pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); } @@ -563,7 +563,7 @@ static void subTaskMotorUpdate(void) startTime = micros(); } - mixTable(¤tProfile->pidProfile); + mixTable(currentPidProfile); #ifdef USE_SERVOS // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 6748d79c7..ecd3f09f9 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -439,10 +439,8 @@ void init(void) LED0_OFF; LED1_OFF; - // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime() - pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime - pidInitFilters(¤tProfile->pidProfile); - pidInitConfig(¤tProfile->pidProfile); + // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit() + pidInit(); imuInit(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f3de87d9e..a88e3f27b 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -612,7 +612,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); sbufWriteU32(dst, packFlightModeFlags()); - sbufWriteU8(dst, getCurrentProfileIndex()); + sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); sbufWriteU8(dst, MAX_PROFILE_COUNT); sbufWriteU8(dst, getCurrentControlRateProfileIndex()); @@ -636,7 +636,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); sbufWriteU32(dst, packFlightModeFlags()); - sbufWriteU8(dst, getCurrentProfileIndex()); + sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); sbufWriteU16(dst, 0); // gyro cycle time break; @@ -761,9 +761,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_PID: for (int i = 0; i < PID_ITEM_COUNT; i++) { - sbufWriteU8(dst, currentProfile->pidProfile.P8[i]); - sbufWriteU8(dst, currentProfile->pidProfile.I8[i]); - sbufWriteU8(dst, currentProfile->pidProfile.D8[i]); + sbufWriteU8(dst, currentPidProfile->P8[i]); + sbufWriteU8(dst, currentPidProfile->I8[i]); + sbufWriteU8(dst, currentPidProfile->D8[i]); } break; @@ -1137,12 +1137,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_FILTER_CONFIG : sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); - sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz); - sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz); + sbufWriteU16(dst, currentPidProfile->dterm_lpf_hz); + sbufWriteU16(dst, currentPidProfile->yaw_lpf_hz); sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); - sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz); - sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff); + sbufWriteU16(dst, currentPidProfile->dterm_notch_hz); + sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff); sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); break; @@ -1150,18 +1150,18 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_PID_ADVANCED: sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); - sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit); + sbufWriteU16(dst, currentPidProfile->yaw_p_limit); sbufWriteU8(dst, 0); // reserved - sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation); - sbufWriteU8(dst, currentProfile->pidProfile.setpointRelaxRatio); - sbufWriteU8(dst, currentProfile->pidProfile.dtermSetpointWeight); + sbufWriteU8(dst, currentPidProfile->vbatPidCompensation); + sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio); + sbufWriteU8(dst, currentPidProfile->dtermSetpointWeight); sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved - sbufWriteU16(dst, (uint16_t)lrintf(currentProfile->pidProfile.rateAccelLimit * 10)); - sbufWriteU16(dst, (uint16_t)lrintf(currentProfile->pidProfile.yawRateAccelLimit * 10)); - sbufWriteU8(dst, currentProfile->pidProfile.levelAngleLimit); - sbufWriteU8(dst, currentProfile->pidProfile.levelSensitivity); + sbufWriteU16(dst, (uint16_t)lrintf(currentPidProfile->rateAccelLimit * 10)); + sbufWriteU16(dst, (uint16_t)lrintf(currentPidProfile->yawRateAccelLimit * 10)); + sbufWriteU8(dst, currentPidProfile->levelAngleLimit); + sbufWriteU8(dst, currentPidProfile->levelSensitivity); break; case MSP_SENSOR_CONFIG: @@ -1270,7 +1270,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (value >= MAX_PROFILE_COUNT) { value = 0; } - changeProfile(value); + changePidProfile(value); } } else { value = value & ~RATEPROFILE_MASK; @@ -1320,11 +1320,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_PID: for (int i = 0; i < PID_ITEM_COUNT; i++) { - currentProfile->pidProfile.P8[i] = sbufReadU8(src); - currentProfile->pidProfile.I8[i] = sbufReadU8(src); - currentProfile->pidProfile.D8[i] = sbufReadU8(src); + currentPidProfile->P8[i] = sbufReadU8(src); + currentPidProfile->I8[i] = sbufReadU8(src); + currentPidProfile->D8[i] = sbufReadU8(src); } - pidInitConfig(¤tProfile->pidProfile); + pidInitConfig(currentPidProfile); break; case MSP_SET_MODE_RANGE: @@ -1339,7 +1339,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), currentPidProfile); } else { return MSP_RESULT_ERROR; } @@ -1481,7 +1481,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_RESET_CURR_PID: - resetProfile(currentProfile); + resetPidProfile(currentPidProfile); break; case MSP_SET_SENSOR_ALIGNMENT: gyroConfigMutable()->gyro_align = sbufReadU8(src); @@ -1518,13 +1518,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_FILTER_CONFIG: gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); - currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); - currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); + currentPidProfile->dterm_lpf_hz = sbufReadU16(src); + currentPidProfile->yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src); gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); - currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src); - currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src); + currentPidProfile->dterm_notch_hz = sbufReadU16(src); + currentPidProfile->dterm_notch_cutoff = sbufReadU16(src); } if (dataSize > 13) { gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src); @@ -1534,27 +1534,27 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) validateAndFixGyroConfig(); gyroInitFilters(); // reinitialize the PID filters with the new values - pidInitFilters(¤tProfile->pidProfile); + pidInitFilters(currentPidProfile); break; case MSP_SET_PID_ADVANCED: sbufReadU16(src); sbufReadU16(src); - currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src); + currentPidProfile->yaw_p_limit = sbufReadU16(src); sbufReadU8(src); // reserved - currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src); - currentProfile->pidProfile.setpointRelaxRatio = sbufReadU8(src); - currentProfile->pidProfile.dtermSetpointWeight = sbufReadU8(src); + currentPidProfile->vbatPidCompensation = sbufReadU8(src); + currentPidProfile->setpointRelaxRatio = sbufReadU8(src); + currentPidProfile->dtermSetpointWeight = sbufReadU8(src); sbufReadU8(src); // reserved sbufReadU8(src); // reserved sbufReadU8(src); // reserved - currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src) / 10.0f; - currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src) / 10.0f; + currentPidProfile->rateAccelLimit = sbufReadU16(src) / 10.0f; + currentPidProfile->yawRateAccelLimit = sbufReadU16(src) / 10.0f; if (dataSize > 17) { - currentProfile->pidProfile.levelAngleLimit = sbufReadU8(src); - currentProfile->pidProfile.levelSensitivity = sbufReadU8(src); + currentPidProfile->levelAngleLimit = sbufReadU8(src); + currentPidProfile->levelSensitivity = sbufReadU8(src); } - pidInitConfig(¤tProfile->pidProfile); + pidInitConfig(currentPidProfile); break; case MSP_SET_SENSOR_CONFIG: diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index eda9e16ff..8b8003297 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -158,7 +158,7 @@ static void scaleRcCommandToFpvCamAngle(void) { static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; const int rxRefreshRateMs = rxRefreshRate / 1000; const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); - const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold; + const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; if (index >= indexMax) @@ -167,7 +167,7 @@ static void scaleRcCommandToFpvCamAngle(void) { const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; if(ABS(rcCommandSpeed) > throttleVelocityThreshold) - pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain); + pidSetItermAccelerator(currentPidProfile->itermAcceleratorGain); else pidSetItermAccelerator(1.0f); } @@ -185,7 +185,7 @@ void processRcCommand(void) if (isRXDataNew) { currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); - if (currentProfile->pidProfile.itermAcceleratorGain > 1.0f) + if (currentPidProfile->itermAcceleratorGain > 1.0f) checkForThrottleErrorResetState(currentRxRefreshRate); } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 58eecc443..4522cedec 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -223,7 +223,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { - changeProfile(i - 1); + changePidProfile(i - 1); return; } diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 03d42395d..5f98d4ddc 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -88,7 +88,7 @@ navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode // When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit() void navigationInit(void) { - gpsUsePIDs(¤tProfile->pidProfile); + gpsUsePIDs(currentPidProfile); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c656f6fb2..a534977e1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -76,6 +76,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 0); +#ifdef USE_PARAMETER_GROUPS void resetPidProfile(pidProfile_t *pidProfile) { RESET_CONFIG(const pidProfile_t, pidProfile, @@ -128,18 +129,13 @@ void resetPidProfile(pidProfile_t *pidProfile) .itermAcceleratorGain = 1.0f ); } + void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) { for (int i = 0; i < MAX_PROFILE_COUNT; i++) { resetPidProfile(&pidProfiles[i]); } } - -#ifdef USE_PARAMETER_GROUPS -void resetProfile(profile_t *profile) -{ - resetPidProfile(&profile->pidProfile); -} #endif void pidSetTargetLooptime(uint32_t pidLooptime) @@ -260,6 +256,13 @@ void pidInitConfig(const pidProfile_t *pidProfile) { ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); } +void pidInit(void) +{ + pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime + pidInitFilters(currentPidProfile); + pidInitConfig(currentPidProfile); +} + static float calcHorizonLevelStrength(void) { float horizonLevelStrength = 0.0f; if (horizonTransition > 0.0f) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 1f47a5f98..1c04c2a7b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -116,4 +116,5 @@ void pidSetTargetLooptime(uint32_t pidLooptime); void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); +void pidInit(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index aff5615da..44f1822d4 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -61,6 +61,7 @@ void pgResetFn_servoConfig(servoConfig_t *servoConfig) servoConfig->dev.servoPwmRate = 50; servoConfig->tri_unarmed_servo = 1; servoConfig->servo_lowpass_freq = 0; + servoConfig->channelForwardingStartChannel = AUX1; int servoIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) { @@ -97,7 +98,6 @@ static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; -static channelForwardingConfig_t *channelForwardingConfig; #define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) @@ -187,11 +187,6 @@ const mixerRules_t servoMixers[] = { { 0, NULL }, }; -void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse) -{ - channelForwardingConfig = channelForwardingConfigToUse; -} - int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) { const uint8_t channelToForwardFrom = servoParams(servoIndex)->forwardFromChannel; @@ -273,27 +268,25 @@ void servoConfigureOutput(void) } -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) +void servoMixerLoadMix(int index) { - int i; - // we're 1-based index++; // clear existing - for (i = 0; i < MAX_SERVO_RULES; i++) - customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0; + for (int i = 0; i < MAX_SERVO_RULES; i++) { + customServoMixersMutable(i)->targetChannel = customServoMixersMutable(i)->inputSource = customServoMixersMutable(i)->rate = customServoMixersMutable(i)->box = 0; + } - for (i = 0; i < servoMixers[index].servoRuleCount; i++) - customServoMixers[i] = servoMixers[index].rule[i]; + for (int i = 0; i < servoMixers[index].servoRuleCount; i++) { + *customServoMixersMutable(i) = servoMixers[index].rule[i]; + } } STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) { // start forwarding from this channel - uint8_t channelOffset = channelForwardingConfig->startChannel; - - uint8_t servoOffset; - for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { + uint8_t channelOffset = servoConfig()->channelForwardingStartChannel; + for (uint8_t servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); } } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index f47df8b47..0f213a049 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -108,6 +108,7 @@ typedef struct servoConfig_s { servoDevConfig_t dev; uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + uint8_t channelForwardingStartChannel; } servoConfig_t; PG_DECLARE(servoConfig_t, servoConfig); @@ -116,17 +117,12 @@ typedef struct servoProfile_s { servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; } servoProfile_t; -typedef struct channelForwardingConfig_s { - uint8_t startChannel; -} channelForwardingConfig_t; - extern int16_t servo[MAX_SUPPORTED_SERVOS]; bool isMixerUsingServos(void); void writeServos(void); -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); +void servoMixerLoadMix(int index); void loadCustomServoMixer(void); -void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse); int servoDirection(int servoIndex, int fromChannel); void servoConfigureOutput(void); void servosInit(void); diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 297c6a911..f07e3e8f1 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -317,12 +317,12 @@ void showProfilePage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - tfp_sprintf(lineBuffer, "Profile: %d", getCurrentProfileIndex()); + tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex()); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"}; - const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + const pidProfile_t *pidProfile = currentPidProfile; for (int axis = 0; axis < 3; ++axis) { tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d", axisTitles[axis], diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 38db0ba25..ed7353849 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -367,21 +367,21 @@ static void osdDrawSingleElement(uint8_t item) case OSD_ROLL_PIDS: { - const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + const pidProfile_t *pidProfile = currentPidProfile; sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]); break; } case OSD_PITCH_PIDS: { - const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + const pidProfile_t *pidProfile = currentPidProfile; sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]); break; } case OSD_YAW_PIDS: { - const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + const pidProfile_t *pidProfile = currentPidProfile; sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]); break; } @@ -394,9 +394,9 @@ static void osdDrawSingleElement(uint8_t item) case OSD_PIDRATE_PROFILE: { - const uint8_t profileIndex = getCurrentProfileIndex(); + const uint8_t pidProfileIndex = getCurrentPidProfileIndex(); const uint8_t rateProfileIndex = getCurrentControlRateProfileIndex(); - sprintf(buff, "%d-%d", profileIndex + 1, rateProfileIndex + 1); + sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1); break; } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index f2685796a..3de3eec9e 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -598,7 +598,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) junk |= 1 << i; } bstWrite32(junk); - bstWrite8(getCurrentProfileIndex()); + bstWrite8(getCurrentPidProfileIndex()); break; case BST_RAW_IMU: { @@ -703,11 +703,11 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { - bstWrite8(currentProfile->pidProfile.P8[i]); - bstWrite8(currentProfile->pidProfile.I8[i]); - bstWrite8(currentProfile->pidProfile.D8[i]); + bstWrite8(currentPidProfile->P8[i]); + bstWrite8(currentPidProfile->I8[i]); + bstWrite8(currentPidProfile->D8[i]); } - pidInitConfig(¤tProfile->pidProfile); + pidInitConfig(currentPidProfile); break; case BST_PIDNAMES: bstWriteNames(pidnames); @@ -1002,12 +1002,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) switch(bstWriteCommand) { case BST_SELECT_SETTING: if (!ARMING_FLAG(ARMED)) { - systemConfigMutable()->current_profile_index = bstRead8(); - if (systemConfig()->current_profile_index > 2) { - systemConfigMutable()->current_profile_index = 0; - } - writeEEPROM(); - readEEPROM(); + changePidProfile(bstRead8()); } break; case BST_SET_HEAD: @@ -1044,9 +1039,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) break; case BST_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { - currentProfile->pidProfile.P8[i] = bstRead8(); - currentProfile->pidProfile.I8[i] = bstRead8(); - currentProfile->pidProfile.D8[i] = bstRead8(); + currentPidProfile->P8[i] = bstRead8(); + currentPidProfile->I8[i] = bstRead8(); + currentPidProfile->D8[i] = bstRead8(); } break; case BST_SET_MODE_RANGE: @@ -1061,7 +1056,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, currentPidProfile); } else { ret = BST_FAILED; } diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 83049fab8..25e1b7b1e 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -39,6 +39,16 @@ #include "hardware_revision.h" +#ifdef USE_PARAMETER_GROUPS +void targetConfiguration(void) +{ + +} +void targetValidateConfiguration(void) +{ + +} +#else void targetConfiguration(master_t *config) { UNUSED(config); @@ -108,10 +118,9 @@ void targetConfiguration(master_t *config) void targetValidateConfiguration(master_t *config) { - UNUSED(config); - if (hardwareRevision < NAZE32_REV5 && config->accelerometerConfig.acc_hardware == ACC_ADXL345) { config->accelerometerConfig.acc_hardware = ACC_NONE; } } #endif +#endif // USE_PARAMETER_GROUPS diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index ac7cc70a3..476a90632 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -60,8 +60,6 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -extern profile_t *currentProfile; - enum { SPSTATE_UNINITIALIZED, @@ -755,19 +753,19 @@ void handleSmartPortTelemetry(void) } else if (telemetryConfig()->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: - tmp2 = currentProfile->pidProfile.P8[ROLL]; - tmp2 += (currentProfile->pidProfile.P8[PITCH]<<8); - tmp2 += (currentProfile->pidProfile.P8[YAW]<<16); + tmp2 = currentPidProfile->P8[ROLL]; + tmp2 += (currentPidProfile->P8[PITCH]<<8); + tmp2 += (currentPidProfile->P8[YAW]<<16); break; case 1: - tmp2 = currentProfile->pidProfile.I8[ROLL]; - tmp2 += (currentProfile->pidProfile.I8[PITCH]<<8); - tmp2 += (currentProfile->pidProfile.I8[YAW]<<16); + tmp2 = currentPidProfile->I8[ROLL]; + tmp2 += (currentPidProfile->I8[PITCH]<<8); + tmp2 += (currentPidProfile->I8[YAW]<<16); break; case 2: - tmp2 = currentProfile->pidProfile.D8[ROLL]; - tmp2 += (currentProfile->pidProfile.D8[PITCH]<<8); - tmp2 += (currentProfile->pidProfile.D8[YAW]<<16); + tmp2 = currentPidProfile->D8[ROLL]; + tmp2 += (currentPidProfile->D8[PITCH]<<8); + tmp2 += (currentPidProfile->D8[YAW]<<16); break; case 3: tmp2 = currentControlRateProfile->rates[FD_ROLL];