diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49ceee0848..45b3bc1dbc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -24,6 +24,9 @@ #ifdef BLACKBOX +#include "blackbox.h" +#include "blackbox_io.h" + #include "build/debug.h" #include "build/version.h" @@ -31,8 +34,10 @@ #include "common/encoding.h" #include "common/utils.h" -#include "blackbox.h" -#include "blackbox_io.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/sensor.h" #include "drivers/compass.h" @@ -43,18 +48,22 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" #include "flight/pid.h" #include "io/beeper.h" +#include "io/serial.h" -#include "sensors/sensors.h" +#include "rx/rx.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" #include "sensors/compass.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" #include "sensors/sonar.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define SLOW_FRAME_INTERVAL 4096 @@ -761,16 +770,16 @@ void validateBlackboxConfig() if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0 || blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) { - blackboxConfig()->rate_num = 1; - blackboxConfig()->rate_denom = 1; + blackboxConfigMutable()->rate_num = 1; + blackboxConfigMutable()->rate_denom = 1; } else { /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat * itself more frequently) */ div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom); - blackboxConfig()->rate_num /= div; - blackboxConfig()->rate_denom /= div; + blackboxConfigMutable()->rate_num /= div; + blackboxConfigMutable()->rate_denom /= div; } // If we've chosen an unsupported device, change the device to serial @@ -786,7 +795,7 @@ void validateBlackboxConfig() break; default: - blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL; + blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL; } } @@ -820,7 +829,7 @@ void startBlackbox(void) */ blackboxBuildConditionCache(); - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX); + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); blackboxIteration = 0; blackboxPFrameIndex = 0; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 388ca2f75d..d085f531c4 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -21,6 +21,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + typedef struct blackboxConfig_s { uint8_t rate_num; uint8_t rate_denom; @@ -28,6 +30,8 @@ typedef struct blackboxConfig_s { uint8_t on_motor_test; } blackboxConfig_t; +PG_DECLARE(blackboxConfig_t, blackboxConfig); + void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void initBlackbox(void); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 307ada96c1..356a98e5ee 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -8,6 +8,7 @@ #ifdef BLACKBOX +#include "blackbox.h" #include "blackbox_io.h" #include "build/version.h" @@ -16,6 +17,10 @@ #include "common/encoding.h" #include "common/printf.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" #include "fc/rc_controls.h" @@ -23,12 +28,10 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "io/serial.h" #include "msp/msp_serial.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #define BLACKBOX_SERIAL_PORT_MODE MODE_TX // How many bytes can we transmit per loop iteration when writing headers? diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 33b01b35d1..ae791cd1fc 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -44,19 +44,24 @@ #include "drivers/system.h" +// For rcData, stopAllMotors, stopPwmAllMotors +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + // For 'ARM' related #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -// For rcData, stopAllMotors, stopPwmAllMotors -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" +#include "flight/mixer.h" -// For VISIBLE* (Actually, included by config_master.h) +// For VISIBLE* #include "io/osd.h" +#include "rx/rx.h" + // DisplayPort management #ifndef CMS_MAX_DEVICE diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index d1ea564a88..b109e64562 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -27,10 +27,10 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" #include "cms/cms.h" @@ -40,8 +40,9 @@ #include "common/utils.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 5899c69f87..7dc533ddd8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -44,8 +44,9 @@ #include "flight/pid.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" // // PID diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index 45f9863f8c..e92f42c99a 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -22,15 +22,16 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "cms/cms.h" #include "cms/cms_types.h" diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 38d1e07160..05e1e54905 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -28,14 +28,15 @@ #include "drivers/system.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #include "cms/cms.h" #include "cms/cms_types.h" #include "cms/cms_menu_ledstrip.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + // // Misc // diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 2d8b88f0ac..28be0d4743 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -21,6 +21,8 @@ #include "platform.h" +#if defined(OSD) && defined(CMS) + #include "build/version.h" #include "cms/cms.h" @@ -28,10 +30,9 @@ #include "cms/cms_menu_osd.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" - -#if defined(OSD) && defined(CMS) +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5}; OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50}; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 066fe8eeca..a07a776b1a 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -21,6 +21,10 @@ #include "platform.h" +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + #include "build/version.h" #include "cms/cms.h" @@ -30,12 +34,9 @@ #include "common/utils.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" - -#ifdef CMS - -#if defined(VTX) || defined(USE_RTC6705) +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" static bool featureRead = false; static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b6ef8cd90f..584c18876e 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -42,10 +42,10 @@ #include "flight/servos.h" #include "flight/imu.h" #include "flight/navigation.h" +#include "flight/pid.h" #include "io/serial.h" #include "io/gimbal.h" -#include "io/motors.h" #include "io/servos.h" #include "io/gps.h" #include "io/osd.h" @@ -64,6 +64,7 @@ #include "sensors/battery.h" #include "sensors/compass.h" +#ifndef USE_PARAMETER_GROUPS #define motorConfig(x) (&masterConfig.motorConfig) #define flight3DConfig(x) (&masterConfig.flight3DConfig) #define servoConfig(x) (&masterConfig.servoConfig) @@ -105,11 +106,71 @@ #define modeActivationProfile(x) (&masterConfig.modeActivationProfile) #define servoProfile(x) (&masterConfig.servoProfile) #define customMotorMixer(i) (&masterConfig.customMotorMixer[i]) -#define customServoMixer(i) (&masterConfig.customServoMixer[i]) +#define customServoMixers(i) (&masterConfig.customServoMixer[i]) #define displayPortProfileMsp(x) (&masterConfig.displayPortProfileMsp) #define displayPortProfileMax7456(x) (&masterConfig.displayPortProfileMax7456) #define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled) + +#define motorConfigMutable(x) (&masterConfig.motorConfig) +#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig) +#define servoConfigMutable(x) (&masterConfig.servoConfig) +#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig) +#define gimbalConfigMutable(x) (&masterConfig.gimbalConfig) +#define boardAlignmentMutable(x) (&masterConfig.boardAlignment) +#define imuConfigMutable(x) (&masterConfig.imuConfig) +#define gyroConfigMutable(x) (&masterConfig.gyroConfig) +#define compassConfigMutable(x) (&masterConfig.compassConfig) +#define accelerometerConfigMutable(x) (&masterConfig.accelerometerConfig) +#define barometerConfigMutable(x) (&masterConfig.barometerConfig) +#define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig) +#define batteryConfigMutable(x) (&masterConfig.batteryConfig) +#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig) +#define gpsProfileMutable(x) (&masterConfig.gpsProfile) +#define gpsConfigMutable(x) (&masterConfig.gpsConfig) +#define rxConfigMutable(x) (&masterConfig.rxConfig) +#define armingConfigMutable(x) (&masterConfig.armingConfig) +#define mixerConfigMutable(x) (&masterConfig.mixerConfig) +#define airplaneConfigMutable(x) (&masterConfig.airplaneConfig) +#define failsafeConfigMutable(x) (&masterConfig.failsafeConfig) +#define serialConfigMutable(x) (&masterConfig.serialConfig) +#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig) +#define ibusTelemetryConfigMutable(x) (&masterConfig.telemetryConfig) +#define ppmConfigMutable(x) (&masterConfig.ppmConfig) +#define pwmConfigMutable(x) (&masterConfig.pwmConfig) +#define adcConfigMutable(x) (&masterConfig.adcConfig) +#define beeperConfigMutable(x) (&masterConfig.beeperConfig) +#define sonarConfigMutable(x) (&masterConfig.sonarConfig) +#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig) +#define statusLedConfigMutable(x) (&masterConfig.statusLedConfig) +#define osdProfileMutable(x) (&masterConfig.osdProfile) +#define vcdProfileMutable(x) (&masterConfig.vcdProfile) +#define sdcardConfigMutable(x) (&masterConfig.sdcardConfig) +#define blackboxConfigMutable(x) (&masterConfig.blackboxConfig) +#define flashConfigMutable(x) (&masterConfig.flashConfig) +#define pidConfigMutable(x) (&masterConfig.pidConfig) +#define adjustmentProfileMutable(x) (&masterConfig.adjustmentProfile) +#define modeActivationProfileMutable(x) (&masterConfig.modeActivationProfile) +#define servoProfileMutable(x) (&masterConfig.servoProfile) +#define customMotorMixerMutable(i) (&masterConfig.customMotorMixer[i]) +#define customServoMixersMutable(i) (&masterConfig.customServoMixer[i]) +#define displayPortProfileMspMutable(x) (&masterConfig.displayPortProfileMsp) +#define displayPortProfileMax7456Mutable(x) (&masterConfig.displayPortProfileMax7456) +#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled) + +#define servoParams(x) (&servoProfile()->servoConf[i]) +#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x]) +#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x]) +#define osdConfig(x) (&masterConfig.osdProfile) +#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) + +#define servoParamsMutable(x) (&servoProfile()->servoConf[i]) +#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[i]) +#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x]) +#define osdConfigMutable(x) (&masterConfig.osdProfile) +#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) +#endif + // System-wide typedef struct master_s { uint8_t version; diff --git a/src/main/config/feature.c b/src/main/config/feature.c index 44b3c12d8b..508e5abeb1 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -21,8 +21,9 @@ #include "platform.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" static uint32_t activeFeaturesLatch = 0; diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 11f6ff355f..2eb75f8a42 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -17,6 +17,14 @@ #pragma once +#include "config/parameter_group.h" + +typedef struct featureConfig_s { + uint32_t enabledFeatures; +} featureConfig_t; + +PG_DECLARE(featureConfig_t, featureConfig); + void latchActiveFeatures(void); bool featureConfigured(uint32_t mask); bool feature(uint32_t mask); @@ -27,4 +35,4 @@ uint32_t featureMask(void); void intFeatureClearAll(uint32_t *features); void intFeatureSet(uint32_t mask, uint32_t *features); -void intFeatureClear(uint32_t mask, uint32_t *features); \ No newline at end of file +void intFeatureClear(uint32_t mask, uint32_t *features); diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index 2711639cc1..58dbe51e1a 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -19,6 +19,8 @@ #include #include +#include "platform.h" + #include "parameter_group.h" #include "common/maths.h" @@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex) } } } - diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 2533b50158..8ee75f9968 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -100,6 +100,7 @@ extern const uint8_t __pg_resetdata_end[]; } while(0) \ /**/ +#ifdef USE_PARAMETER_GROUPS // Declare system config #define PG_DECLARE(_type, _name) \ extern _type _name ## _System; \ @@ -125,6 +126,20 @@ extern const uint8_t __pg_resetdata_end[]; struct _dummy \ /**/ +#else + +#define PG_DECLARE(_type, _name) \ + extern _type _name ## _System + +#define PG_DECLARE_ARRAY(_type, _size, _name) \ + extern _type _name ## _SystemArray[_size] + +// Declare profile config +#define PG_DECLARE_PROFILE(_type, _name) \ + extern _type *_name ## _ProfileCurrent + +#endif // USE_PARAMETER_GROUPS + // Register system config #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ _type _name ## _System; \ diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index fbef2e84d5..44ad13b037 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -15,6 +15,10 @@ * along with Cleanflight. If not, see . */ +#ifndef USE_PARAMETER_GROUPS +#include "config/config_master.h" +#endif + // FC configuration #define PG_FAILSAFE_CONFIG 1 // struct OK #define PG_BOARD_ALIGNMENT 2 // struct OK diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c8982b3408..a6af785ec1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -32,6 +32,8 @@ uint8_t cliMode = 0; #ifdef USE_CLI +#include "blackbox/blackbox.h" + #include "build/build_config.h" #include "build/debug.h" #include "build/version.h" @@ -45,9 +47,10 @@ uint8_t cliMode = 0; #include "common/typeconversion.h" #include "config/config_eeprom.h" -#include "config/config_master.h" #include "config/config_profile.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -81,6 +84,7 @@ uint8_t cliMode = 0; #include "flight/mixer.h" #include "flight/navigation.h" #include "flight/pid.h" +#include "flight/servos.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -823,6 +827,71 @@ static const clivalue_t valueTable[] = { }; #endif +#ifdef USE_PARAMETER_GROUPS +static featureConfig_t featureConfigCopy; +static gyroConfig_t gyroConfigCopy; +static accelerometerConfig_t accelerometerConfigCopy; +#ifdef MAG +static compassConfig_t compassConfigCopy; +#endif +#ifdef BARO +static barometerConfig_t barometerConfigCopy; +#endif +#ifdef PITOT +static pitotmeterConfig_t pitotmeterConfigCopy; +#endif +static featureConfig_t featureConfigCopy; +static rxConfig_t rxConfigCopy; +#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 failsafeConfig_t failsafeConfigCopy; +static boardAlignment_t boardAlignmentCopy; +#ifdef USE_SERVOS +static servoConfig_t servoConfigCopy; +static gimbalConfig_t gimbalConfigCopy; +static servoMixer_t customServoMixersCopy[MAX_SERVO_RULES]; +static servoParam_t servoParamsCopy[MAX_SUPPORTED_SERVOS]; +#endif +static batteryConfig_t batteryConfigCopy; +static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS]; +static mixerConfig_t mixerConfigCopy; +static flight3DConfig_t flight3DConfigCopy; +static serialConfig_t serialConfigCopy; +static imuConfig_t imuConfigCopy; +static armingConfig_t armingConfigCopy; +static rcControlsConfig_t rcControlsConfigCopy; +#ifdef GPS +static gpsConfig_t gpsConfigCopy; +#endif +#ifdef NAV +static positionEstimationConfig_t positionEstimationConfigCopy; +static navConfig_t navConfigCopy; +#endif +#ifdef TELEMETRY +static telemetryConfig_t telemetryConfigCopy; +#endif +static modeActivationCondition_t modeActivationConditionsCopy[MAX_MODE_ACTIVATION_CONDITION_COUNT]; +static adjustmentRange_t adjustmentRangesCopy[MAX_ADJUSTMENT_RANGE_COUNT]; +#ifdef LED_STRIP +static ledStripConfig_t ledStripConfigCopy; +#endif +#ifdef OSD +static osdConfig_t osdConfigCopy; +#endif +static systemConfig_t systemConfigCopy; +#ifdef BEEPER +static beeperConfig_t beeperConfigCopy; +#endif +static controlRateConfig_t controlRateProfilesCopy[MAX_CONTROL_RATE_PROFILE_COUNT]; +static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; +static modeActivationOperatorConfig_t modeActivationOperatorConfigCopy; +static beeperConfig_t beeperConfigCopy; +#endif // USE_PARAMETER_GROUPS + static void cliPrint(const char *str) { while (*str) { @@ -994,6 +1063,180 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn static cliCurrentAndDefaultConfig_t ret; switch (pgn) { + case PG_GYRO_CONFIG: + ret.currentConfig = &gyroConfigCopy; + ret.defaultConfig = gyroConfig(); + break; + case PG_ACCELEROMETER_CONFIG: + ret.currentConfig = &accelerometerConfigCopy; + ret.defaultConfig = accelerometerConfig(); + break; +#ifdef MAG + case PG_COMPASS_CONFIG: + ret.currentConfig = &compassConfigCopy; + ret.defaultConfig = compassConfig(); + break; +#endif +#ifdef BARO + case PG_BAROMETER_CONFIG: + ret.currentConfig = &barometerConfigCopy; + ret.defaultConfig = barometerConfig(); + break; +#endif +#ifdef PITOT + case PG_PITOTMETER_CONFIG: + ret.currentConfig = &pitotmeterConfigCopy; + ret.defaultConfig = pitotmeterConfig(); + break; +#endif + case PG_FEATURE_CONFIG: + ret.currentConfig = &featureConfigCopy; + ret.defaultConfig = featureConfig(); + break; + case PG_RX_CONFIG: + ret.currentConfig = &rxConfigCopy; + ret.defaultConfig = rxConfig(); + break; +#ifdef BLACKBOX + case PG_BLACKBOX_CONFIG: + ret.currentConfig = &blackboxConfigCopy; + ret.defaultConfig = blackboxConfig(); + break; +#endif + case PG_MOTOR_CONFIG: + ret.currentConfig = &motorConfigCopy; + ret.defaultConfig = motorConfig(); + break; + case PG_FAILSAFE_CONFIG: + ret.currentConfig = &failsafeConfigCopy; + ret.defaultConfig = failsafeConfig(); + break; + case PG_BOARD_ALIGNMENT: + ret.currentConfig = &boardAlignmentCopy; + ret.defaultConfig = boardAlignment(); + break; + case PG_MIXER_CONFIG: + ret.currentConfig = &mixerConfigCopy; + ret.defaultConfig = mixerConfig(); + break; + case PG_MOTOR_3D_CONFIG: + ret.currentConfig = &flight3DConfigCopy; + ret.defaultConfig = flight3DConfig(); + break; +#ifdef USE_SERVOS + case PG_SERVO_CONFIG: + ret.currentConfig = &servoConfigCopy; + ret.defaultConfig = servoConfig(); + break; + case PG_GIMBAL_CONFIG: + ret.currentConfig = &gimbalConfigCopy; + ret.defaultConfig = gimbalConfig(); + break; +#endif + case PG_BATTERY_CONFIG: + ret.currentConfig = &batteryConfigCopy; + ret.defaultConfig = batteryConfig(); + break; + case PG_SERIAL_CONFIG: + ret.currentConfig = &serialConfigCopy; + ret.defaultConfig = serialConfig(); + break; + case PG_IMU_CONFIG: + ret.currentConfig = &imuConfigCopy; + ret.defaultConfig = imuConfig(); + break; + case PG_RC_CONTROLS_CONFIG: + ret.currentConfig = &rcControlsConfigCopy; + ret.defaultConfig = rcControlsConfig(); + break; + case PG_ARMING_CONFIG: + ret.currentConfig = &armingConfigCopy; + ret.defaultConfig = armingConfig(); + break; +#ifdef GPS + case PG_GPS_CONFIG: + 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: + ret.currentConfig = &navConfigCopy; + ret.defaultConfig = navConfig(); + break; +#endif +#ifdef TELEMETRY + case PG_TELEMETRY_CONFIG: + ret.currentConfig = &telemetryConfigCopy; + ret.defaultConfig = telemetryConfig(); + break; +#endif +#ifdef LED_STRIP + case PG_LED_STRIP_CONFIG: + ret.currentConfig = &ledStripConfigCopy; + ret.defaultConfig = ledStripConfig(); + break; +#endif +#ifdef OSD + case PG_OSD_CONFIG: + ret.currentConfig = &osdConfigCopy; + ret.defaultConfig = osdConfig(); + break; +#endif + case PG_SYSTEM_CONFIG: + ret.currentConfig = &systemConfigCopy; + ret.defaultConfig = systemConfig(); + break; + case PG_MODE_ACTIVATION_OPERATOR_CONFIG: + ret.currentConfig = &modeActivationOperatorConfigCopy; + ret.defaultConfig = modeActivationOperatorConfig(); + break; + case PG_CONTROL_RATE_PROFILES: + ret.currentConfig = &controlRateProfilesCopy[0]; + ret.defaultConfig = controlRateProfiles(0); + break; + case PG_PID_PROFILE: + ret.currentConfig = &pidProfileCopy[getConfigProfile()]; + ret.defaultConfig = pidProfile(); + break; + case PG_RX_FAILSAFE_CHANNEL_CONFIG: + ret.currentConfig = &rxFailsafeChannelConfigsCopy[0]; + ret.defaultConfig = rxFailsafeChannelConfigs(0); + break; + case PG_RX_CHANNEL_RANGE_CONFIG: + ret.currentConfig = &rxChannelRangeConfigsCopy[0]; + ret.defaultConfig = rxChannelRangeConfigs(0); + break; +#ifdef USE_SERVOS + case PG_SERVO_MIXER: + ret.currentConfig = &customServoMixersCopy[0]; + ret.defaultConfig = customServoMixers(0); + break; + case PG_SERVO_PARAMS: + ret.currentConfig = &servoParamsCopy[0]; + ret.defaultConfig = servoParams(0); + break; +#endif + case PG_MOTOR_MIXER: + ret.currentConfig = &customMotorMixerCopy[0]; + ret.defaultConfig = customMotorMixer(0); + break; + case PG_MODE_ACTIVATION_PROFILE: + ret.currentConfig = &modeActivationConditionsCopy[0]; + ret.defaultConfig = modeActivationConditions(0); + break; + case PG_ADJUSTMENT_RANGE_CONFIG: + ret.currentConfig = &adjustmentRangesCopy[0]; + ret.defaultConfig = adjustmentRanges(0); + break; + case PG_BEEPER_CONFIG: + ret.currentConfig = &beeperConfigCopy; + ret.defaultConfig = beeperConfig(); + break; default: ret.currentConfig = NULL; ret.defaultConfig = NULL; @@ -1810,7 +2053,7 @@ static void cliMotorMix(char *cmdline) } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - customMotorMixer(i)->throttle = 0.0f; + customMotorMixerMutable(i)->throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = nextArg(cmdline); if (ptr) { @@ -1821,7 +2064,7 @@ static void cliMotorMix(char *cmdline) break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { - mixerLoadMix(i, masterConfig.customMotorMixer); + mixerLoadMix(i, customMotorMixerMutable(0)); cliPrintf("Loaded %s\r\n", mixerNames[i]); cliMotorMix(""); break; @@ -1834,22 +2077,22 @@ static void cliMotorMix(char *cmdline) if (i < MAX_SUPPORTED_MOTORS) { ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->throttle = fastA2F(ptr); + customMotorMixerMutable(i)->throttle = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->roll = fastA2F(ptr); + customMotorMixerMutable(i)->roll = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->pitch = fastA2F(ptr); + customMotorMixerMutable(i)->pitch = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->yaw = fastA2F(ptr); + customMotorMixerMutable(i)->yaw = fastA2F(ptr); check++; } if (check != 4) { @@ -2200,7 +2443,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) { const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - servoMixer_t customServoMixer = *customServoMixer(i); + const servoMixer_t customServoMixer = *customServoMixers(i); if (customServoMixer.rate == 0) { break; } @@ -2361,13 +2604,13 @@ static void cliServoMix(char *cmdline) args[MIN] >= 0 && args[MIN] <= 100 && args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - customServoMixer(i)->targetChannel = args[TARGET]; - customServoMixer(i)->inputSource = args[INPUT]; - customServoMixer(i)->rate = args[RATE]; - customServoMixer(i)->speed = args[SPEED]; - customServoMixer(i)->min = args[MIN]; - customServoMixer(i)->max = args[MAX]; - customServoMixer(i)->box = args[BOX]; + customServoMixersMutable(i)->targetChannel = args[TARGET]; + customServoMixersMutable(i)->inputSource = args[INPUT]; + customServoMixersMutable(i)->rate = args[RATE]; + customServoMixersMutable(i)->speed = args[SPEED]; + customServoMixersMutable(i)->min = args[MIN]; + customServoMixersMutable(i)->max = args[MAX]; + customServoMixersMutable(i)->box = args[BOX]; cliServoMix(""); } else { cliShowParseError(); @@ -3023,7 +3266,7 @@ static void cliMixer(char *cmdline) return; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - mixerConfig()->mixerMode = i + 1; + mixerConfigMutable()->mixerMode = i + 1; break; } } @@ -3675,12 +3918,45 @@ static void cliResource(char *cmdline) #ifdef USE_PARAMETER_GROUPS static void backupConfigs(void) { - // make copies of configs to do differencing - + // make copies of configs to do differencing + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("BACKUP %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(&pidProfileCopy[0], reg->address, sizeof(pidProfile_t) * MAX_PROFILE_COUNT); } static void restoreConfigs(void) { + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("RESTORE %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(reg->address, &pidProfileCopy[0], sizeof(pidProfile_t) * MAX_PROFILE_COUNT); } #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8ee0519c81..06066752e0 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -951,7 +951,7 @@ void activateConfig(void) void validateAndFixConfig(void) { if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ - motorConfig()->mincommand = 1000; + motorConfigMutable()->mincommand = 1000; } if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { @@ -1064,18 +1064,18 @@ void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { - gyroConfig()->gyro_soft_notch_hz_1 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; } if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { - gyroConfig()->gyro_soft_notch_hz_2 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } float samplingTime = 0.000125f; if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { - pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - gyroConfig()->gyro_sync_denom = 1; - gyroConfig()->gyro_use_32khz = false; + pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + gyroConfigMutable()->gyro_sync_denom = 1; + gyroConfigMutable()->gyro_use_32khz = false; samplingTime = 0.001f; } @@ -1094,7 +1094,7 @@ void validateAndFixGyroConfig(void) } #if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfig()->gyro_isr_update = false; + gyroConfigMutable()->gyro_isr_update = false; #endif // check for looptime restrictions based on motor protocol. Motor times have safety margin @@ -1123,14 +1123,14 @@ void validateAndFixGyroConfig(void) } if(pidLooptime < motorUpdateRestriction) - pidConfig()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom); + pidConfigMutable()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom); // Prevent overriding the max rate of motors if(motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED)) { uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); if(motorConfig()->motorPwmRate > maxEscRate) - motorConfig()->motorPwmRate = maxEscRate; + motorConfigMutable()->motorPwmRate = maxEscRate; } } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 7a85b85384..3b69a1f189 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -61,6 +61,12 @@ typedef enum { FEATURE_ESC_SENSOR = 1 << 27, } features_e; +typedef struct systemConfig_s { + uint8_t debug_mode; +} systemConfig_t; + +//!!TODOPG_DECLARE(systemConfig_t, systemConfig); + void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); void beeperOffClear(uint32_t mask); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 94897ca306..eea39c5f2d 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -29,15 +29,21 @@ #include "common/utils.h" #include "common/filter.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/light_led.h" #include "drivers/system.h" #include "drivers/gyro_sync.h" -#include "sensors/sensors.h" -#include "sensors/boardalignment.h" #include "sensors/acceleration.h" -#include "sensors/gyro.h" +#include "sensors/barometer.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -60,15 +66,14 @@ #include "scheduler/scheduler.h" +#include "telemetry/telemetry.h" + #include "flight/mixer.h" #include "flight/servos.h" #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" // June 2013 V2.2-dev @@ -96,8 +101,8 @@ static bool armingCalibrationWasInitialised; void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { - accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; - accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; + accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; + accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; saveConfigAndNotify(); } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7d0dfbc86e..a5224e8508 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -28,6 +28,12 @@ #include "common/maths.h" #include "common/printf.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "cms/cms.h" #include "cms/cms_types.h" @@ -115,10 +121,6 @@ #include "flight/failsafe.h" #include "flight/navigation.h" -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 45587418d7..dbf8815a72 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -36,9 +36,11 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "drivers/system.h" #include "drivers/accgyro.h" #include "drivers/bus_i2c.h" #include "drivers/compass.h" @@ -673,25 +675,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SERVO_CONFIGURATIONS: for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - sbufWriteU16(dst, servoProfile()->servoConf[i].min); - sbufWriteU16(dst, servoProfile()->servoConf[i].max); - sbufWriteU16(dst, servoProfile()->servoConf[i].middle); - sbufWriteU8(dst, servoProfile()->servoConf[i].rate); - sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin); - sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax); - sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel); - sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources); + sbufWriteU16(dst, servoParams(i)->min); + sbufWriteU16(dst, servoParams(i)->max); + sbufWriteU16(dst, servoParams(i)->middle); + sbufWriteU8(dst, servoParams(i)->rate); + sbufWriteU8(dst, servoParams(i)->angleAtMin); + sbufWriteU8(dst, servoParams(i)->angleAtMax); + sbufWriteU8(dst, servoParams(i)->forwardFromChannel); + sbufWriteU32(dst, servoParams(i)->reversedSources); } break; case MSP_SERVO_MIX_RULES: for (int i = 0; i < MAX_SERVO_RULES; i++) { - sbufWriteU8(dst, customServoMixer(i)->targetChannel); - sbufWriteU8(dst, customServoMixer(i)->inputSource); - sbufWriteU8(dst, customServoMixer(i)->rate); - sbufWriteU8(dst, customServoMixer(i)->speed); - sbufWriteU8(dst, customServoMixer(i)->min); - sbufWriteU8(dst, customServoMixer(i)->max); - sbufWriteU8(dst, customServoMixer(i)->box); + sbufWriteU8(dst, customServoMixers(i)->targetChannel); + sbufWriteU8(dst, customServoMixers(i)->inputSource); + sbufWriteU8(dst, customServoMixers(i)->rate); + sbufWriteU8(dst, customServoMixers(i)->speed); + sbufWriteU8(dst, customServoMixers(i)->min); + sbufWriteU8(dst, customServoMixers(i)->max); + sbufWriteU8(dst, customServoMixers(i)->box); } break; #endif @@ -798,7 +800,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_ADJUSTMENT_RANGES: for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + const adjustmentRange_t *adjRange = adjustmentRanges(i); sbufWriteU8(dst, adjRange->adjustmentIndex); sbufWriteU8(dst, adjRange->auxChannelIndex); sbufWriteU8(dst, adjRange->range.startStep); @@ -965,8 +967,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_RXFAIL_CONFIG: for (int i = 0; i < rxRuntimeConfig.channelCount; i++) { - sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode); - sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step)); + sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode); + sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step)); } break; @@ -1012,7 +1014,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #ifdef LED_STRIP case MSP_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + const hsvColor_t *color = &ledStripConfig()->colors[i]; sbufWriteU16(dst, color->h); sbufWriteU8(dst, color->s); sbufWriteU8(dst, color->v); @@ -1021,7 +1023,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_LED_STRIP_CONFIG: for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; sbufWriteU32(dst, *ledConfig); } break; @@ -1089,13 +1091,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #else sbufWriteU8(dst, 0); #endif - sbufWriteU8(dst, osdProfile()->units); - sbufWriteU8(dst, osdProfile()->rssi_alarm); - sbufWriteU16(dst, osdProfile()->cap_alarm); - sbufWriteU16(dst, osdProfile()->time_alarm); - sbufWriteU16(dst, osdProfile()->alt_alarm); + sbufWriteU8(dst, osdConfig()->units); + sbufWriteU8(dst, osdConfig()->rssi_alarm); + sbufWriteU16(dst, osdConfig()->cap_alarm); + sbufWriteU16(dst, osdConfig()->time_alarm); + sbufWriteU16(dst, osdConfig()->alt_alarm); for (int i = 0; i < OSD_ITEM_COUNT; i++) { - sbufWriteU16(dst, osdProfile()->item_pos[i]); + sbufWriteU16(dst, osdConfig()->item_pos[i]); } #else sbufWriteU8(dst, 0); // OSD not supported @@ -1315,12 +1317,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif break; case MSP_SET_ACC_TRIM: - accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src); - accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src); + accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src); + accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src); break; case MSP_SET_ARMING_CONFIG: - armingConfig()->auto_disarm_delay = sbufReadU8(src); - armingConfig()->disarm_kill_switch = sbufReadU8(src); + armingConfigMutable()->auto_disarm_delay = sbufReadU8(src); + armingConfigMutable()->disarm_kill_switch = sbufReadU8(src); break; case MSP_SET_LOOP_TIME: @@ -1342,7 +1344,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_MODE_RANGE: i = sbufReadU8(src); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i]; + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); i = sbufReadU8(src); const box_t *box = findBoxByPermenantId(i); if (box) { @@ -1351,7 +1353,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), motorConfig(), ¤tProfile->pidProfile); } else { return MSP_RESULT_ERROR; } @@ -1363,7 +1365,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_ADJUSTMENT_RANGE: i = sbufReadU8(src); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + adjustmentRange_t *adjRange = adjustmentRangesMutable(i); i = sbufReadU8(src); if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { adjRange->adjustmentIndex = i; @@ -1405,32 +1407,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_MISC: - rxConfig()->midrc = sbufReadU16(src); - motorConfig()->minthrottle = sbufReadU16(src); - motorConfig()->maxthrottle = sbufReadU16(src); - motorConfig()->mincommand = sbufReadU16(src); + rxConfigMutable()->midrc = sbufReadU16(src); + motorConfigMutable()->minthrottle = sbufReadU16(src); + motorConfigMutable()->maxthrottle = sbufReadU16(src); + motorConfigMutable()->mincommand = sbufReadU16(src); - failsafeConfig()->failsafe_throttle = sbufReadU16(src); + failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); #ifdef GPS - gpsConfig()->provider = sbufReadU8(src); // gps_type + gpsConfigMutable()->provider = sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate - gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas + gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas #else sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate sbufReadU8(src); // gps_ubx_sbas #endif - batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src); - rxConfig()->rssi_channel = sbufReadU8(src); + batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src); + rxConfigMutable()->rssi_channel = sbufReadU8(src); sbufReadU8(src); - compassConfig()->mag_declination = sbufReadU16(src) * 10; + compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; - batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert break; case MSP_SET_MOTOR: @@ -1448,14 +1450,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= MAX_SUPPORTED_SERVOS) { return MSP_RESULT_ERROR; } else { - servoProfile()->servoConf[i].min = sbufReadU16(src); - servoProfile()->servoConf[i].max = sbufReadU16(src); - servoProfile()->servoConf[i].middle = sbufReadU16(src); - servoProfile()->servoConf[i].rate = sbufReadU8(src); - servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src); - servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src); - servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src); - servoProfile()->servoConf[i].reversedSources = sbufReadU32(src); + servoParamsMutable(i)->min = sbufReadU16(src); + servoParamsMutable(i)->max = sbufReadU16(src); + servoParamsMutable(i)->middle = sbufReadU16(src); + servoParamsMutable(i)->rate = sbufReadU8(src); + servoParamsMutable(i)->angleAtMin = sbufReadU8(src); + servoParamsMutable(i)->angleAtMax = sbufReadU8(src); + servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src); + servoParamsMutable(i)->reversedSources = sbufReadU32(src); } #endif break; @@ -1466,59 +1468,59 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= MAX_SERVO_RULES) { return MSP_RESULT_ERROR; } else { - customServoMixer(i)->targetChannel = sbufReadU8(src); - customServoMixer(i)->inputSource = sbufReadU8(src); - customServoMixer(i)->rate = sbufReadU8(src); - customServoMixer(i)->speed = sbufReadU8(src); - customServoMixer(i)->min = sbufReadU8(src); - customServoMixer(i)->max = sbufReadU8(src); - customServoMixer(i)->box = sbufReadU8(src); + customServoMixersMutable(i)->targetChannel = sbufReadU8(src); + customServoMixersMutable(i)->inputSource = sbufReadU8(src); + customServoMixersMutable(i)->rate = sbufReadU8(src); + customServoMixersMutable(i)->speed = sbufReadU8(src); + customServoMixersMutable(i)->min = sbufReadU8(src); + customServoMixersMutable(i)->max = sbufReadU8(src); + customServoMixersMutable(i)->box = sbufReadU8(src); loadCustomServoMixer(); } #endif break; case MSP_SET_3D: - flight3DConfig()->deadband3d_low = sbufReadU16(src); - flight3DConfig()->deadband3d_high = sbufReadU16(src); - flight3DConfig()->neutral3d = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_low = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_high = sbufReadU16(src); + flight3DConfigMutable()->neutral3d = sbufReadU16(src); break; case MSP_SET_RC_DEADBAND: - rcControlsConfig()->deadband = sbufReadU8(src); - rcControlsConfig()->yaw_deadband = sbufReadU8(src); - rcControlsConfig()->alt_hold_deadband = sbufReadU8(src); - flight3DConfig()->deadband3d_throttle = sbufReadU16(src); + rcControlsConfigMutable()->deadband = sbufReadU8(src); + rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); + rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); + flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RESET_CURR_PID: resetProfile(currentProfile); break; case MSP_SET_SENSOR_ALIGNMENT: - gyroConfig()->gyro_align = sbufReadU8(src); - accelerometerConfig()->acc_align = sbufReadU8(src); - compassConfig()->mag_align = sbufReadU8(src); + gyroConfigMutable()->gyro_align = sbufReadU8(src); + accelerometerConfigMutable()->acc_align = sbufReadU8(src); + compassConfigMutable()->mag_align = sbufReadU8(src); break; case MSP_SET_ADVANCED_CONFIG: - gyroConfig()->gyro_sync_denom = sbufReadU8(src); - pidConfig()->pid_process_denom = sbufReadU8(src); - motorConfig()->useUnsyncedPwm = sbufReadU8(src); + gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src); + pidConfigMutable()->pid_process_denom = sbufReadU8(src); + motorConfigMutable()->useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT - motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); + motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); #else - motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); + motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif motorConfig()->motorPwmRate = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } if (sbufBytesRemaining(src)) { - gyroConfig()->gyro_use_32khz = sbufReadU8(src); + gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src); } //!!TODO gyro_isr_update to be added pending decision /*if (sbufBytesRemaining(src)) { - gyroConfig()->gyro_isr_update = sbufReadU8(src); + gyroConfigMutable()->gyro_isr_update = sbufReadU8(src); }*/ validateAndFixGyroConfig(); @@ -1528,18 +1530,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_FILTER_CONFIG: - gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src); + gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { - gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src); - gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); + 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); } if (dataSize > 13) { - gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src); - gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); @@ -1569,9 +1571,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - accelerometerConfig()->acc_hardware = sbufReadU8(src); - barometerConfig()->baro_hardware = sbufReadU8(src); - compassConfig()->mag_hardware = sbufReadU8(src); + accelerometerConfigMutable()->acc_hardware = sbufReadU8(src); + barometerConfigMutable()->baro_hardware = sbufReadU8(src); + compassConfigMutable()->mag_hardware = sbufReadU8(src); break; case MSP_RESET_CONF: @@ -1603,9 +1605,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_BLACKBOX_CONFIG: // Don't allow config to be updated while Blackbox is logging if (blackboxMayEditConfig()) { - blackboxConfig()->device = sbufReadU8(src); - blackboxConfig()->rate_num = sbufReadU8(src); - blackboxConfig()->rate_denom = sbufReadU8(src); + blackboxConfigMutable()->device = sbufReadU8(src); + blackboxConfigMutable()->rate_num = sbufReadU8(src); + blackboxConfigMutable()->rate_denom = sbufReadU8(src); } break; #endif @@ -1761,53 +1763,53 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_BOARD_ALIGNMENT: - boardAlignment()->rollDegrees = sbufReadU16(src); - boardAlignment()->pitchDegrees = sbufReadU16(src); - boardAlignment()->yawDegrees = sbufReadU16(src); + boardAlignmentMutable()->rollDegrees = sbufReadU16(src); + boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); + boardAlignmentMutable()->yawDegrees = sbufReadU16(src); break; case MSP_SET_VOLTAGE_METER_CONFIG: - batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert if (dataSize > 4) { - batteryConfig()->batteryMeterType = sbufReadU8(src); + batteryConfigMutable()->batteryMeterType = sbufReadU8(src); } break; case MSP_SET_CURRENT_METER_CONFIG: - batteryConfig()->currentMeterScale = sbufReadU16(src); - batteryConfig()->currentMeterOffset = sbufReadU16(src); - batteryConfig()->currentMeterType = sbufReadU8(src); - batteryConfig()->batteryCapacity = sbufReadU16(src); + batteryConfigMutable()->currentMeterScale = sbufReadU16(src); + batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); + batteryConfigMutable()->currentMeterType = sbufReadU8(src); + batteryConfigMutable()->batteryCapacity = sbufReadU16(src); break; #ifndef USE_QUAD_MIXER_ONLY case MSP_SET_MIXER: - mixerConfig()->mixerMode = sbufReadU8(src); + mixerConfigMutable()->mixerMode = sbufReadU8(src); break; #endif case MSP_SET_RX_CONFIG: - rxConfig()->serialrx_provider = sbufReadU8(src); - rxConfig()->maxcheck = sbufReadU16(src); - rxConfig()->midrc = sbufReadU16(src); - rxConfig()->mincheck = sbufReadU16(src); - rxConfig()->spektrum_sat_bind = sbufReadU8(src); + rxConfigMutable()->serialrx_provider = sbufReadU8(src); + rxConfigMutable()->maxcheck = sbufReadU16(src); + rxConfigMutable()->midrc = sbufReadU16(src); + rxConfigMutable()->mincheck = sbufReadU16(src); + rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); if (dataSize > 8) { - rxConfig()->rx_min_usec = sbufReadU16(src); - rxConfig()->rx_max_usec = sbufReadU16(src); + rxConfigMutable()->rx_min_usec = sbufReadU16(src); + rxConfigMutable()->rx_max_usec = sbufReadU16(src); } if (dataSize > 12) { - rxConfig()->rcInterpolation = sbufReadU8(src); - rxConfig()->rcInterpolationInterval = sbufReadU8(src); - rxConfig()->airModeActivateThreshold = sbufReadU16(src); + rxConfigMutable()->rcInterpolation = sbufReadU8(src); + rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src); + rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src); } if (dataSize > 16) { - rxConfig()->rx_spi_protocol = sbufReadU8(src); - rxConfig()->rx_spi_id = sbufReadU32(src); - rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src); + rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); + rxConfigMutable()->rx_spi_id = sbufReadU32(src); + rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); } if (dataSize > 22) { rxConfig()->fpvCamAngleDegrees = sbufReadU8(src); @@ -1815,31 +1817,31 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_FAILSAFE_CONFIG: - failsafeConfig()->failsafe_delay = sbufReadU8(src); - failsafeConfig()->failsafe_off_delay = sbufReadU8(src); - failsafeConfig()->failsafe_throttle = sbufReadU16(src); - failsafeConfig()->failsafe_kill_switch = sbufReadU8(src); - failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src); - failsafeConfig()->failsafe_procedure = sbufReadU8(src); + failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); + failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src); + failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); + failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src); break; case MSP_SET_RXFAIL_CONFIG: i = sbufReadU8(src); if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { - rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src); - rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); + rxConfigMutable()->failsafe_channel_configurations[i].mode = sbufReadU8(src); + rxConfigMutable()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); } else { return MSP_RESULT_ERROR; } break; case MSP_SET_RSSI_CONFIG: - rxConfig()->rssi_channel = sbufReadU8(src); + rxConfigMutable()->rssi_channel = sbufReadU8(src); break; case MSP_SET_RX_MAP: for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - rxConfig()->rcmap[i] = sbufReadU8(src); + rxConfigMutable()->rcmap[i] = sbufReadU8(src); } break; @@ -1847,20 +1849,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_QUAD_MIXER_ONLY sbufReadU8(src); // mixerMode ignored #else - mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode + mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode #endif featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap - rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type + rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type - boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll - boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch - boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw + boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll + boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch + boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw - batteryConfig()->currentMeterScale = sbufReadU16(src); - batteryConfig()->currentMeterOffset = sbufReadU16(src); + batteryConfigMutable()->currentMeterScale = sbufReadU16(src); + batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); break; case MSP_SET_CF_SERIAL_CONFIG: @@ -1894,7 +1896,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef LED_STRIP case MSP_SET_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + hsvColor_t *color = &ledStripConfigMutable()->colors[i]; color->h = sbufReadU16(src); color->s = sbufReadU8(src); color->v = sbufReadU8(src); @@ -1907,7 +1909,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) { return MSP_RESULT_ERROR; } - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i]; *ledConfig = sbufReadU32(src); reevaluateLedConfig(); } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ec12ed8090..a729abe88a 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -27,6 +27,11 @@ #include "common/color.h" #include "common/utils.h" +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" @@ -43,8 +48,10 @@ #include "fc/cli.h" #include "fc/fc_dispatch.h" -#include "flight/pid.h" #include "flight/altitudehold.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/pid.h" #include "io/beeper.h" #include "io/dashboard.h" @@ -72,10 +79,6 @@ #include "telemetry/telemetry.h" -#include "config/feature.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #ifdef USE_BST void taskBstMasterProcess(timeUs_t currentTimeUs); #endif @@ -94,7 +97,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - accUpdate(&accelerometerConfig()->accelerometerTrims); + accUpdate(&accelerometerConfigMutable()->accelerometerTrims); } static void taskHandleSerial(timeUs_t currentTimeUs) @@ -128,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + updateCurrentMeter(ibatTimeSinceLastServiced, rxConfig(), flight3DConfig()->deadband3d_throttle); } } } @@ -201,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs) telemetryCheckState(); if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + telemetryProcess(currentTimeUs, rxConfig(), flight3DConfig()->deadband3d_throttle); } } #endif diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index fea3bdf4b6..f13d105fb0 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -31,6 +31,8 @@ #include "common/maths.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -59,7 +61,7 @@ #include "flight/failsafe.h" -static motorConfig_t *motorConfig; +static const motorConfig_t *motorConfig; static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) @@ -115,7 +117,7 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) @@ -128,7 +130,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) +void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -318,12 +320,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat } -bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId) +bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId) { uint8_t index; for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; + const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { return true; @@ -647,7 +649,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) #define RESET_FREQUENCY_2HZ (1000 / 2) -void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig) +void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig) { uint8_t adjustmentIndex; uint32_t now = millis(); @@ -728,7 +730,7 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) { motorConfig = motorConfigToUse; pidProfile = pidProfileToUse; @@ -740,4 +742,3 @@ void resetAdjustmentStates(void) { memset(adjustmentStates, 0, sizeof(adjustmentStates)); } - diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index e416b682b1..4f2533afd1 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -19,6 +19,8 @@ #include +#include "config/parameter_group.h" + typedef enum { BOXARM = 0, BOXANGLE, @@ -140,6 +142,8 @@ typedef struct modeActivationCondition_s { channelRange_t range; } modeActivationCondition_t; +PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); + typedef struct modeActivationProfile_s { modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; } modeActivationProfile_t; @@ -158,6 +162,8 @@ typedef struct controlRateConfig_s { uint16_t tpa_breakpoint; // Breakpoint where TPA is activated } controlRateConfig_t; +//!!TODO PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); + extern int16_t rcCommand[4]; extern int16_t rcCommandSmooth[4]; @@ -169,18 +175,22 @@ typedef struct rcControlsConfig_s { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) } rcControlsConfig_t; +PG_DECLARE(rcControlsConfig_t, rcControlsConfig); + typedef struct armingConfig_s { uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 } armingConfig_t; +PG_DECLARE(armingConfig_t, armingConfig); + bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); struct rxConfig_s; -throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +throttleStatus_e calculateThrottleStatus(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -267,6 +277,8 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 15 +PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); + typedef struct adjustmentProfile_s { adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; } adjustmentProfile_t; @@ -274,12 +286,12 @@ typedef struct adjustmentProfile_s { bool isAirmodeActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig, const struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); -bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); +bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; struct motorConfig_s; -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 8f447cb2cb..84b437b221 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -34,7 +34,7 @@ #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t *motorConfig) +void generateThrottleCurve(controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig) { uint8_t i; uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle); diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index 41132a98f3..c13851969e 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -19,7 +19,7 @@ struct controlRateConfig_s; struct motorConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct motorConfig_s *motorConfig); +void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index c0603b4f01..f0d2a9e16f 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -24,6 +24,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "fc/config.h" @@ -50,9 +53,10 @@ static failsafeState_t failsafeState; -static failsafeConfig_t *failsafeConfig; - -static rxConfig_t *rxConfig; +#ifndef USE_PARAMETER_GROPUS +static const failsafeConfig_t *failsafeConfig; +static const rxConfig_t *rxConfig; +#endif static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC @@ -72,15 +76,23 @@ static void failsafeReset(void) /* * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. */ -void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse) +void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(failsafeConfigToUse); +#else failsafeConfig = failsafeConfigToUse; +#endif failsafeReset(); } -void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) +void failsafeInit(const rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) { +#ifdef USE_PARAMETER_GROUPS + (void)(intialRxConfig); +#else rxConfig = intialRxConfig; +#endif deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d166e1c360..be32b769e5 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + #define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5) #define MILLIS_PER_TENTH_SECOND 100 #define MILLIS_PER_SECOND 1000 @@ -36,6 +38,8 @@ typedef struct failsafeConfig_s { uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it } failsafeConfig_t; +PG_DECLARE(failsafeConfig_t, failsafeConfig); + typedef enum { FAILSAFE_IDLE = 0, FAILSAFE_RX_LOSS_DETECTED, @@ -71,8 +75,8 @@ typedef struct failsafeState_s { } failsafeState_t; struct rxConfig_s; -void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); -void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); +void failsafeInit(const struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); +void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse); void failsafeStartMonitoring(void); void failsafeUpdateState(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a25d992285..fadaf5e782 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -30,6 +30,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "sensors/sensors.h" diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9525c251ea..719a9f9ff5 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -21,6 +21,8 @@ #include "common/maths.h" #include "common/time.h" +#include "config/parameter_group.h" + #include "sensors/acceleration.h" // Exported symbols @@ -65,6 +67,8 @@ typedef struct imuConfig_s { accDeadband_t accDeadband; } imuConfig_t; +PG_DECLARE(imuConfig_t, imuConfig); + typedef struct imuRuntimeConfig_s { float dcm_ki; float dcm_kp; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index accc55505e..ddac5029db 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -28,6 +28,10 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/pwm_output.h" @@ -46,9 +50,6 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/feature.h" -#include "config/config_master.h" - #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 @@ -61,11 +62,13 @@ static float motorMixRange; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; -static mixerConfig_t *mixerConfig; -static flight3DConfig_t *flight3DConfig; -static motorConfig_t *motorConfig; static airplaneConfig_t *airplaneConfig; -rxConfig_t *rxConfig; +#ifndef USE_PARAMETER_GROUPS +static const mixerConfig_t *mixerConfig; +static const flight3DConfig_t *flight3DConfig; +static const motorConfig_t *motorConfig; +const rxConfig_t *rxConfig; +#endif mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -253,7 +256,7 @@ float getMotorMixRange() bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT - switch(motorConfig->motorPwmProtocol) { + switch(motorConfig()->motorPwmProtocol) { case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: @@ -273,25 +276,25 @@ void initEscEndpoints(void) { if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); else - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; } else #endif { - disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; - motorOutputLow = motorConfig->minthrottle; - motorOutputHigh = motorConfig->maxthrottle; - deadbandMotor3dHigh = flight3DConfig->deadband3d_high; - deadbandMotor3dLow = flight3DConfig->deadband3d_low; + disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; + motorOutputLow = motorConfig()->minthrottle; + motorOutputHigh = motorConfig()->maxthrottle; + deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; + deadbandMotor3dLow = flight3DConfig()->deadband3d_low; } - rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck); - rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle; - rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle; + rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); + rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle; + rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; } void mixerUseConfigs( @@ -301,11 +304,18 @@ void mixerUseConfigs( airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(flight3DConfigToUse); + (void)(motorConfigToUse); + (void)(mixerConfigToUse); + (void)(rxConfigToUse); +#else flight3DConfig = flight3DConfigToUse; motorConfig = motorConfigToUse; mixerConfig = mixerConfigToUse; - airplaneConfig = airplaneConfigToUse; rxConfig = rxConfigToUse; +#endif + airplaneConfig = airplaneConfigToUse; } void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) @@ -440,25 +450,25 @@ void mixTable(pidProfile_t *pidProfile) // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { - if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. - if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling + if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; - } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling + } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling motorOutputMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive + } else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; - throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle; + throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; } else { // Deadband handling from positive to negative @@ -468,7 +478,7 @@ void mixTable(pidProfile_t *pidProfile) currentThrottleInputRange = rcCommandThrottleRange3dHigh; } } else { - throttle = rcCommand[THROTTLE] - rxConfig->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange; motorOutputMin = motorOutputLow; motorOutputMax = motorOutputHigh; @@ -493,7 +503,7 @@ void mixTable(pidProfile_t *pidProfile) motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + - scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction); + scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction); if (vbatCompensationFactor > 1.0f) { motorMix[i] *= vbatCompensationFactor; // Add voltage compensation @@ -541,7 +551,7 @@ void mixTable(pidProfile_t *pidProfile) // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { - if (((rcData[THROTTLE]) < rxConfig->mincheck)) { + if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { motor[i] = disarmMotorOutput; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8a84ea8acb..6a206f4e86 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -17,6 +17,9 @@ #pragma once +#include "config/parameter_group.h" +#include "drivers/io_types.h" + #define MAX_SUPPORTED_MOTORS 12 #define QUAD_MOTOR_COUNT 4 @@ -85,6 +88,8 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; +PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer); + // Custom mixer configuration typedef struct mixer_s { uint8_t motorCount; @@ -97,6 +102,8 @@ typedef struct mixerConfig_s { int8_t yaw_motor_direction; } mixerConfig_t; +PG_DECLARE(mixerConfig_t, mixerConfig); + typedef struct flight3DConfig_s { uint16_t deadband3d_low; // min 3d value uint16_t deadband3d_high; // max 3d value @@ -104,6 +111,22 @@ typedef struct flight3DConfig_s { uint16_t deadband3d_throttle; // default throttle deadband from MIDRC } flight3DConfig_t; +PG_DECLARE(flight3DConfig_t, flight3DConfig); + +typedef struct motorConfig_s { + uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. + uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 + uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint8_t motorPwmProtocol; // Pwm Protocol + uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation + uint8_t useUnsyncedPwm; + float digitalIdleOffsetPercent; + ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; +} motorConfig_t; + +PG_DECLARE(motorConfig_t, motorConfig); + typedef struct airplaneConfig_s { int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign } airplaneConfig_t; @@ -113,8 +136,6 @@ typedef struct airplaneConfig_s { extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; - -struct motorConfig_s; struct rxConfig_s; uint8_t getMotorCount(); @@ -122,7 +143,7 @@ float getMotorMixRange(); void mixerUseConfigs( flight3DConfig_t *flight3DConfigToUse, - struct motorConfig_s *motorConfigToUse, + motorConfig_t *motorConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, struct rxConfig_s *rxConfigToUse); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index fc1c4a144d..0b0229c3de 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -31,6 +31,9 @@ #include "common/maths.h" #include "common/time.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "fc/config.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bcbdbaaaa9..1f855739f9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -28,6 +28,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/fc_core.h" #include "fc/fc_rc.h" diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 2c40241d34..d88a937a5b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,6 +19,8 @@ #include +#include "config/parameter_group.h" + #define PID_CONTROLLER_BETAFLIGHT 1 #define PID_MIXER_SCALING 1000.0f #define PID_SERVO_MIXER_SCALING 0.7f @@ -85,6 +87,8 @@ typedef struct pidProfile_s { float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms } pidProfile_t; +PG_DECLARE_PROFILE(pidProfile_t, pidProfile); + typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate } pidConfig_t; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 2ba83eb026..87a276fe16 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -28,6 +28,9 @@ #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/pwm_output.h" #include "drivers/system.h" @@ -47,7 +50,6 @@ #include "config/feature.h" extern mixerMode_e currentMixerMode; -extern rxConfig_t *rxConfig; static servoMixerConfig_t *servoMixerConfig; @@ -276,7 +278,7 @@ void writeServos(void) case MIXER_TRI: case MIXER_CUSTOM_TRI: - if (servoMixerConfig->tri_unarmed_servo) { + if (servoMixerConfig()->tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); } else { @@ -346,7 +348,7 @@ STATIC_UNIT_TESTED void servoMixer(void) input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { input[INPUT_STABILIZED_YAW] *= -1; } } @@ -362,14 +364,14 @@ STATIC_UNIT_TESTED void servoMixer(void) // 2000 - 1500 = +500 // 1500 - 1500 = 0 // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; - input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; - input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; - input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; - input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; - input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; - input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; + input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc; + input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc; + input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc; + input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc; + input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig()->midrc; + input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig()->midrc; + input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig()->midrc; + input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig()->midrc; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) servo[i] = 0; @@ -440,7 +442,7 @@ void servoTable(void) servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { + if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { @@ -471,10 +473,10 @@ void filterServos(void) uint32_t startTime = micros(); #endif - if (servoMixerConfig->servo_lowpass_enable) { + if (servoMixerConfig()->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime); + biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig()->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 43f0b19489..8c5b735413 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + #define MAX_SUPPORTED_SERVOS 8 // These must be consecutive, see 'reversedSources' @@ -87,6 +89,8 @@ typedef struct servoMixer_s { #define MAX_SERVO_SPEED UINT8_MAX #define MAX_SERVO_BOXES 3 +PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers); + // Custom mixer configuration typedef struct mixerRules_s { uint8_t servoRuleCount; @@ -94,6 +98,7 @@ typedef struct mixerRules_s { } mixerRules_t; typedef struct servoParam_s { + uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted int16_t min; // servo min int16_t max; // servo max int16_t middle; // servo middle @@ -101,8 +106,9 @@ typedef struct servoParam_s { uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted -} __attribute__ ((__packed__)) servoParam_t; +} servoParam_t; + +PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams); typedef struct servoMixerConfig_s{ uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed @@ -110,6 +116,8 @@ typedef struct servoMixerConfig_s{ int8_t servo_lowpass_enable; // enable/disable lowpass filter } servoMixerConfig_t; +//!!TODO PG_DECLARE(servoConfig_t, servoConfig); + typedef struct servoProfile_s { servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; } servoProfile_t; diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index 79e1815dae..cb015b0987 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -24,7 +24,8 @@ #include "common/utils.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/display.h" #include "drivers/max7456.h" diff --git a/src/main/io/gimbal.h b/src/main/io/gimbal.h index 914ce05ba5..969242fee3 100644 --- a/src/main/io/gimbal.h +++ b/src/main/io/gimbal.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + typedef enum { GIMBAL_MODE_NORMAL = 0, GIMBAL_MODE_MIXTILT = 1 @@ -27,3 +29,5 @@ typedef enum { typedef struct gimbalConfig_s { uint8_t mode; } gimbalConfig_t; + +PG_DECLARE(gimbalConfig_t, gimbalConfig); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 9cae15f265..75f5e3c84d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -35,6 +35,11 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/system.h" +#include "drivers/light_led.h" #include "drivers/light_led.h" #include "drivers/system.h" diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 484f7bf52a..f776de69e2 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -19,6 +19,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + #define LAT 0 #define LON 1 @@ -68,6 +70,8 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; } gpsConfig_t; +PG_DECLARE(gpsConfig_t, gpsConfig); + typedef struct gpsCoordinateDDDMMmmmm_s { int16_t dddmm; int16_t mmmm; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 31734e9fd3..5b0074c9d5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -31,6 +31,9 @@ #include "common/maths.h" #include "common/typeconversion.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/light_ws2811strip.h" #include "drivers/system.h" #include "drivers/serial.h" @@ -44,6 +47,11 @@ #include "common/axis.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -73,10 +81,6 @@ #include "telemetry/telemetry.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - /* PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 4362a3d555..f90f935792 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -19,6 +19,7 @@ #include "common/color.h" #include "common/time.h" +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 @@ -147,6 +148,8 @@ typedef struct ledStripConfig_s { ioTag_t ioTag; } ledStripConfig_t; +PG_DECLARE(ledStripConfig_t, ledStripConfig); + ledConfig_t *ledConfigs; hsvColor_t *colors; modeColorIndexes_t *modeColors; diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 82517b2e9d..817e950356 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -20,14 +20,4 @@ #include "drivers/io_types.h" #include "flight/mixer.h" -typedef struct motorConfig_s { - uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. - uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 - uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint16_t motorPwmRate; // The update rate of motor outputs - uint8_t motorPwmProtocol; // Pwm Protocol - uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation - uint8_t useUnsyncedPwm; - float digitalIdleOffsetPercent; - ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; -} motorConfig_t; + diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 206f1a1e5d..f31e4d72e4 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -40,6 +40,11 @@ #include "common/printf.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/max7456_symbols.h" #include "drivers/display.h" #include "drivers/system.h" @@ -51,19 +56,15 @@ #include "cms/cms_types.h" #include "cms/cms_menu_osd.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/osd.h" #include "io/vtx.h" -#include "io/asyncfatfs/asyncfatfs.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c6539b42d7..00b6abc163 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define VISIBLE_FLAG 0x0800 #define VISIBLE(x) (x & VISIBLE_FLAG) @@ -67,6 +68,9 @@ typedef struct osd_profile_s { osd_unit_e units; } osd_profile_t; +// !!TODO change to osdConfig_t +PG_DECLARE(osd_profile_t, osdConfig); + struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); void osdResetConfig(osd_profile_t *osdProfile); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 6c96238aae..d79d135a54 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 5a68a18041..cfed31388c 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/serial.h" typedef enum { @@ -111,6 +112,8 @@ typedef struct serialConfig_s { uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. } serialConfig_t; +PG_DECLARE(serialConfig_t, serialConfig); + typedef void serialConsumer(uint8_t); // diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 711ceec755..804b77b0b0 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -26,3 +26,5 @@ typedef struct servoConfig_s { uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz) ioTag_t ioTags[MAX_SUPPORTED_SERVOS]; } servoConfig_t; + +PG_DECLARE(servoConfig_t, servoConfig); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 8db4c4279b..eb44e3f0b4 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -26,10 +26,14 @@ #include "io/osd.h" //External dependencies -#include "config/config_master.h" #include "config/config_eeprom.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/vtx_rtc6705.h" + #include "fc/runtime_config.h" + #include "io/beeper.h" diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index e87db2694d..e6c726f503 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -19,32 +19,36 @@ #include #include +#include #include #include "platform.h" #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) +#include "build/build_config.h" + #include "cms/cms.h" #include "cms/cms_types.h" -#include "string.h" #include "common/printf.h" #include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/vtx_common.h" -#include "io/serial.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx_string.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "flight/pid.h" -#include "config/config_master.h" -#include "build/build_config.h" +#include "io/serial.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx_string.h" //#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 897e8bc0bd..adf8939f7c 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -30,6 +30,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/rx_pwm.h" @@ -90,7 +92,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) rxRuntimeConfig_t rxRuntimeConfig; -static const rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) @@ -108,7 +109,7 @@ static uint8_t nullFrameStatus(void) void useRxConfig(const rxConfig_t *rxConfigToUse) { - rxConfig = rxConfigToUse; + (void)(rxConfigToUse); } #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels @@ -126,8 +127,8 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration) { - return pulseDuration >= rxConfig->rx_min_usec && - pulseDuration <= rxConfig->rx_max_usec; + return pulseDuration >= rxConfig()->rx_min_usec && + pulseDuration <= rxConfig()->rx_max_usec; } // pulse duration is in micro seconds (usec) @@ -204,20 +205,20 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } #endif -void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions) +void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions) { - useRxConfig(rxConfig); + useRxConfig(initialRxConfig); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rcSampleIndex = 0; needRxSignalMaxDelayUs = DELAY_10_HZ; for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcData[i] = rxConfig->midrc; + rcData[i] = rxConfig()->midrc; rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec; + rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -237,7 +238,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { - const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig); + const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SERIAL); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -248,14 +249,14 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef USE_RX_MSP if (feature(FEATURE_RX_MSP)) { - rxMspInit(rxConfig, &rxRuntimeConfig); + rxMspInit(rxConfig(), &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifdef USE_RX_SPI if (feature(FEATURE_RX_SPI)) { - const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig); + const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SPI); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -266,7 +267,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #if defined(USE_PWM) || defined(USE_PPM) if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { - rxPwmInit(rxConfig, &rxRuntimeConfig); + rxPwmInit(rxConfig(), &rxRuntimeConfig); } #endif } @@ -376,7 +377,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static uint16_t getRxfailValue(uint8_t channel) { - const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; + const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig()->failsafe_channel_configurations[channel]; switch(channelFailsafeConfiguration->mode) { case RX_FAILSAFE_MODE_AUTO: @@ -384,12 +385,12 @@ static uint16_t getRxfailValue(uint8_t channel) case ROLL: case PITCH: case YAW: - return rxConfig->midrc; + return rxConfig()->midrc; case THROTTLE: if (feature(FEATURE_3D)) - return rxConfig->midrc; + return rxConfig()->midrc; else - return rxConfig->rx_min_usec; + return rxConfig()->rx_min_usec; } /* no break */ @@ -420,7 +421,7 @@ static uint8_t getRxChannelCount(void) { static uint8_t maxChannelsAllowed; if (!maxChannelsAllowed) { - uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT; + uint8_t maxChannels = rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT; if (maxChannels > rxRuntimeConfig.channelCount) { maxChannelsAllowed = rxRuntimeConfig.channelCount; } else { @@ -436,14 +437,14 @@ static void readRxChannelsApplyRanges(void) const int channelCount = getRxChannelCount(); for (int channel = 0; channel < channelCount; channel++) { - const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel); // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { - sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]); + sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]); } rcRaw[channel] = sample; @@ -548,10 +549,10 @@ static void updateRSSIPWM(void) { int16_t pwmRssi = 0; // Read value of AUX channel as rssi - pwmRssi = rcData[rxConfig->rssi_channel - 1]; + pwmRssi = rcData[rxConfig()->rssi_channel - 1]; // RSSI_Invert option - if (rxConfig->rssi_ppm_invert) { + if (rxConfig()->rssi_ppm_invert) { pwmRssi = ((2000 - pwmRssi) + 1000); } @@ -578,7 +579,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) int16_t adcRssiMean = 0; uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale; + uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; @@ -599,7 +600,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) void updateRSSI(timeUs_t currentTimeUs) { - if (rxConfig->rssi_channel > 0) { + if (rxConfig()->rssi_channel > 0) { updateRSSIPWM(); } else if (feature(FEATURE_RSSI_ADC)) { updateRSSIADC(currentTimeUs); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 484d65914a..17f8d27763 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define STICK_CHANNEL_COUNT 4 @@ -105,11 +106,17 @@ typedef struct rxFailsafeChannelConfiguration_s { uint8_t step; } rxFailsafeChannelConfiguration_t; +//!!TODO change to rxFailsafeChannelConfig_t +PG_DECLARE_ARRAY(rxFailsafeChannelConfiguration_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs); + typedef struct rxChannelRangeConfiguration_s { uint16_t min; uint16_t max; } rxChannelRangeConfiguration_t; +//!!TODO change to rxChannelRangeConfig_t +PG_DECLARE_ARRAY(rxChannelRangeConfiguration_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs); + typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. @@ -139,6 +146,8 @@ typedef struct rxConfig_s { rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT]; } rxConfig_t; +PG_DECLARE(rxConfig_t, rxConfig); + #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) struct rxRuntimeConfig_s; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index ded23de3cf..d8395f141e 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -27,6 +27,9 @@ #include "common/axis.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index fda259e940..70905bb3a9 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" @@ -66,6 +67,8 @@ typedef struct accelerometerConfig_s { rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; +PG_DECLARE(accelerometerConfig_t, accelerometerConfig); + bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bac86ad16c..42a013259b 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -23,6 +23,9 @@ #include "common/maths.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/barometer.h" #include "drivers/barometer_bmp085.h" #include "drivers/barometer_bmp280.h" @@ -51,7 +54,9 @@ static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; +#ifndef USE_PARAMETER_GROUPS static const barometerConfig_t *barometerConfig; +#endif bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { @@ -121,7 +126,11 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) void useBarometerConfig(const barometerConfig_t *barometerConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(barometerConfigToUse); +#else barometerConfig = barometerConfigToUse; +#endif } bool isBaroCalibrationComplete(void) @@ -160,7 +169,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading) return newPressureReading; } -#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1) +#define PRESSURE_SAMPLE_COUNT (barometerConfig()->baro_sample_count - 1) static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) { @@ -213,7 +222,7 @@ uint32_t baroUpdate(void) baro.dev.get_up(); baro.dev.start_ut(); baro.dev.calculate(&baroPressure, &baroTemperature); - baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); + baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure); state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; @@ -228,7 +237,7 @@ int32_t baroCalculateAltitude(void) // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise + baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise return baro.BaroAlt; } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 536e6c453a..1f6e6687fa 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/barometer.h" typedef enum { @@ -37,6 +38,8 @@ typedef struct barometerConfig_s { float baro_cf_alt; // apply CF to use ACC for height estimation } barometerConfig_t; +PG_DECLARE(barometerConfig_t, barometerConfig); + typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index f79e581d2e..c3f6993f76 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -25,6 +25,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/adc.h" #include "drivers/system.h" @@ -65,6 +68,10 @@ int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start +#ifndef USE_PARAMETER_GROUPS +batteryConfig_t *batteryConfig; +#endif + static batteryState_e vBatState; static batteryState_e consumptionState; @@ -72,7 +79,7 @@ static uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier); + return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier); } static void updateBatteryVoltage(void) @@ -86,7 +93,7 @@ static void updateBatteryVoltage(void) } #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { + if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) { escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0; if (debugMode == DEBUG_BATTERY) { @@ -134,20 +141,20 @@ void updateBattery(void) uint16_t vBatMeasured = vbatLatest; /* battery has just been connected*/ - if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { + if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ vBatState = BATTERY_OK; - unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; + unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } batteryCellCount = cells; - batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; - batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */ - } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { + batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; + batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */ + } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { vBatState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; @@ -159,16 +166,16 @@ void updateBattery(void) debug[3] = batteryCellCount; } - if (batteryConfig->useVBatAlerts) { + if (batteryConfig()->useVBatAlerts) { switch(vBatState) { case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_WARNING; } break; case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_CRITICAL; } else if (vbat > batteryWarningVoltage) { vBatState = BATTERY_OK; @@ -208,7 +215,11 @@ const char * getBatteryStateString(void) void batteryInit(batteryConfig_t *initialBatteryConfig) { +#ifndef USE_PARAMETER_GROUPS + (void)initialBatteryConfig; +#else batteryConfig = initialBatteryConfig; +#endif vBatState = BATTERY_NOT_PRESENT; consumptionState = BATTERY_OK; batteryCellCount = 0; @@ -221,9 +232,9 @@ static int32_t currentSensorToCentiamps(uint16_t src) int32_t millivolts; millivolts = ((uint32_t)src * ADCVREF) / 4096; - millivolts -= batteryConfig->currentMeterOffset; + millivolts -= batteryConfig()->currentMeterOffset; - return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps + return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps } static void updateBatteryCurrent(void) @@ -251,10 +262,10 @@ static void updateCurrentDrawn(int32_t lastUpdateAt) void updateConsumptionWarning(void) { - if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { + if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { if (calculateBatteryPercentage() == 0) { vBatState = BATTERY_CRITICAL; - } else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) { + } else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) { consumptionState = BATTERY_WARNING; } else { consumptionState = BATTERY_OK; @@ -264,35 +275,30 @@ void updateConsumptionWarning(void) } } -void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { + (void)(rxConfig); if (getBatteryState() != BATTERY_NOT_PRESENT) { - switch(batteryConfig->currentMeterType) { + switch(batteryConfig()->currentMeterType) { case CURRENT_SENSOR_ADC: updateBatteryCurrent(); - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_VIRTUAL: - amperageLatest = (int32_t)batteryConfig->currentMeterOffset; + amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000; } amperage = amperageLatest; - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR @@ -328,7 +334,7 @@ float calculateVbatPidCompensation(void) { float batteryScaler = 1.0f; if (feature(FEATURE_VBAT) && batteryCellCount > 0) { // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference - batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); + batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); } return batteryScaler; } @@ -337,11 +343,11 @@ uint8_t calculateBatteryPercentage(void) { uint8_t batteryPercentage = 0; if (batteryCellCount > 0) { - uint16_t batteryCapacity = batteryConfig->batteryCapacity; + uint16_t batteryCapacity = batteryConfig()->batteryCapacity; if (batteryCapacity > 0) { batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100); } else { - batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100); } } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3eb68940c8..69be1bf98d 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "common/maths.h" // for fix12_t #ifndef VBAT_SCALE_DEFAULT @@ -63,6 +64,8 @@ typedef struct batteryConfig_s { uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning } batteryConfig_t; +PG_DECLARE(batteryConfig_t, batteryConfig); + typedef enum { BATTERY_OK = 0, BATTERY_WARNING, @@ -77,15 +80,17 @@ extern uint16_t batteryWarningVoltage; extern int32_t amperageLatest; extern int32_t amperage; extern int32_t mAhDrawn; +#ifndef USE_PARAMETER_GROUPS +extern batteryConfig_t *batteryConfig; +#endif batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); void batteryInit(batteryConfig_t *initialBatteryConfig); -batteryConfig_t *batteryConfig; struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index c4f1988274..0b45b8ffe8 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -23,6 +23,9 @@ #include "common/maths.h" #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "boardalignment.h" diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index b1ab74cdf9..8c224f24e2 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -17,11 +17,15 @@ #pragma once +#include "config/parameter_group.h" + typedef struct boardAlignment_s { int32_t rollDegrees; int32_t pitchDegrees; int32_t yawDegrees; } boardAlignment_t; +PG_DECLARE(boardAlignment_t, boardAlignment); + void alignSensors(int32_t *dest, uint8_t rotation); void initBoardAlignment(const boardAlignment_t *boardAlignment); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 9cc1e29fc3..be53d8c9b6 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -22,6 +22,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/compass.h" #include "drivers/compass_ak8975.h" #include "drivers/compass_ak8963.h" diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 4e534fea08..59b997d76f 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -17,8 +17,8 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/compass.h" - #include "sensors/sensors.h" @@ -47,6 +47,8 @@ typedef struct compassConfig_s { flightDynamicsTrims_t magZero; } compassConfig_t; +PG_DECLARE(compassConfig_t, compassConfig); + bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); void compassInit(const compassConfig_t *compassConfig); union flightDynamicsTrims_u; diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index aad7b1ea4a..4fe0876ba1 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -21,26 +21,28 @@ #include -#include "fc/config.h" +#include "build/debug.h" + #include "config/feature.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/utils.h" -#include "drivers/system.h" +#include "drivers/pwm_output.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "drivers/pwm_output.h" +#include "drivers/system.h" -#include "io/serial.h" +#include "esc_sensor.h" + +#include "fc/config.h" #include "flight/mixer.h" #include "sensors/battery.h" -#include "build/debug.h" - -#include "esc_sensor.h" +#include "io/serial.h" /* KISS ESC TELEMETRY PROTOCOL diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d0b15ece6b..1fb1f66181 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,6 +28,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" @@ -69,7 +72,9 @@ gyro_t gyro; // gyro access functions static int32_t gyroADC[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; +#ifndef USE_PARAMETER_GROUPS static const gyroConfig_t *gyroConfig; +#endif static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -233,7 +238,11 @@ static bool gyroDetect(gyroDev_t *dev) bool gyroInit(const gyroConfig_t *gyroConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(gyroConfigToUse); +#else gyroConfig = gyroConfigToUse; +#endif memset(&gyro, 0, sizeof(gyro)); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); @@ -261,8 +270,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) } // Must set gyro sample rate before initialisation - gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz); - gyro.dev.lpf = gyroConfig->gyro_lpf; + gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); + gyro.dev.lpf = gyroConfig()->gyro_lpf; gyro.dev.init(&gyro.dev); gyroInitFilters(); return true; @@ -280,43 +289,43 @@ void gyroInitFilters(void) notchFilter1ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply; - if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known - if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { + if (gyroConfig()->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known + if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } - } else if (gyroConfig->gyro_soft_lpf_type == FILTER_PT1) { + } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt); + pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + firFilterDenoiseInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } } } - if (gyroConfig->gyro_soft_notch_hz_1) { + if (gyroConfig()->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); + const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(notchFilter1[axis], gyroConfig()->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } - if (gyroConfig->gyro_soft_notch_hz_2) { + if (gyroConfig()->gyro_soft_notch_hz_2) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); + const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } @@ -441,7 +450,7 @@ void gyroUpdate(void) if (calibrationComplete) { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) // SPI-based gyro so can read and update in ISR - if (gyroConfig->gyro_isr_update) { + if (gyroConfig()->gyro_isr_update) { mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR); return; } @@ -450,7 +459,7 @@ void gyroUpdate(void) debug[3] = (uint16_t)(micros() & 0xffff); #endif } else { - performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); + performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold); } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 1fdf8820a6..5bd68fcb91 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "common/axis.h" @@ -59,6 +60,8 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_2; } gyroConfig_t; +PG_DECLARE(gyroConfig_t, gyroConfig); + void gyroSetCalibrationCycles(void); bool gyroInit(const gyroConfig_t *gyroConfigToUse); void gyroInitFilters(void); diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index e8c78c6087..0e3d0768a0 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -29,6 +29,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/runtime_config.h" diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 16151fc2f9..d123679394 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,10 +17,9 @@ #pragma once +#include "config/parameter_group.h" #include "common/time.h" - #include "drivers/sonar_hcsr04.h" - #include "sensors/battery.h" #define SONAR_OUT_OF_RANGE (-1) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 18717103cd..4912bc15f8 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -77,8 +77,6 @@ #include "bus_bst.h" #include "i2c_bst.h" -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse); - #define BST_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made @@ -640,13 +638,13 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_SERVO_MIX_RULES: for (i = 0; i < MAX_SERVO_RULES; i++) { - bstWrite8(customServoMixer(i)->targetChannel); - bstWrite8(customServoMixer(i)->inputSource); - bstWrite8(customServoMixer(i)->rate); - bstWrite8(customServoMixer(i)->speed); - bstWrite8(customServoMixer(i)->min); - bstWrite8(customServoMixer(i)->max); - bstWrite8(customServoMixer(i)->box); + bstWrite8(customServoMixers(i)->targetChannel); + bstWrite8(customServoMixers(i)->inputSource); + bstWrite8(customServoMixers(i)->rate); + bstWrite8(customServoMixers(i)->speed); + bstWrite8(customServoMixers(i)->min); + bstWrite8(customServoMixers(i)->max); + bstWrite8(customServoMixers(i)->box); } break; #endif @@ -1179,13 +1177,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (i >= MAX_SERVO_RULES) { ret = BST_FAILED; } else { - customServoMixer(i)->targetChannel = bstRead8(); - customServoMixer(i)->inputSource = bstRead8(); - customServoMixer(i)->rate = bstRead8(); - customServoMixer(i)->speed = bstRead8(); - customServoMixer(i)->min = bstRead8(); - customServoMixer(i)->max = bstRead8(); - customServoMixer(i)->box = bstRead8(); + customServoMixers(i)->targetChannel = bstRead8(); + customServoMixers(i)->inputSource = bstRead8(); + customServoMixers(i)->rate = bstRead8(); + customServoMixers(i)->speed = bstRead8(); + customServoMixers(i)->min = bstRead8(); + customServoMixers(i)->max = bstRead8(); + customServoMixers(i)->box = bstRead8(); loadCustomServoMixer(); } #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index a9270aa8af..313a73b635 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,6 +17,7 @@ #pragma once +//#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/crsf.c b/src/main/telemetry/crsf.c index d62326e113..baa00554a5 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -32,10 +32,8 @@ #define CLEANFLIGHT #endif -#ifdef CLEANFLIGHT #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#endif #include "common/streambuf.h" #include "common/utils.h" @@ -184,7 +182,7 @@ void crsfFrameBatterySensor(sbuf_t *dst) const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); #else crsfSerialize16(dst, amperage / 10); - const uint32_t batteryCapacity = batteryConfig->batteryCapacity; + const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); #endif crsfSerialize8(dst, (batteryCapacity >> 16)); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index f696419016..3dad42c3fc 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -33,6 +33,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -72,13 +74,13 @@ static serialPortConfig_t *portConfig; #define FRSKY_BAUDRATE 9600 #define FRSKY_INITIAL_PORT_MODE MODE_TX -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool frskyTelemetryEnabled = false; static portSharing_e frskyPortSharing; -extern batteryConfig_t *batteryConfig; - #define CYCLETIME 125 #define PROTOCOL_HEADER 0x5E @@ -196,7 +198,7 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { sendDataHead(ID_RPM); #ifdef USE_ESC_SENSOR @@ -213,7 +215,7 @@ static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadba throttleForRPM = 0; serialize16(throttleForRPM); } else { - serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER)); + serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER)); } #endif } @@ -240,7 +242,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void) } sendDataHead(ID_TEMPRATURE2); - if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) { + if (telemetryConfig()->frsky_unit == FRSKY_UNIT_METRICS) { serialize16(satellite); } else { float tmp = (satellite - 32) / 1.8f; @@ -286,7 +288,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left - if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) { + if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { result->dddmm = deg * 100 + min; } else { result->dddmm = deg * 60 + min; @@ -332,8 +334,8 @@ static void sendFakeLatLong(void) // Heading is only displayed on OpenTX if non-zero lat/long is also sent int32_t coord[2] = {0,0}; - coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); + coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); + coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); sendLatLong(coord); } @@ -409,7 +411,7 @@ static void sendVoltage(void) */ static void sendVoltageAmp(void) { - if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { + if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { /* * Use new ID 0x39 to send voltage directly in 0.1 volts resolution */ @@ -418,7 +420,7 @@ static void sendVoltageAmp(void) } else { uint16_t voltage = (getVbat() * 110) / 21; uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = voltage / batteryCellCount; } else { vfasVoltage = voltage; @@ -440,7 +442,7 @@ static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); - if (batteryConfig->batteryCapacity > 0) { + if (batteryConfig()->batteryCapacity > 0) { serialize16((uint16_t)calculateBatteryPercentage()); } else { serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); @@ -455,9 +457,13 @@ static void sendHeading(void) serialize16(0); } -void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initFrSkyTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); } @@ -475,7 +481,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!frskyPort) { return; } @@ -509,7 +515,7 @@ void checkFrSkyTelemetryState(void) } } -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void handleFrSkyTelemetry(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { if (!frskyTelemetryEnabled) { return; diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 89ba617c8f..419b4b407b 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -23,11 +23,11 @@ typedef enum { } frskyVFasPrecision_e; struct rxConfig_s; -void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void handleFrSkyTelemetry(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); void checkFrSkyTelemetryState(void); struct telemetryConfig_s; -void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig); +void initFrSkyTelemetry(const struct telemetryConfig_s *telemetryConfig); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index c43822b2a3..dacf2393f1 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -65,6 +65,10 @@ #include "common/axis.h" #include "common/time.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -108,7 +112,9 @@ static uint8_t hottMsgCrc; static serialPort_t *hottPort = NULL; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool hottTelemetryEnabled = false; static portSharing_e hottPortSharing; @@ -219,7 +225,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) static bool shouldTriggerBatteryAlarmNow(void) { - return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); + return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); } static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) @@ -289,9 +295,13 @@ void freeHoTTTelemetryPort(void) hottTelemetryEnabled = false; } -void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initHoTTTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 5370dc24f3..bd437afa93 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -490,7 +490,7 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(timeUs_t currentTimeUs); void checkHoTTTelemetryState(void); -void initHoTTTelemetry(telemetryConfig_t *telemetryConfig); +void initHoTTTelemetry(const telemetryConfig_t *telemetryConfig); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 8e6341e90b..9bb193454c 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -32,7 +32,8 @@ #if defined(TELEMETRY) && defined(TELEMETRY_IBUS) -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/axis.h" diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h index 8bbd30c5d8..3688ea2ffc 100644 --- a/src/main/telemetry/ibus.h +++ b/src/main/telemetry/ibus.h @@ -17,13 +17,11 @@ #pragma once -/* typedef struct ibusTelemetryConfig_s { uint8_t report_cell_voltage; // report vbatt divided with cellcount } ibusTelemetryConfig_t; PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig); -*/ void initIbusTelemetry(void); @@ -31,4 +29,4 @@ void handleIbusTelemetry(void); bool checkIbusTelemetryState(void); void configureIbusTelemetryPort(void); -void freeIbusTelemetryPort(void); \ No newline at end of file +void freeIbusTelemetryPort(void); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 038bbf497c..ab5a237db1 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -40,6 +40,7 @@ #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -80,7 +81,9 @@ static serialPort_t *ltmPort; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool ltmEnabled; static portSharing_e ltmPortSharing; static uint8_t ltm_crc; @@ -268,9 +271,13 @@ void freeLtmTelemetryPort(void) ltmEnabled = false; } -void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initLtmTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM); ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM); } diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index e5b4790c5a..e0603707da 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -20,7 +20,7 @@ #pragma once struct telemetryConfig_s; -void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initLtmTelemetry(const struct telemetryConfig_s *initialTelemetryConfig); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 7dbf92a1e2..5423919613 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -32,26 +32,18 @@ #include "common/axis.h" #include "common/color.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "fc/config.h" #include "fc/rc_controls.h" - -#include "io/serial.h" -#include "io/gimbal.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/motors.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" -#include "sensors/barometer.h" -#include "sensors/boardalignment.h" -#include "sensors/battery.h" - -#include "rx/rx.h" +#include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -60,16 +52,24 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + #include "telemetry/telemetry.h" #include "telemetry/mavlink.h" -#include "fc/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#include "fc/runtime_config.h" - // mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used // until this is resolved in mavlink library - ignore -Wpedantic for mavlink code #pragma GCC diagnostic push diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5b047861e6..13e09f4c6c 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -16,6 +16,11 @@ #include "common/maths.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -52,9 +57,6 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "config/config_profile.h" -#include "config/feature.h" - #include "msp/msp.h" extern profile_t *currentProfile; @@ -148,7 +150,9 @@ const uint16_t frSkyDataIdTable[] = { static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port. static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool smartPortTelemetryEnabled = false; static portSharing_e smartPortPortSharing; @@ -302,9 +306,13 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortSendPackageEx(FSSP_DATA_FRAME,payload); } -void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initSmartPortTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT); smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT); } @@ -326,11 +334,11 @@ void configureSmartPortTelemetryPort(void) portOptions_t portOptions = 0; - if (telemetryConfig->sportHalfDuplex) { + if (telemetryConfig()->sportHalfDuplex) { portOptions |= SERIAL_BIDIR; } - if (telemetryConfig->telemetry_inversion) { + if (telemetryConfig()->telemetry_inversion) { portOptions |= SERIAL_INVERTED; } @@ -622,7 +630,7 @@ void handleSmartPortTelemetry(void) case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT) && batteryCellCount > 0) { uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = getVbat() / batteryCellCount; } else { vfasVoltage = getVbat(); @@ -752,7 +760,7 @@ void handleSmartPortTelemetry(void) } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig()->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index d50c1fd575..e7d7722b2c 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,7 +7,7 @@ #pragma once -void initSmartPortTelemetry(telemetryConfig_t *); +void initSmartPortTelemetry(const telemetryConfig_t *); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 928acdc2b7..0d635d793b 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -50,29 +53,35 @@ #include "telemetry/srxl.h" #include "telemetry/ibus.h" +#ifndef USE_PARAMETER_GROUPS static telemetryConfig_t *telemetryConfig; +#endif void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(telemetryConfigToUse); +#else telemetryConfig = telemetryConfigToUse; +#endif } void telemetryInit(void) { #ifdef TELEMETRY_FRSKY - initFrSkyTelemetry(telemetryConfig); + initFrSkyTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_HOTT - initHoTTTelemetry(telemetryConfig); + initHoTTTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_SMARTPORT - initSmartPortTelemetry(telemetryConfig); + initSmartPortTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_LTM - initLtmTelemetry(telemetryConfig); + initLtmTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_JETIEXBUS - initJetiExBusTelemetry(telemetryConfig); + initJetiExBusTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); @@ -95,7 +104,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) bool enabled = portSharing == PORTSHARING_NOT_SHARED; if (portSharing == PORTSHARING_SHARED) { - if (telemetryConfig->telemetry_switch) + if (telemetryConfig()->telemetry_switch) enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY); else enabled = ARMING_FLAG(ARMED); @@ -142,7 +151,7 @@ void telemetryCheckState(void) #endif } -void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void telemetryProcess(uint32_t currentTime, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { #ifdef TELEMETRY_FRSKY handleFrSkyTelemetry(rxConfig, deadband3d_throttle); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index aa60912147..34f59f2711 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -24,6 +24,9 @@ #pragma once +#include "config/parameter_group.h" +#include "io/serial.h" + typedef enum { FRSKY_FORMAT_DMS = 0, FRSKY_FORMAT_NMEA @@ -49,13 +52,15 @@ typedef struct telemetryConfig_s { uint8_t report_cell_voltage; } telemetryConfig_t; +PG_DECLARE(telemetryConfig_t, telemetryConfig); + void telemetryInit(void); bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); extern serialPort_t *telemetrySharedPort; void telemetryCheckState(void); struct rxConfig_s; -void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void telemetryProcess(uint32_t currentTime, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); bool telemetryDetermineEnabledState(portSharing_e portSharing); diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index b839912c00..a6b038441a 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -26,6 +26,7 @@ #define BARO extern "C" { + #include "platform.h"" #include "target.h" #include "drivers/display.h" #include "cms/cms.h" diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc index a48e98e055..82c857e38a 100644 --- a/src/test/unit/parameter_groups_unittest.cc +++ b/src/test/unit/parameter_groups_unittest.cc @@ -29,7 +29,7 @@ extern "C" { #include "io/motors.h" -PG_DECLARE(motorConfig_t, motorConfig); +//PG_DECLARE(motorConfig_t, motorConfig); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index c5c400aa85..6b5f77acff 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,8 @@ #pragma once +#define USE_PARAMETER_GROUPS + #define U_ID_0 0 #define U_ID_1 1 #define U_ID_2 2 @@ -30,8 +32,6 @@ #define USE_SERVOS #define TRANSPONDER -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - typedef enum { Mode_TEST = 0x0, diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index cfe99b7943..a5c4b8faa4 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -22,10 +22,10 @@ #include extern "C" { - #include "build/debug.h" - #include + #include "build/debug.h" + #include "common/axis.h" #include "common/filter.h" #include "common/gps_conversion.h" @@ -56,6 +56,8 @@ extern "C" { bool airMode; uint16_t vbat; serialPort_t *telemetrySharedPort; + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" @@ -135,10 +137,7 @@ TEST(TelemetryCrsfTest, TestGPS) TEST(TelemetryCrsfTest, TestBattery) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - batteryConfig_t workingBatteryConfig; - batteryConfig = &workingBatteryConfig; - memset(batteryConfig, 0, sizeof(batteryConfig_t)); vbat = 0; // 0.1V units int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); @@ -157,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery) vbat = 33; // 3.3V = 3300 mv amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps - batteryConfig->batteryCapacity = 1234; + batteryConfigMutable()->batteryCapacity = 1234; frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index bc0d60ad01..54caa75e99 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -29,6 +29,10 @@ extern "C" { #include "common/axis.h" #include "common/gps_conversion.h" + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/system.h" @@ -45,6 +49,8 @@ extern "C" { #include "telemetry/telemetry.h" #include "telemetry/hott.h" + + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h"