diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 63c36b4652..65d5759038 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1136,7 +1136,7 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); break; case 4: - blackboxPrintfHeaderLine("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile.rcRate8); + blackboxPrintfHeaderLine("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); break; case 5: blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); diff --git a/src/main/config/config.c b/src/main/config/config.c index b67138d279..0c939c0c10 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -350,7 +350,8 @@ uint8_t getCurrentProfile(void) static void setProfile(uint8_t profileIndex) { currentProfile = &masterConfig.profile[profileIndex]; - currentControlRateProfile = ¤tProfile->controlRateProfile; + currentControlRateProfileIndex = currentProfile->activeRateProfile; + currentControlRateProfile = ¤tProfile->controlRateProfile[currentControlRateProfileIndex]; } uint8_t getCurrentControlRateProfile(void) @@ -358,8 +359,15 @@ uint8_t getCurrentControlRateProfile(void) return currentControlRateProfileIndex; } +static void setControlRateProfile(uint8_t profileIndex) { + currentControlRateProfileIndex = profileIndex; + masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex; + currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex]; +} + + controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) { - return &masterConfig.profile[profileIndex].controlRateProfile; + return &masterConfig.profile[profileIndex].controlRateProfile[masterConfig.profile[profileIndex].activeRateProfile]; } uint16_t getCurrentMinthrottle(void) @@ -490,9 +498,11 @@ static void resetConf(void) masterConfig.emf_avoidance = 0; resetPidProfile(¤tProfile->pidProfile); - - resetControlRateConfig(&masterConfig.profile[0].controlRateProfile); - + + uint8_t rI; + for (rI = 0; rI MAX_RATEPROFILES) { + profileIndex = MAX_RATEPROFILES - 1; + } + setControlRateProfile(profileIndex); + activateControlRateConfig(); +} + void handleOneshotFeatureChangeOnRestart(void) { // Shutdown PWM on all motors prior to soft restart diff --git a/src/main/config/config.h b/src/main/config/config.h index ce3a6e1e67..6ef3a1cdad 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -18,6 +18,7 @@ #pragma once #define MAX_PROFILE_COUNT 2 +#define MAX_RATEPROFILES 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 typedef enum { @@ -67,7 +68,7 @@ uint8_t getCurrentProfile(void); void changeProfile(uint8_t profileIndex); uint8_t getCurrentControlRateProfile(void); - +void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 3251b97917..1ecbc29fcd 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -19,5 +19,6 @@ typedef struct profile_s { pidProfile_t pidProfile; - controlRateConfig_t controlRateProfile; + uint8_t activeRateProfile; + controlRateConfig_t controlRateProfile[MAX_RATEPROFILES]; } profile_t; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index e827765c1c..c89c50bef3 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -651,7 +651,6 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm }; } -/* Is this needed ? void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) { bool applied = false; @@ -659,7 +658,7 @@ void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) switch(adjustmentFunction) { case ADJUSTMENT_RATE_PROFILE: if (getCurrentControlRateProfile() != position) { - changeProfile(position); // Is this safe to change profile "in flight" ? + changeControlRateProfile(position); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); applied = true; } @@ -670,7 +669,7 @@ void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) beeperConfirmationBeeps(position + 1); } } -*/ + #define RESET_FREQUENCY_2HZ (1000 / 2) void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig) @@ -724,10 +723,10 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { - //uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); TODO - cleanup - //uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; TODO - cleanup + uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); + uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; - // applySelectAdjustment(adjustmentFunction, position); + applySelectAdjustment(adjustmentFunction, position); } MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex); } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0ffd146d1f..f03f90c70a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -113,6 +113,7 @@ static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); static void cliPlaySound(char *cmdline); static void cliProfile(char *cmdline); +static void cliRateProfile(char *cmdline); static void cliReboot(void); static void cliSave(char *cmdline); static void cliSerial(char *cmdline); @@ -279,6 +280,7 @@ const clicmd_t cmdTable[] = { "[]\r\n", cliPlaySound), CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), + CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), @@ -470,7 +472,7 @@ typedef enum { // value section MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), - + PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // value mode MODE_DIRECT = (0 << VALUE_MODE_OFFSET), MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) @@ -634,16 +636,17 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rcRate8, .config.minmax = { 0, 250 } }, - { "rc_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rcExpo8, .config.minmax = { 0, 100 } }, - { "rc_yaw_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rcYawExpo8, .config.minmax = { 0, 100 } }, - { "thr_mid", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.thrMid8, .config.minmax = { 0, 100 } }, - { "thr_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.thrExpo8, .config.minmax = { 0, 100 } }, - { "roll_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "pitch_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, - { "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, - { "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, + { "rateProfile", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].activeRateProfile, .config.minmax = { 0, MAX_RATEPROFILES } }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, + { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, + { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, + { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, + { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, + { "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, + { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, + { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "acro_plus_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, { "acro_plus_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, @@ -1678,9 +1681,10 @@ static void dumpValues(uint16_t valueSection) typedef enum { DUMP_MASTER = (1 << 0), DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), } dumpFlags_e; -#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE) +#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE | DUMP_RATES) static const char* const sectionBreak = "\r\n"; @@ -1704,6 +1708,9 @@ static void cliDump(char *cmdline) if (strcasecmp(cmdline, "profile") == 0) { dumpMask = DUMP_PROFILE; // only } + if (strcasecmp(cmdline, "rates") == 0) { + dumpMask = DUMP_RATES; + } if (dumpMask & DUMP_MASTER) { @@ -1842,7 +1849,19 @@ static void cliDump(char *cmdline) printSectionBreak(); dumpValues(PROFILE_VALUE); + dumpValues(PROFILE_RATE_VALUE); } + if (dumpMask & DUMP_RATES) { + cliPrint("\r\n# dump rates\r\n"); + + cliPrint("\r\n# rateprofile\r\n"); + cliRateProfile(""); + + printSectionBreak(); + + dumpValues(PROFILE_RATE_VALUE); + } + } void cliEnter(serialPort_t *serialPort) @@ -2138,6 +2157,21 @@ static void cliProfile(char *cmdline) } } +static void cliRateProfile(char *cmdline) { + int i; + + if (isEmpty(cmdline)) { + cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfile()); + return; + } else { + i = atoi(cmdline); + if (i >= 0 && i < MAX_RATEPROFILES) { + changeControlRateProfile(i); + cliRateProfile(""); + } + } +} + static void cliReboot(void) { cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); @@ -2200,6 +2234,10 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); } + if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); + } + switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: value = *(uint8_t *)ptr; @@ -2249,6 +2287,9 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value) if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); } + if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); + } switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: