diff --git a/Makefile b/Makefile index 1b1eef68c0..d85e240077 100644 --- a/Makefile +++ b/Makefile @@ -688,6 +688,7 @@ COMMON_SRC = \ io/vtx_string.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ + io/vtx.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 239e9302e2..5834504bfa 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -97,7 +97,8 @@ #define PG_DISPLAY_PORT_MAX7456_CONFIG 513 #define PG_VCD_CONFIG 514 #define PG_VTX_CONFIG 515 -#define PG_BETAFLIGHT_END 515 +#define PG_SONAR_CONFIG 516 +#define PG_BETAFLIGHT_END 516 // OSD configuration (subject to change) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e0e5728ced..9821de0730 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -91,6 +91,8 @@ extern uint8_t __config_end; #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" #include "io/flashfs.h" +#include "io/displayport_max7456.h" +#include "io/displayport_msp.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -715,6 +717,9 @@ static const clivalue_t valueTable[] = { { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_direction) }, +// PG_PID_CONFIG + { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) }, + // PG_PID_PROFILE { "d_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, { "d_lowpass", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) }, @@ -731,7 +736,6 @@ static const clivalue_t valueTable[] = { { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, - { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) }, { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0.1, 1.0 }, PG_PID_CONFIG, offsetof(pidProfile_t, pidSumLimit) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PITCH]) }, @@ -793,6 +797,7 @@ static const clivalue_t valueTable[] = { { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_visual_beeper) }, #endif +// PG_SDCARD_CONFIG #ifdef USE_SDCARD { "sdcard_dma", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, useDma) }, #endif @@ -834,28 +839,35 @@ static const clivalue_t valueTable[] = { { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, #endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, + +// PG_VTX_CONFIG #ifdef VTX { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_band) }, { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_channel) }, { "vtx_mode", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_mode) }, { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 5600, 5950 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_mhz) }, #endif - #if defined(USE_RTC6705) { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 39 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_channel) }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_power) }, #endif + +// PG_VCD_CONFIG #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) }, { "vcd_h_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -32, 31 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, h_offset) }, { "vcd_v_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -15, 16 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, v_offset) }, #endif + +// PG_DISPLAY_PORT_MSP_CONFIG #ifdef USE_MSP_DISPLAYPORT { "displayport_msp_col_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, { "displayport_msp_row_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) }, #endif + +// PG_DISPLAY_PORT_MSP_CONFIG #ifdef USE_MAX7456 - { "displayport_max7456_col_adjust", VAR_INT8| MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MAX7456_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, + { "displayport_max7456_col_adjust", VAR_INT8| MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, { "displayport_max7456_row_adjust", VAR_INT8| MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MAX7456_CONFIG, offsetof(displayPortProfile_t, rowAdjust) }, #endif }; @@ -1209,12 +1221,17 @@ static pitotmeterConfig_t pitotmeterConfigCopy; #endif static featureConfig_t featureConfigCopy; static rxConfig_t rxConfigCopy; +// PG_PWM_CONFIG +#ifdef USE_PWM +static pwmConfig_t pwmConfigCopy; +#endif #ifdef BLACKBOX static blackboxConfig_t blackboxConfigCopy; #endif static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT]; static motorConfig_t motorConfigCopy; +static throttleCorrectionConfig_t throttleCorrectionConfigCopy; static failsafeConfig_t failsafeConfigCopy; static boardAlignment_t boardAlignmentCopy; #ifdef USE_SERVOS @@ -1233,11 +1250,9 @@ static armingConfig_t armingConfigCopy; static rcControlsConfig_t rcControlsConfigCopy; #ifdef GPS static gpsConfig_t gpsConfigCopy; -#endif -#ifdef NAV -static positionEstimationConfig_t positionEstimationConfigCopy; static navigationConfig_t navigationConfigCopy; #endif +static airplaneConfig_t airplaneConfigCopy; #ifdef TELEMETRY static telemetryConfig_t telemetryConfigCopy; #endif @@ -1252,7 +1267,21 @@ static osdConfig_t osdConfigCopy; static systemConfig_t systemConfigCopy; #ifdef BEEPER static beeperDevConfig_t beeperDevConfigCopy; +static beeperConfig_t beeperConfigCopy; #endif +#if defined(USE_RTC6705) || defined(VTX) +static vtxConfig_t vtxConfigCopy; +#endif +#ifdef USE_MAX7456 +vcdProfile_t vcdProfileCopy; +#endif +#ifdef USE_MSP_DISPLAYPORT +displayPortProfile_t displayPortProfileMspCopy; +#endif +#ifdef USE_MAX7456 +displayPortProfile_t displayPortProfileMax7456Copy; +#endif +static pidConfig_t pidConfigCopy; static controlRateConfig_t controlRateProfilesCopy[CONTROL_RATE_PROFILE_COUNT]; static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; #endif // USE_PARAMETER_GROUPS @@ -1462,6 +1491,12 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.currentConfig = &rxConfigCopy; ret.defaultConfig = rxConfig(); break; +#ifdef USE_PWM + case PG_PWM_CONFIG: + ret.currentConfig = &pwmConfigCopy; + ret.defaultConfig = pwmConfig(); + break; +#endif #ifdef BLACKBOX case PG_BLACKBOX_CONFIG: ret.currentConfig = &blackboxConfigCopy; @@ -1472,6 +1507,9 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.currentConfig = &motorConfigCopy; ret.defaultConfig = motorConfig(); break; + case PG_THROTTLE_CORRECTION_CONFIG: + ret.currentConfig = &throttleCorrectionConfigCopy; + ret.defaultConfig = throttleCorrectionConfig(); case PG_FAILSAFE_CONFIG: ret.currentConfig = &failsafeConfigCopy; ret.defaultConfig = failsafeConfig(); @@ -1523,17 +1561,15 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.currentConfig = &gpsConfigCopy; ret.defaultConfig = gpsConfig(); break; -#endif -#ifdef NAV - case PG_POSITION_ESTIMATION_CONFIG: - ret.currentConfig = &positionEstimationConfigCopy; - ret.defaultConfig = positionEstimationConfig(); - break; - case PG_NAV_CONFIG: + case PG_NAVIGATION_CONFIG: ret.currentConfig = &navigationConfigCopy; ret.defaultConfig = navigationConfig(); break; #endif + case PG_AIRPLANE_CONFIG: + ret.currentConfig = &airplaneConfigCopy; + ret.defaultConfig = airplaneConfig(); + break; #ifdef TELEMETRY case PG_TELEMETRY_CONFIG: ret.currentConfig = &telemetryConfigCopy; @@ -1560,10 +1596,10 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.currentConfig = &controlRateProfilesCopy[0]; ret.defaultConfig = controlRateProfiles(0); break; -/*!!TODO case PG_PID_PROFILE: - ret.currentConfig = &pidProfileCopy[getConfigProfile()]; - ret.defaultConfig = pidProfile(); - break;*/ + case PG_PID_PROFILE: + ret.currentConfig = &pidProfileCopy[0]; + ret.defaultConfig = pidProfiles(0); + break; case PG_RX_FAILSAFE_CHANNEL_CONFIG: ret.currentConfig = &rxFailsafeChannelConfigsCopy[0]; ret.defaultConfig = rxFailsafeChannelConfigs(0); @@ -1594,10 +1630,38 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.currentConfig = &adjustmentRangesCopy[0]; ret.defaultConfig = adjustmentRanges(0); break; +#ifdef BEEPER case PG_BEEPER_CONFIG: + ret.currentConfig = &beeperConfigCopy; + ret.defaultConfig = beeperConfig(); + break; + case PG_BEEPER_DEV_CONFIG: ret.currentConfig = &beeperDevConfigCopy; ret.defaultConfig = beeperDevConfig(); break; +#endif +#ifdef USE_MAX7456 + case PG_VCD_CONFIG: + ret.currentConfig = &vcdProfileCopy; + ret.defaultConfig = vcdProfile(); + break; +#endif +#ifdef USE_MSP_DISPLAYPORT + case PG_DISPLAY_PORT_MSP_CONFIG: + ret.currentConfig = &displayPortProfileMspCopy; + ret.defaultConfig = displayPortProfileMsp(); + break; +#endif +#ifdef USE_MAX7456 + case PG_DISPLAY_PORT_MAX7456_CONFIG: + ret.currentConfig = &displayPortProfileMax7456Copy; + ret.defaultConfig = displayPortProfileMax7456(); + break; +#endif + case PG_PID_CONFIG: + ret.currentConfig = &pidConfigCopy; + ret.defaultConfig = pidConfig(); + break; default: ret.currentConfig = NULL; ret.defaultConfig = NULL; @@ -1725,16 +1789,7 @@ 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; @@ -1756,7 +1811,14 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t * } } } -#endif +#endif // USE_PARAMETER_GROUPS + +static void cliPrintVar(const clivalue_t *var, uint32_t full) +{ + const void *ptr = getValuePointer(var); + + printValuePointer(var, ptr, full); +} static void cliPrintVarRange(const clivalue_t *var) { @@ -3139,7 +3201,7 @@ static void cliFlashRead(char *cmdline) #endif #endif -#ifdef VTX +#if defined(USE_RTC6705) || defined(VTX) static void printVtx(uint8_t dumpMask, const vtxConfig_t *vtxConfig, const vtxConfig_t *vtxConfigDefault) { // print out vtx channel settings @@ -3185,7 +3247,7 @@ static void cliVtx(char *cmdline) ptr = cmdline; i = atoi(ptr++); if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { - vtxChannelActivationCondition_t *cac = &vtxConfig()->vtxChannelActivationConditions[i]; + vtxChannelActivationCondition_t *cac = &vtxConfigMutable()->vtxChannelActivationConditions[i]; uint8_t validArgumentCount = 0; ptr = nextArg(ptr); if (ptr) { @@ -3797,7 +3859,33 @@ static void cliRateProfile(char *cmdline) } } -#ifndef USE_PARAMETER_GROUPS +#ifdef USE_PARAMETER_GROUPS +static void cliDumpPidProfile(uint8_t pidProfileIndex, uint8_t dumpMask) +{ + if (pidProfileIndex >= MAX_PROFILE_COUNT) { + // Faulty values + return; + } + changePidProfile(pidProfileIndex); + cliPrintHashLine("profile"); + cliProfile(""); + cliPrint("\r\n"); + dumpAllValues(PROFILE_VALUE, dumpMask); +} + +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask) +{ + if (rateProfileIndex >= CONTROL_RATE_PROFILE_COUNT) { + // Faulty values + return; + } + changeControlRateProfile(rateProfileIndex); + cliPrintHashLine("rateprofile"); + cliRateProfile(""); + cliPrint("\r\n"); + dumpAllValues(PROFILE_RATE_VALUE, dumpMask); +} +#else static void cliDumpPidProfile(uint8_t pidProfileIndex, uint8_t dumpMask, const master_t *defaultConfig) { if (pidProfileIndex >= MAX_PROFILE_COUNT) { @@ -4130,6 +4218,7 @@ const cliResourceValue_t resourceTable[] = { }; #ifndef USE_PARAMETER_GROUPS +//!! TODO for parameter groups static void printResource(uint8_t dumpMask, const master_t *defaultConfig) { for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { @@ -4352,8 +4441,6 @@ static void backupConfigs(void) #endif } } - const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); - memcpy(&pidProfileCopy[0], reg->address, sizeof(pidProfile_t) * MAX_PROFILE_COUNT); } static void restoreConfigs(void) @@ -4373,8 +4460,6 @@ static void restoreConfigs(void) #endif } } - const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); - memcpy(reg->address, &pidProfileCopy[0], sizeof(pidProfile_t) * MAX_PROFILE_COUNT); } #endif @@ -4398,18 +4483,12 @@ static void printConfig(char *cmdline, bool doDiff) dumpMask = dumpMask | DO_DIFF; } -#ifdef USE_PARAMETER_GROUPS backupConfigs(); // reset all configs to defaults to do differencing resetConfigs(); + #if defined(TARGET_CONFIG) 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 @@ -4429,80 +4508,80 @@ static void printConfig(char *cmdline, bool doDiff) #ifdef USE_RESOURCE_MGMT cliPrintHashLine("resources"); - printResource(dumpMask, &defaultConfig); + //!!TODO printResource(dumpMask, &defaultConfig); #endif #ifndef USE_QUAD_MIXER_ONLY cliPrintHashLine("mixer"); - const bool equalsDefault = mixerConfig()->mixerMode == defaultConfig.mixerConfig.mixerMode; + const bool equalsDefault = mixerConfigCopy.mixerMode == mixerConfig()->mixerMode; const char *formatMixer = "mixer %s\r\n"; - cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerConfig.mixerMode - 1]); - cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfig()->mixerMode - 1]); + cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfig()->mixerMode - 1]); + cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfigCopy.mixerMode - 1]); - cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "\r\nmmix reset\r\n\r\n"); + cliDumpPrintf(dumpMask, customMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n\r\n"); - printMotorMix(dumpMask, customMotorMixer(0), defaultConfig.customMotorMixer); + printMotorMix(dumpMask, customMotorMixerCopy, customMotorMixer(0)); #ifdef USE_SERVOS cliPrintHashLine("servo"); - printServo(dumpMask, servoParams(0), defaultConfig.servoProfile.servoConf); + printServo(dumpMask, servoParamsCopy, servoParams(0)); cliPrintHashLine("servo mix"); // print custom servo mixer if exists - cliDumpPrintf(dumpMask, masterConfig.customServoMixer[0].rate == 0, "smix reset\r\n\r\n"); - printServoMix(dumpMask, customServoMixers(0), defaultConfig.customServoMixer); + cliDumpPrintf(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n\r\n"); + printServoMix(dumpMask, customServoMixersCopy, customServoMixers(0)); #endif #endif cliPrintHashLine("feature"); - printFeature(dumpMask, featureConfig(), &defaultConfig.featureConfig); + printFeature(dumpMask, &featureConfigCopy, featureConfig()); #ifdef BEEPER cliPrintHashLine("beeper"); - printBeeper(dumpMask, beeperConfig(), &defaultConfig.beeperConfig); + printBeeper(dumpMask, &beeperConfigCopy, beeperConfig()); #endif cliPrintHashLine("map"); - printMap(dumpMask, rxConfig(), &defaultConfig.rxConfig); + printMap(dumpMask, &rxConfigCopy, rxConfig()); cliPrintHashLine("serial"); - printSerial(dumpMask, serialConfig(), &defaultConfig.serialConfig); + printSerial(dumpMask, &serialConfigCopy, serialConfig()); #ifdef LED_STRIP cliPrintHashLine("led"); - printLed(dumpMask, ledStripConfig()->ledConfigs, defaultConfig.ledStripConfig.ledConfigs); + printLed(dumpMask, ledStripConfigCopy.ledConfigs, ledStripConfig()->ledConfigs); cliPrintHashLine("color"); - printColor(dumpMask, ledStripConfig()->colors, defaultConfig.ledStripConfig.colors); + printColor(dumpMask, ledStripConfigCopy.colors, ledStripConfig()->colors); cliPrintHashLine("mode_color"); - printModeColor(dumpMask, ledStripConfig(), &defaultConfig.ledStripConfig); + printModeColor(dumpMask, &ledStripConfigCopy, ledStripConfig()); #endif cliPrintHashLine("aux"); - printAux(dumpMask, modeActivationConditions(0), defaultConfig.modeActivationProfile.modeActivationConditions); + printAux(dumpMask, modeActivationConditionsCopy, modeActivationConditions(0)); cliPrintHashLine("adjrange"); - printAdjustmentRange(dumpMask, adjustmentRanges(0), defaultConfig.adjustmentProfile.adjustmentRanges); + printAdjustmentRange(dumpMask, adjustmentRangesCopy, adjustmentRanges(0)); cliPrintHashLine("rxrange"); - printRxRange(dumpMask, rxConfig()->channelRanges, defaultConfig.rxConfig.channelRanges); + printRxRange(dumpMask, rxChannelRangeConfigsCopy, rxChannelRangeConfigs(0)); -#ifdef VTX +#if defined(USE_RTC6705) || defined(VTX) cliPrintHashLine("vtx"); - printVtx(dumpMask, vtxConfig(), &defaultConfig.vtxConfig); + printVtx(dumpMask, &vtxConfigCopy, vtxConfig()); #endif cliPrintHashLine("rxfail"); - printRxFailsafe(dumpMask, rxConfig()->failsafe_channel_configurations, defaultConfig.rxConfig.failsafe_channel_configurations); + printRxFailsafe(dumpMask, rxFailsafeChannelConfigsCopy, rxFailsafeChannelConfigs(0)); cliPrintHashLine("master"); - dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); + dumpAllValues(MASTER_VALUE, dumpMask); if (dumpMask & DUMP_ALL) { const uint8_t pidProfileIndexSave = getCurrentPidProfileIndex(); for (uint32_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { - cliDumpPidProfile(pidProfileIndex, dumpMask, &defaultConfig); + cliDumpPidProfile(pidProfileIndex, dumpMask); } changePidProfile(pidProfileIndexSave); cliPrintHashLine("restore original profile selection"); @@ -4510,7 +4589,7 @@ static void printConfig(char *cmdline, bool doDiff) const uint8_t controlRateProfileIndexSave = getCurrentControlRateProfileIndex(); for (uint32_t rateIndex = 0; rateIndex < CONTROL_RATE_PROFILE_COUNT; rateIndex++) { - cliDumpRateProfile(rateIndex, dumpMask, &defaultConfig); + cliDumpRateProfile(rateIndex, dumpMask); } changeControlRateProfile(controlRateProfileIndexSave); cliPrintHashLine("restore original rateprofile selection"); @@ -4519,20 +4598,19 @@ static void printConfig(char *cmdline, bool doDiff) cliPrintHashLine("save configuration"); cliPrint("save"); } else { - cliDumpPidProfile(getCurrentPidProfileIndex(), dumpMask, &defaultConfig); + cliDumpPidProfile(getCurrentPidProfileIndex(), dumpMask); - cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask, &defaultConfig); + cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask); } } if (dumpMask & DUMP_PROFILE) { - cliDumpPidProfile(getCurrentPidProfileIndex(), dumpMask, &defaultConfig); + cliDumpPidProfile(getCurrentPidProfileIndex(), dumpMask); } if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask, &defaultConfig); + cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask); } -#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 61432ca423..b93395afb6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -125,11 +125,18 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = { 0 } ); +#ifdef BEEPER PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); - +#endif +#ifdef USE_ADC PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0); +#endif +#ifdef USE_PWM PG_REGISTER_WITH_RESET_FN(pwmConfig_t, pwmConfig, PG_PWM_CONFIG, 0); +#endif +#ifdef USE_PPM PG_REGISTER_WITH_RESET_FN(ppmConfig_t, ppmConfig, PG_PPM_CONFIG, 0); +#endif PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(serialPinConfig_t, serialPinConfig, PG_SERIAL_PIN_CONFIG, 0); @@ -819,7 +826,7 @@ static void setPidProfile(uint8_t pidProfileIndex) if (pidProfileIndex < MAX_PROFILE_COUNT) { systemConfigMutable()->pidProfileIndex = pidProfileIndex; currentPidProfile = pidProfilesMutable(pidProfileIndex); - pidInit(); // re-initialise pid controller to re-initialise filters and config + pidInit(currentPidProfile); // re-initialise pid controller to re-initialise filters and config } } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3e8c09ec44..2ecfa88d25 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -56,6 +56,7 @@ #include "msp/msp_serial.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" #include "io/gps.h" #include "io/motors.h" @@ -63,7 +64,7 @@ #include "io/serial.h" #include "io/statusindicator.h" #include "io/transponder_ir.h" -#include "io/asyncfatfs/asyncfatfs.h" +#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index dfa25672f1..7ae10f1bd8 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -428,7 +428,7 @@ void init(void) LED1_OFF; // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit() - pidInit(); + pidInit(currentPidProfile); imuInit(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f9ba6f96b6..e3bf9c1d97 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -83,6 +83,7 @@ #include "io/serial_4way.h" #include "io/servos.h" #include "io/transponder_ir.h" +#include "io/vtx.h" #include "msp/msp.h" #include "msp/msp_protocol.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2ce01ec706..e5a612d3a1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -256,11 +256,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) { ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); } -void pidInit(void) +void pidInit(const pidProfile_t *pidProfile) { pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime - pidInitFilters(currentPidProfile); - pidInitConfig(currentPidProfile); + pidInitFilters(pidProfile); + pidInitConfig(pidProfile); } static float calcHorizonLevelStrength(void) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e17ba102ba..38e44fe565 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -115,5 +115,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); +void pidInit(const pidProfile_t *pidProfile); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index ba38d0fd6c..67233aa9b5 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -53,13 +53,15 @@ PG_REGISTER_WITH_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, PG_BEEPER_DE #define IS_OPEN_DRAIN true #define IS_INVERTED false #endif -#ifndef BEEPER -#define BEEPER NONE +#ifdef BEEPER +#define BEEPER_PIN BEEPER +#else +#define BEEPER_PIN NONE #endif PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, .isOpenDrain = IS_OPEN_DRAIN, .isInverted = IS_INVERTED, - .ioTag = IO_TAG(BEEPER) + .ioTag = IO_TAG(BEEPER_PIN) ); #if FLASH_SIZE > 64 diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index b52bf0a1f2..eb99977f04 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -37,6 +37,12 @@ #include "io/transponder_ir.h" +PG_REGISTER_WITH_RESET_TEMPLATE(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0); + +PG_RESET_TEMPLATE(transponderConfig_t, transponderConfig, + .data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC } // Note, this is NOT a valid transponder code, it's just for testing production hardware +); + static bool transponderInitialised = false; static bool transponderRepeat = false; diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 9501cea1ee..006fd2feb8 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -19,8 +19,6 @@ // Get target build configuration #include "platform.h" -#ifdef VTX - #include "common/maths.h" #include "config/config_eeprom.h" @@ -29,12 +27,14 @@ #include "drivers/vtx_rtc6705.h" +#include "fc/config.h" #include "fc/runtime_config.h" #include "io/beeper.h" #include "io/osd.h" #include "io/vtx.h" +#if defined(USE_RTC6705) || defined(VTX) PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 0); @@ -45,6 +45,10 @@ PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, .vtx_mhz = 5740 //F0 ); +#endif + +#ifdef VTX + static uint8_t locked = 0; void vtxInit(void) @@ -77,22 +81,22 @@ static void setChannelSaveAndNotify(uint8_t *bandOrChannel, uint8_t step, int32_ void vtxIncrementBand(void) { - setChannelSaveAndNotify(&(vtxConfig()->vtx_band), 1, RTC6705_BAND_MIN, RTC6705_BAND_MAX); + setChannelSaveAndNotify(&(vtxConfigMutable()->vtx_band), 1, RTC6705_BAND_MIN, RTC6705_BAND_MAX); } void vtxDecrementBand(void) { - setChannelSaveAndNotify(&(vtxConfig()->vtx_band), -1, RTC6705_BAND_MIN, RTC6705_BAND_MAX); + setChannelSaveAndNotify(&(vtxConfigMutable()->vtx_band), -1, RTC6705_BAND_MIN, RTC6705_BAND_MAX); } void vtxIncrementChannel(void) { - setChannelSaveAndNotify(&(vtxConfig()->vtx_channel), 1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); + setChannelSaveAndNotify(&(vtxConfigMutable()->vtx_channel), 1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); } void vtxDecrementChannel(void) { - setChannelSaveAndNotify(&(vtxConfig()->vtx_channel), -1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); + setChannelSaveAndNotify(&(vtxConfigMutable()->vtx_channel), -1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); } void vtxUpdateActivatedChannel(void) @@ -103,10 +107,9 @@ void vtxUpdateActivatedChannel(void) if (vtxConfig()->vtx_mode == 2 && !locked) { static uint8_t lastIndex = -1; - uint8_t index; - for (index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { - vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index]; + for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { + const vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index]; if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range) && index != lastIndex) { diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 0e3d0768a0..cd8e11799f 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -32,6 +32,8 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/io.h" + #include "fc/runtime_config.h" #include "sensors/sensors.h" @@ -49,6 +51,13 @@ float sonarMaxTiltCos; static int32_t calculatedAltitude; +PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t, sonarConfig, PG_SONAR_CONFIG, 0); + +PG_RESET_TEMPLATE(sonarConfig_t, sonarConfig, + .triggerTag = IO_TAG(SONAR_TRIGGER_PIN), + .echoTag = IO_TAG(SONAR_ECHO_PIN) +); + void sonarInit(const sonarConfig_t *sonarConfig) { sonarRange_t sonarRange; diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index d123679394..43a5ac2c7d 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -28,6 +28,8 @@ extern int16_t sonarMaxRangeCm; extern int16_t sonarCfAltCm; extern int16_t sonarMaxAltWithTiltCm; +PG_DECLARE(sonarConfig_t, sonarConfig); + void sonarInit(const sonarConfig_t *sonarConfig); void sonarUpdate(timeUs_t currentTimeUs); int32_t sonarRead(void); diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 03ee90dffd..d9667eac9a 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -21,20 +21,18 @@ #include #ifdef TARGET_CONFIG + +#include "common/axis.h" + #include "drivers/pwm_esc_detect.h" -#include "drivers/pwm_output.h" -#include "fc/rc_controls.h" +#include "fc/config.h" -#include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" #include "rx/rx.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #ifdef BRUSHED_MOTORS_PWM_RATE #undef BRUSHED_MOTORS_PWM_RATE #endif @@ -42,29 +40,29 @@ #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->rxConfig.spektrum_sat_bind = 5; - config->rxConfig.spektrum_sat_bind_autoreset = 1; + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; } - config->profile[0].pidProfile.P8[ROLL] = 90; - config->profile[0].pidProfile.I8[ROLL] = 44; - config->profile[0].pidProfile.D8[ROLL] = 60; - config->profile[0].pidProfile.P8[PITCH] = 90; - config->profile[0].pidProfile.I8[PITCH] = 44; - config->profile[0].pidProfile.D8[PITCH] = 60; + pidProfilesMutable(0)->P8[FD_ROLL] = 90; + pidProfilesMutable(0)->I8[FD_ROLL] = 44; + pidProfilesMutable(0)->D8[FD_ROLL] = 60; + pidProfilesMutable(0)->P8[FD_PITCH] = 90; + pidProfilesMutable(0)->I8[FD_PITCH] = 44; + pidProfilesMutable(0)->D8[FD_PITCH] = 60; - config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } #endif diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index dfbf92461c..ce79365016 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -21,27 +21,21 @@ #include #ifdef TARGET_CONFIG + #include "common/axis.h" -#include "drivers/compass.h" -#include "drivers/io.h" +#include "drivers/light_led.h" #include "drivers/pwm_esc_detect.h" -#include "drivers/pwm_output.h" -#include "drivers/sensor.h" -#include "fc/rc_controls.h" +#include "fc/config.h" -#include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" #include "rx/rx.h" -#include "sensors/sensors.h" #include "sensors/compass.h" - -#include "config/config_profile.h" -#include "config/config_master.h" +#include "sensors/gyro.h" #include "hardware_revision.h" @@ -52,11 +46,11 @@ #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { /* depending on revision ... depends on the LEDs to be utilised. */ if (hardwareRevision == AFF3_REV_2) { - config->statusLedConfig.polarity = 0 + statusLedConfigMutable()->polarity = 0 #ifdef LED0_A_INVERTED | BIT(0) #endif @@ -69,45 +63,45 @@ void targetConfiguration(master_t *config) ; for (int i = 0; i < LED_NUMBER; i++) { - config->statusLedConfig.ledTags[i] = IO_TAG_NONE; + statusLedConfigMutable()->ledTags[i] = IO_TAG_NONE; } #ifdef LED0_A - config->statusLedConfig.ledTags[0] = IO_TAG(LED0_A); + statusLedConfigMutable()->ledTags[0] = IO_TAG(LED0_A); #endif #ifdef LED1_A - config->statusLedConfig.ledTags[1] = IO_TAG(LED1_A); + statusLedConfigMutable()->ledTags[1] = IO_TAG(LED1_A); #endif #ifdef LED2_A - config->statusLedConfig.ledTags[2] = IO_TAG(LED2_A); + statusLedConfigMutable()->ledTags[2] = IO_TAG(LED2_A); #endif } else { - config->gyroConfig.gyro_sync_denom = 2; - config->pidConfig.pid_process_denom = 2; + gyroConfigMutable()->gyro_sync_denom = 2; + pidConfigMutable()->pid_process_denom = 2; } - config->rxConfig.spektrum_sat_bind = 5; - config->rxConfig.spektrum_sat_bind_autoreset = 1; - config->compassConfig.mag_hardware = MAG_NONE; // disabled by default + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; + compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - config->pidConfig.pid_process_denom = 1; + motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + pidConfigMutable()->pid_process_denom = 1; } - config->profile[0].pidProfile.P8[ROLL] = 90; - config->profile[0].pidProfile.I8[ROLL] = 44; - config->profile[0].pidProfile.D8[ROLL] = 60; - config->profile[0].pidProfile.P8[PITCH] = 90; - config->profile[0].pidProfile.I8[PITCH] = 44; - config->profile[0].pidProfile.D8[PITCH] = 60; + pidProfilesMutable(0)->P8[FD_ROLL] = 90; + pidProfilesMutable(0)->I8[FD_ROLL] = 44; + pidProfilesMutable(0)->D8[FD_ROLL] = 60; + pidProfilesMutable(0)->P8[FD_PITCH] = 90; + pidProfilesMutable(0)->I8[FD_PITCH] = 44; + pidProfilesMutable(0)->D8[FD_PITCH] = 60; - config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } #endif diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index eba42556fc..b70f3623a0 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -21,17 +21,15 @@ #include #ifdef TARGET_CONFIG + #include "common/axis.h" -#include "drivers/sensor.h" -#include "drivers/compass.h" +#include "config/feature.h" + #include "drivers/pwm_esc_detect.h" -#include "drivers/pwm_output.h" -#include "drivers/serial.h" -#include "fc/rc_controls.h" +#include "fc/config.h" -#include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -41,13 +39,9 @@ #include "telemetry/telemetry.h" -#include "sensors/sensors.h" +#include "sensors/battery.h" #include "sensors/compass.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #include "hardware_revision.h" #define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V @@ -60,45 +54,45 @@ #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->batteryConfig.currentMeterOffset = CURRENTOFFSET; - config->batteryConfig.currentMeterScale = CURRENTSCALE; - config->compassConfig.mag_hardware = MAG_NONE; // disabled by default + batteryConfigMutable()->currentMeterOffset = CURRENTOFFSET; + batteryConfigMutable()->currentMeterScale = CURRENTSCALE; + compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - config->pidConfig.pid_process_denom = 1; + motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + pidConfigMutable()->pid_process_denom = 1; } if (hardwareRevision == AFF4_REV_1) { - config->rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; - config->rxConfig.spektrum_sat_bind = 5; - config->rxConfig.spektrum_sat_bind_autoreset = 1; + rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048; + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; } else { - config->rxConfig.serialrx_provider = SERIALRX_SBUS; - config->rxConfig.sbus_inversion = 0; - config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; - config->telemetryConfig.telemetry_inversion = 0; - intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY, &config->featureConfig.enabledFeatures); + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + rxConfigMutable()->sbus_inversion = 0; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; + telemetryConfigMutable()->telemetry_inversion = 0; + featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY); } - config->profile[0].pidProfile.P8[ROLL] = 53; - config->profile[0].pidProfile.I8[ROLL] = 45; - config->profile[0].pidProfile.D8[ROLL] = 52; - config->profile[0].pidProfile.P8[PITCH] = 53; - config->profile[0].pidProfile.I8[PITCH] = 45; - config->profile[0].pidProfile.D8[PITCH] = 52; - config->profile[0].pidProfile.P8[YAW] = 64; - config->profile[0].pidProfile.D8[YAW] = 18; + pidProfilesMutable(0)->P8[FD_ROLL] = 53; + pidProfilesMutable(0)->I8[FD_ROLL] = 45; + pidProfilesMutable(0)->D8[FD_ROLL] = 52; + pidProfilesMutable(0)->P8[FD_PITCH] = 53; + pidProfilesMutable(0)->I8[FD_PITCH] = 45; + pidProfilesMutable(0)->D8[FD_PITCH] = 52; + pidProfilesMutable(0)->P8[FD_YAW] = 64; + pidProfilesMutable(0)->D8[FD_YAW] = 18; - config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } #endif diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 153bc7d333..02330c6692 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -23,15 +23,12 @@ #ifdef TARGET_CONFIG #include "common/axis.h" -#include "drivers/sensor.h" -#include "drivers/compass.h" +#include "config/feature.h" + #include "drivers/pwm_esc_detect.h" -#include "drivers/pwm_output.h" -#include "drivers/serial.h" -#include "fc/rc_controls.h" +#include "fc/config.h" -#include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -41,13 +38,9 @@ #include "telemetry/telemetry.h" -#include "sensors/sensors.h" +#include "sensors/battery.h" #include "sensors/compass.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #include "hardware_revision.h" #define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V @@ -60,45 +53,45 @@ #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->batteryConfig.currentMeterOffset = CURRENTOFFSET; - config->batteryConfig.currentMeterScale = CURRENTSCALE; - config->compassConfig.mag_hardware = MAG_NONE; // disabled by default + batteryConfigMutable()->currentMeterOffset = CURRENTOFFSET; + batteryConfigMutable()->currentMeterScale = CURRENTSCALE; + compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - config->pidConfig.pid_process_denom = 1; + motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + pidConfigMutable()->pid_process_denom = 1; } if (hardwareRevision == AFF7_REV_1) { - config->rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; - config->rxConfig.spektrum_sat_bind = 5; - config->rxConfig.spektrum_sat_bind_autoreset = 1; + rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048; + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; } else { - config->rxConfig.serialrx_provider = SERIALRX_SBUS; - config->rxConfig.sbus_inversion = 0; - config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; - config->telemetryConfig.telemetry_inversion = 0; - intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY, &config->featureConfig.enabledFeatures); + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + rxConfigMutable()->sbus_inversion = 0; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; + telemetryConfigMutable()->telemetry_inversion = 0; + featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY); } - config->profile[0].pidProfile.P8[ROLL] = 53; - config->profile[0].pidProfile.I8[ROLL] = 45; - config->profile[0].pidProfile.D8[ROLL] = 52; - config->profile[0].pidProfile.P8[PITCH] = 53; - config->profile[0].pidProfile.I8[PITCH] = 45; - config->profile[0].pidProfile.D8[PITCH] = 52; - config->profile[0].pidProfile.P8[YAW] = 64; - config->profile[0].pidProfile.D8[YAW] = 18; + pidProfilesMutable(0)->P8[FD_ROLL] = 53; + pidProfilesMutable(0)->I8[FD_ROLL] = 45; + pidProfilesMutable(0)->D8[FD_ROLL] = 52; + pidProfilesMutable(0)->P8[FD_PITCH] = 53; + pidProfilesMutable(0)->I8[FD_PITCH] = 45; + pidProfilesMutable(0)->D8[FD_PITCH] = 52; + pidProfilesMutable(0)->P8[FD_YAW] = 64; + pidProfilesMutable(0)->D8[FD_YAW] = 18; - config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } #endif diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index 4eb9db1da4..2cc57a0e80 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -21,11 +21,12 @@ #include #ifdef TARGET_CONFIG -#include "config/config_master.h" -#include "config/feature.h" -void targetConfiguration(master_t *config) +#include "fc/config.h" +#include "sensors/battery.h" + +void targetConfiguration(void) { - config->batteryConfig.currentMeterScale = 220; + batteryConfigMutable()->currentMeterScale = 220; } #endif diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 077287fcfe..008badd389 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -21,41 +21,46 @@ #include #ifdef TARGET_CONFIG -#include "blackbox/blackbox_io.h" -#include "config/config_master.h" +#include "blackbox/blackbox.h" + #include "config/feature.h" #include "drivers/io.h" +#include "fc/config.h" + +#include "sensors/acceleration.h" +#include "sensors/gyro.h" + #include "hardware_revision.h" // alternative defaults settings for BlueJayF4 targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { - config->gyroConfig.gyro_align = CW180_DEG; - config->accelerometerConfig.acc_align = CW180_DEG; - config->beeperDevConfig.ioTag = IO_TAG(BEEPER_OPT); + gyroConfigMutable()->gyro_align = CW180_DEG; + accelerometerConfigMutable()->acc_align = CW180_DEG; + beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); } if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { - intFeatureClear(FEATURE_SDCARD, &config->featureConfig.enabledFeatures); + featureClear(FEATURE_SDCARD); } if (hardwareRevision == BJF4_MINI_REV3A) { - config->adcConfig.vbat.ioTag = IO_TAG(PA4); + adcConfigMutable()->vbat.ioTag = IO_TAG(PA4); } } -void targetValidateConfiguration(master_t *config) +void targetValidateConfiguration(void) { /* make sure the SDCARD cannot be turned on */ if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { - intFeatureClear(FEATURE_SDCARD, &config->featureConfig.enabledFeatures); + featureClear(FEATURE_SDCARD); - if (config->blackboxConfig.device == BLACKBOX_DEVICE_SDCARD) { - config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH; + if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) { + blackboxConfigMutable()->device = BLACKBOX_DEVICE_FLASH; } } } diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 061f37f3b6..d1b1844fa6 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -21,37 +21,41 @@ #include "platform.h" #ifdef TARGET_CONFIG + +#include "common/axis.h" + #include "drivers/serial.h" +#include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "flight/mixer.h" #include "flight/pid.h" -#include "io/motors.h" #include "io/serial.h" #include "rx/rx.h" -#include "config/config_profile.h" -#include "config/config_master.h" +#include "sensors/boardalignment.h" +#include "sensors/compass.h" + // alternative defaults settings for Colibri/Gemini targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->mixerConfig.mixerMode = MIXER_HEX6X; - config->rxConfig.serialrx_provider = 2; + mixerConfigMutable()->mixerMode = MIXER_HEX6X; + rxConfigMutable()->serialrx_provider = 2; - config->motorConfig.minthrottle = 1070; - config->motorConfig.maxthrottle = 2000; + motorConfigMutable()->minthrottle = 1070; + motorConfigMutable()->maxthrottle = 2000; - config->boardAlignment.pitchDegrees = 10; - //config->rcControlsConfig.deadband = 10; - //config->rcControlsConfig.yaw_deadband = 10; - config->compassConfig.mag_hardware = 1; + boardAlignmentMutable()->pitchDegrees = 10; + //rcControlsConfigMutable()->deadband = 10; + //rcControlsConfigMutable()->yaw_deadband = 10; + compassConfigMutable()->mag_hardware = 1; - config->controlRateProfile[0].dynThrPID = 45; - config->controlRateProfile[0].tpa_breakpoint = 1700; - config->serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; + controlRateProfilesMutable(0)->dynThrPID = 45; + controlRateProfilesMutable(0)->tpa_breakpoint = 1700; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } #endif diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index d79a4fa9ac..08001bc8f4 100644 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -16,22 +16,38 @@ */ #include +#include #include #include #ifdef TARGET_CONFIG -#include "common/maths.h" -#include "common/utils.h" +#include "blackbox/blackbox.h" + +#include "common/axis.h" +#include "common/maths.h" -#include "config/config_master.h" #include "config/feature.h" #include "io/ledstrip.h" -#include "io/motors.h" +#include "io/serial.h" + +#include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/servos.h" + +#include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/gyro.h" + +#include "telemetry/telemetry.h" void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs) @@ -52,61 +68,59 @@ void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs) } // alternative defaults settings for COLIBRI RACE targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->motorConfig.minthrottle = 1025; - config->motorConfig.maxthrottle = 1980; - config->motorConfig.mincommand = 1000; - config->servoConfig.dev.servoCenterPulse = 1500; + motorConfigMutable()->minthrottle = 1025; + motorConfigMutable()->maxthrottle = 1980; + motorConfigMutable()->mincommand = 1000; + servoConfigMutable()->dev.servoCenterPulse = 1500; - config->batteryConfig.vbatmaxcellvoltage = 45; - config->batteryConfig.vbatmincellvoltage = 30; - config->batteryConfig.vbatwarningcellvoltage = 35; + batteryConfigMutable()->vbatmaxcellvoltage = 45; + batteryConfigMutable()->vbatmincellvoltage = 30; + batteryConfigMutable()->vbatwarningcellvoltage = 35; - config->flight3DConfig.deadband3d_low = 1406; - config->flight3DConfig.deadband3d_high = 1514; - config->flight3DConfig.neutral3d = 1460; - config->flight3DConfig.deadband3d_throttle = 0; + flight3DConfigMutable()->deadband3d_low = 1406; + flight3DConfigMutable()->deadband3d_high = 1514; + flight3DConfigMutable()->neutral3d = 1460; + flight3DConfigMutable()->deadband3d_throttle = 0; - config->failsafeConfig.failsafe_procedure = 1; - config->failsafeConfig.failsafe_throttle_low_delay = 10; + failsafeConfigMutable()->failsafe_procedure = 1; + failsafeConfigMutable()->failsafe_throttle_low_delay = 10; - config->gyroConfig.gyro_sync_denom = 1; - config->pidConfig.pid_process_denom = 3; - config->blackboxConfig.rate_num = 1; - config->blackboxConfig.rate_denom = 1; + gyroConfigMutable()->gyro_sync_denom = 1; + pidConfigMutable()->pid_process_denom = 3; + blackboxConfigMutable()->rate_num = 1; + blackboxConfigMutable()->rate_denom = 1; - config->rcControlsConfig.deadband = 5; - config->rcControlsConfig.yaw_deadband = 5; + rcControlsConfigMutable()->deadband = 5; + rcControlsConfigMutable()->yaw_deadband = 5; - config->failsafeConfig.failsafe_delay = 10; + failsafeConfigMutable()->failsafe_delay = 10; - config->telemetryConfig.telemetry_inversion = 1; + telemetryConfigMutable()->telemetry_inversion = 1; - config->profile[0].pidProfile.vbatPidCompensation = 1; + pidProfilesMutable(0)->vbatPidCompensation = 1; - config->profile[0].pidProfile.P8[ROLL] = 46; // new PID with preliminary defaults test carefully - config->profile[0].pidProfile.I8[ROLL] = 48; - config->profile[0].pidProfile.D8[ROLL] = 23; - config->profile[0].pidProfile.P8[PITCH] = 89; - config->profile[0].pidProfile.I8[PITCH] = 59; - config->profile[0].pidProfile.D8[PITCH] = 25; - config->profile[0].pidProfile.P8[YAW] = 129; - config->profile[0].pidProfile.I8[YAW] = 50; - config->profile[0].pidProfile.D8[YAW] = 20; + pidProfilesMutable(0)->P8[ROLL] = 46; // new PID with preliminary defaults test carefully + pidProfilesMutable(0)->I8[ROLL] = 48; + pidProfilesMutable(0)->D8[ROLL] = 23; + pidProfilesMutable(0)->P8[PITCH] = 89; + pidProfilesMutable(0)->I8[PITCH] = 59; + pidProfilesMutable(0)->D8[PITCH] = 25; + pidProfilesMutable(0)->P8[YAW] = 129; + pidProfilesMutable(0)->I8[YAW] = 50; + pidProfilesMutable(0)->D8[YAW] = 20; - config->controlRateProfile[0].rates[FD_ROLL] = 86; - config->controlRateProfile[0].rates[FD_PITCH] = 86; - config->controlRateProfile[0].rates[FD_YAW] = 80; + controlRateProfilesMutable(0)->rates[FD_ROLL] = 86; + controlRateProfilesMutable(0)->rates[FD_PITCH] = 86; + controlRateProfilesMutable(0)->rates[FD_YAW] = 80; - targetApplyDefaultLedStripConfig(config->ledStripConfig.ledConfigs); + targetApplyDefaultLedStripConfig(ledStripConfigMutable()->ledConfigs); } -void targetValidateConfiguration(master_t *config) +void targetValidateConfiguration() { - UNUSED(config); - - serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[0].functionMask = FUNCTION_MSP; if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) { featureClear(FEATURE_RX_PARALLEL_PWM); featureClear(FEATURE_RX_MSP); diff --git a/src/main/target/FISHDRONEF4/target.mk b/src/main/target/FISHDRONEF4/target.mk index 0fd79b1f82..c724536673 100644 --- a/src/main/target/FISHDRONEF4/target.mk +++ b/src/main/target/FISHDRONEF4/target.mk @@ -5,4 +5,4 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ drivers/max7456.c \ - drivers/vtx_soft_spi_rtc6705.c \ No newline at end of file + drivers/vtx_soft_spi_rtc6705.c diff --git a/src/main/target/KISSFC/config.c b/src/main/target/KISSFC/config.c index d2ecd36704..f915bec091 100755 --- a/src/main/target/KISSFC/config.c +++ b/src/main/target/KISSFC/config.c @@ -21,32 +21,18 @@ #include #ifdef TARGET_CONFIG -#include "common/utils.h" -#include "drivers/io.h" - -#include "fc/rc_controls.h" - -#include "flight/failsafe.h" -#include "flight/mixer.h" -#include "flight/pid.h" - -#include "rx/rx.h" - -#include "config/config_profile.h" -#include "config/config_master.h" +#include "fc/config.h" #include "sensors/boardalignment.h" -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - UNUSED(config); - #ifdef KISSCC // alternative defaults settings for Beebrain target - config->boardAlignment.rollDegrees = 180; - config->boardAlignment.pitchDegrees = 0; - config->boardAlignment.yawDegrees = 0; + boardAlignmentMutable()->rollDegrees = 180; + boardAlignmentMutable()->pitchDegrees = 0; + boardAlignmentMutable()->yawDegrees = 0; #endif } #endif diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c index 1a86943511..ee9abc666a 100755 --- a/src/main/target/MOTOLAB/config.c +++ b/src/main/target/MOTOLAB/config.c @@ -21,12 +21,17 @@ #include #ifdef TARGET_CONFIG -#include "config/config_master.h" + +#include "fc/config.h" + +#include "flight/pid.h" + +#include "sensors/gyro.h" // Motolab target supports 2 different type of boards Tornado / Cyclone. -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->gyroConfig.gyro_sync_denom = 4; - config->pidConfig.pid_process_denom = 1; + gyroConfigMutable()->gyro_sync_denom = 4; + pidConfigMutable()->pid_process_denom = 1; } #endif diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 56656f9d87..2d959ede47 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -21,12 +21,12 @@ #include #ifdef TARGET_CONFIG + #include "common/axis.h" #include "common/maths.h" -#include "drivers/sensor.h" -#include "drivers/compass.h" - +#include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "flight/failsafe.h" @@ -35,55 +35,54 @@ #include "rx/rx.h" -#include "sensors/sensors.h" +#include "sensors/battery.h" #include "sensors/compass.h" +#include "sensors/gyro.h" -#include "config/config_profile.h" -#include "config/config_master.h" // alternative defaults settings for MULTIFLITEPICO targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->compassConfig.mag_hardware = MAG_NONE; // disabled by default + compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default - config->batteryConfig.vbatscale = 100; - config->batteryConfig.vbatresdivval = 15; - config->batteryConfig.vbatresdivmultiplier = 4; - config->batteryConfig.vbatmaxcellvoltage = 44; - config->batteryConfig.vbatmincellvoltage = 32; - config->batteryConfig.vbatwarningcellvoltage = 33; + batteryConfigMutable()->vbatscale = 100; + batteryConfigMutable()->vbatresdivval = 15; + batteryConfigMutable()->vbatresdivmultiplier = 4; + batteryConfigMutable()->vbatmaxcellvoltage = 44; + batteryConfigMutable()->vbatmincellvoltage = 32; + batteryConfigMutable()->vbatwarningcellvoltage = 33; - config->rxConfig.spektrum_sat_bind = 5; - config->rxConfig.spektrum_sat_bind_autoreset = 1; + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; - config->rcControlsConfig.yaw_deadband = 2; - config->rcControlsConfig.deadband = 2; + rcControlsConfigMutable()->yaw_deadband = 2; + rcControlsConfigMutable()->deadband = 2; - config->modeActivationProfile.modeActivationConditions[0].modeId = BOXANGLE; - config->modeActivationProfile.modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; - config->modeActivationProfile.modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(900); - config->modeActivationProfile.modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1400); - config->modeActivationProfile.modeActivationConditions[1].modeId = BOXHORIZON; - config->modeActivationProfile.modeActivationConditions[1].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; - config->modeActivationProfile.modeActivationConditions[1].range.startStep = CHANNEL_VALUE_TO_STEP(1425); - config->modeActivationProfile.modeActivationConditions[1].range.endStep = CHANNEL_VALUE_TO_STEP(1575); + modeActivationConditionsMutable(0)->modeId = BOXANGLE; + modeActivationConditionsMutable(0)->auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1400); + modeActivationConditionsMutable(1)->modeId = BOXHORIZON; + modeActivationConditionsMutable(1)->auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1425); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(1575); - config->failsafeConfig.failsafe_delay = 2; - config->failsafeConfig.failsafe_off_delay = 0; + failsafeConfigMutable()->failsafe_delay = 2; + failsafeConfigMutable()->failsafe_off_delay = 0; - config->motorConfig.dev.motorPwmRate = 17000; + motorConfigMutable()->dev.motorPwmRate = 17000; - config->gyroConfig.gyro_sync_denom = 4; - config->pidConfig.pid_process_denom = 1; + gyroConfigMutable()->gyro_sync_denom = 4; + pidConfigMutable()->pid_process_denom = 1; - config->profile[0].pidProfile.P8[ROLL] = 70; - config->profile[0].pidProfile.I8[ROLL] = 62; - config->profile[0].pidProfile.D8[ROLL] = 19; - config->profile[0].pidProfile.P8[PITCH] = 70; - config->profile[0].pidProfile.I8[PITCH] = 62; - config->profile[0].pidProfile.D8[PITCH] = 19; + pidProfilesMutable(0)->P8[ROLL] = 70; + pidProfilesMutable(0)->I8[ROLL] = 62; + pidProfilesMutable(0)->D8[ROLL] = 19; + pidProfilesMutable(0)->P8[PITCH] = 70; + pidProfilesMutable(0)->I8[PITCH] = 62; + pidProfilesMutable(0)->D8[PITCH] = 19; - config->controlRateProfile[0].rcRate8 = 70; - config->profile[0].pidProfile.I8[PIDLEVEL] = 40; + controlRateProfilesMutable(0)->rcRate8 = 70; + pidProfilesMutable(0)->I8[PIDLEVEL] = 40; } #endif diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 25e1b7b1e6..1384a1b4d6 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -21,10 +21,11 @@ #include #ifdef TARGET_CONFIG + +#include "common/axis.h" #include "common/utils.h" -#include "drivers/io.h" - +#include "fc/config.h" #include "fc/rc_controls.h" #include "fc/controlrate_profile.h" @@ -34,65 +35,54 @@ #include "rx/rx.h" -#include "config/config_profile.h" -#include "config/config_master.h" +#include "sensors/acceleration.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" #include "hardware_revision.h" -#ifdef USE_PARAMETER_GROUPS void targetConfiguration(void) { - -} -void targetValidateConfiguration(void) -{ - -} -#else -void targetConfiguration(master_t *config) -{ - UNUSED(config); - #ifdef BEEBRAIN // alternative defaults settings for Beebrain target - config->motorConfig.dev.motorPwmRate = 4000; - config->failsafeConfig.failsafe_delay = 2; - config->failsafeConfig.failsafe_off_delay = 0; + motorConfigMutable()->dev.motorPwmRate = 4000; + failsafeConfigMutable()->failsafe_delay = 2; + failsafeConfigMutable()->failsafe_off_delay = 0; - config->motorConfig.minthrottle = 1049; + motorConfigMutable()->minthrottle = 1049; - config->gyroConfig.gyro_lpf = GYRO_LPF_188HZ; - config->gyroConfig.gyro_soft_lpf_hz = 100; - config->gyroConfig.gyro_soft_notch_hz_1 = 0; - config->gyroConfig.gyro_soft_notch_hz_2 = 0; + gyroConfigMutable()->gyro_lpf = GYRO_LPF_188HZ; + gyroConfigMutable()->gyro_soft_lpf_hz = 100; + gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { - config->rxConfig.channelRanges[channel].min = 1180; - config->rxConfig.channelRanges[channel].max = 1860; + rxConfigMutable()->channelRanges[channel].min = 1180; + rxConfigMutable()->channelRanges[channel].max = 1860; }*/ for (int profileId = 0; profileId < 2; profileId++) { - config->profile[profileId].pidProfile.P8[ROLL] = 60; - config->profile[profileId].pidProfile.I8[ROLL] = 70; - config->profile[profileId].pidProfile.D8[ROLL] = 17; - config->profile[profileId].pidProfile.P8[PITCH] = 80; - config->profile[profileId].pidProfile.I8[PITCH] = 90; - config->profile[profileId].pidProfile.D8[PITCH] = 18; - config->profile[profileId].pidProfile.P8[YAW] = 200; - config->profile[profileId].pidProfile.I8[YAW] = 45; - config->profile[profileId].pidProfile.P8[PIDLEVEL] = 30; - config->profile[profileId].pidProfile.D8[PIDLEVEL] = 30; + pidProfilesMutable(0)->P8[ROLL] = 60; + pidProfilesMutable(0)->I8[ROLL] = 70; + pidProfilesMutable(0)->D8[ROLL] = 17; + pidProfilesMutable(0)->P8[PITCH] = 80; + pidProfilesMutable(0)->I8[PITCH] = 90; + pidProfilesMutable(0)->D8[PITCH] = 18; + pidProfilesMutable(0)->P8[YAW] = 200; + pidProfilesMutable(0)->I8[YAW] = 45; + pidProfilesMutable(0)->P8[PIDLEVEL] = 30; + pidProfilesMutable(0)->D8[PIDLEVEL] = 30; 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; - config->controlRateProfile[rateProfileId].rates[ROLL] = 77; - config->controlRateProfile[rateProfileId].rates[PITCH] = 77; - config->controlRateProfile[rateProfileId].rates[YAW] = 80; + controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; + controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 110; + controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; + controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 77; + controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 77; + controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 80; - config->profile[profileId].pidProfile.dtermSetpointWeight = 200; - config->profile[profileId].pidProfile.setpointRelaxRatio = 50; + pidProfilesMutable(0)->dtermSetpointWeight = 200; + pidProfilesMutable(0)->setpointRelaxRatio = 50; } } #endif @@ -100,27 +90,26 @@ void targetConfiguration(master_t *config) #if !defined(AFROMINI) && !defined(BEEBRAIN) if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. - config->beeperDevConfig.isOpenDrain = false; - config->beeperDevConfig.isInverted = true; + beeperDevConfigMutable()->isOpenDrain = false; + beeperDevConfigMutable()->isInverted = true; } else { - config->beeperDevConfig.isOpenDrain = true; - config->beeperDevConfig.isInverted = false; - config->flashConfig.csTag = IO_TAG_NONE; + beeperDevConfigMutable()->isOpenDrain = true; + beeperDevConfigMutable()->isInverted = false; + flashConfigMutable()->csTag = IO_TAG_NONE; } #endif #ifdef MAG_INT_EXTI if (hardwareRevision < NAZE32_REV5) { - config->compassConfig.interruptTag = IO_TAG(PB12); + compassConfigMutable()->interruptTag = IO_TAG(PB12); } #endif } -void targetValidateConfiguration(master_t *config) +void targetValidateConfiguration(void) { - if (hardwareRevision < NAZE32_REV5 && config->accelerometerConfig.acc_hardware == ACC_ADXL345) { - config->accelerometerConfig.acc_hardware = ACC_NONE; + if (hardwareRevision < NAZE32_REV5 && accelerometerConfig()->acc_hardware == ACC_ADXL345) { + accelerometerConfigMutable()->acc_hardware = ACC_NONE; } } #endif -#endif // USE_PARAMETER_GROUPS diff --git a/src/main/target/PIKOBLX/config.c b/src/main/target/PIKOBLX/config.c index 8a18f7c4c2..b901f5bfb9 100644 --- a/src/main/target/PIKOBLX/config.c +++ b/src/main/target/PIKOBLX/config.c @@ -20,9 +20,13 @@ #include #ifdef TARGET_CONFIG -#include "config/config_master.h" -void targetConfiguration(master_t *config) { - config->batteryConfig.currentMeterScale = 125; +#include "fc/config.h" + +#include "sensors/battery.h" + +void targetConfiguration(void) +{ + batteryConfigMutable()->currentMeterScale = 125; } #endif diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c index d4c63c0c6f..663e8cc01a 100755 --- a/src/main/target/RACEBASE/config.c +++ b/src/main/target/RACEBASE/config.c @@ -21,15 +21,17 @@ #include #ifdef TARGET_CONFIG + +#include "fc/config.h" + #include "rx/rx.h" #include "config/config_master.h" -// alternative defaults settings for COLIBRI RACE targets -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->rxConfig.sbus_inversion = 0; - config->rxConfig.rssi_scale = 19; - config->rxConfig.serialrx_provider = SERIALRX_SBUS; + rxConfigMutable()->sbus_inversion = 0; + rxConfigMutable()->rssi_scale = 19; + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; } #endif diff --git a/src/main/target/RG_SSD_F3/config.c b/src/main/target/RG_SSD_F3/config.c index 56fd96c4cc..93039ea940 100644 --- a/src/main/target/RG_SSD_F3/config.c +++ b/src/main/target/RG_SSD_F3/config.c @@ -20,10 +20,14 @@ #include #ifdef TARGET_CONFIG -#include "config/config_master.h" -void targetConfiguration(master_t *config) { - config->batteryConfig.vbatmaxcellvoltage = 45; - config->batteryConfig.vbatscale = VBAT_SCALE_DEFAULT; +#include "fc/config.h" + +#include "sensors/battery.h" + +void targetConfiguration(void) +{ + batteryConfigMutable()->vbatmaxcellvoltage = 45; + batteryConfigMutable()->vbatscale = VBAT_SCALE_DEFAULT; } #endif diff --git a/src/main/target/SINGULARITY/target.mk b/src/main/target/SINGULARITY/target.mk index 13c531b9c7..fd4eb78419 100644 --- a/src/main/target/SINGULARITY/target.mk +++ b/src/main/target/SINGULARITY/target.mk @@ -4,6 +4,4 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ - drivers/vtx_rtc6705.c \ - io/vtx.c - + drivers/vtx_rtc6705.c diff --git a/src/main/target/SPRACINGF3EVO/config.c b/src/main/target/SPRACINGF3EVO/config.c index 68806ec584..1da3d86eec 100644 --- a/src/main/target/SPRACINGF3EVO/config.c +++ b/src/main/target/SPRACINGF3EVO/config.c @@ -18,11 +18,12 @@ #include #ifdef TARGET_CONFIG -#include "config/config_master.h" -void targetConfiguration(master_t *config) +#include "fc/config.h" + +void targetConfiguration(void) { // Temporary workaround: Disable SDCard DMA by default since it causes errors on this target - config->sdcardConfig.useDma = false; + sdcardConfigMutable()->useDma = false; } #endif diff --git a/src/main/target/TINYFISH/config.c b/src/main/target/TINYFISH/config.c index 009ce10b3f..fe04790e29 100644 --- a/src/main/target/TINYFISH/config.c +++ b/src/main/target/TINYFISH/config.c @@ -20,39 +20,42 @@ #include #ifdef TARGET_CONFIG -#include "drivers/io.h" - -#include "drivers/timer.h" #include "drivers/dma.h" +#include "drivers/io.h" +#include "drivers/timer.h" -#include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "fc/config.h" + #include "io/serial.h" -#include "target.h" +#include "rx/rx.h" + +#include "sensors/battery.h" + +#include "telemetry/telemetry.h" #define TARGET_CPU_VOLTAGE 3.0 // set default settings to match our target -void targetConfiguration(master_t *config) +void targetConfiguration(void) { - config->batteryConfig.currentMeterOffset = 0; + batteryConfigMutable()->currentMeterOffset = 0; // we use an ina139, RL=0.005, Rs=30000 // V/A = (0.005 * 0.001 * 30000) * I // rescale to 1/10th mV / A -> * 1000 * 10 // we use 3.0V as cpu and adc voltage -> rescale by 3.0/3.3 - config->batteryConfig.currentMeterScale = (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3); + batteryConfigMutable()->currentMeterScale = (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3); // we use the same uart for frsky telemetry and SBUS, both non inverted - int index = findSerialPortIndexByIdentifier(SBUS_TELEMETRY_UART); - config->serialConfig.portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; + const int index = findSerialPortIndexByIdentifier(SBUS_TELEMETRY_UART); + serialConfigMutable()->portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - config->rxConfig.serialrx_provider = SERIALRX_SBUS; - config->telemetryConfig.telemetry_inversion = 0; - config->rxConfig.sbus_inversion = 0; + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + rxConfigMutable()->sbus_inversion = 0; + telemetryConfigMutable()->telemetry_inversion = 0; - intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->featureConfig.enabledFeatures); + featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT); } #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index c284d0ad03..9c1f2173b9 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,7 +17,7 @@ #pragma once -//#define USE_PARAMETER_GROUPS +#define USE_PARAMETER_GROUPS // type conversion warnings. // -Wconversion can be turned on to enable the process of eliminating these warnings //#pragma GCC diagnostic warning "-Wconversion" diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 04777a245e..e154090962 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -46,10 +46,11 @@ #include "io/serial.h" -#include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/barometer.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" #include "scheduler/scheduler.h"