diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5b31dd5dbe..00795d43e4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -94,6 +94,9 @@ #include "telemetry/telemetry.h" +#ifndef DEFAULT_FEATURES +#define DEFAULT_FEATURES 0 +#endif #ifndef DEFAULT_RX_FEATURE #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM #endif @@ -104,6 +107,25 @@ #define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHLESS_MOTORS_PWM_RATE 480 +PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); + +PG_RESET_TEMPLATE(featureConfig_t, featureConfig, + .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_FAILSAFE +); + +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + +PG_RESET_TEMPLATE(systemConfig_t, systemConfig, + .current_profile_index = 0, + //!!TODO.activeRateProfile = 0, + .debug_mode = DEBUG_MODE, + .task_statistics = true, + .name = { 0 } +); + +PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); + + master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; @@ -358,7 +380,7 @@ void resetAdcConfig(adcConfig_t *adcConfig) #ifdef BEEPER -void resetBeeperConfig(beeperDevConfig_t *beeperDevConfig) +void resetBeeperDevConfig(beeperDevConfig_t *beeperDevConfig) { #ifdef BEEPER_INVERTED beeperDevConfig->isOpenDrain = false; @@ -400,6 +422,7 @@ void resetPwmConfig(pwmConfig_t *pwmConfig) } #endif +#ifndef USE_PARAMETER_GROUPS void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) { flight3DConfig->deadband3d_low = 1406; @@ -407,7 +430,9 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) flight3DConfig->neutral3d = 1460; flight3DConfig->deadband3d_throttle = 50; } +#endif +#ifndef USE_PARAMETER_GROUPS #ifdef TELEMETRY void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { @@ -427,6 +452,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) #endif } #endif +#endif #ifndef USE_PARAMETER_GROUPS void resetBatteryConfig(batteryConfig_t *batteryConfig) @@ -627,6 +653,7 @@ void resetSerialPinConfig(serialPinConfig_t *pSerialPinConfig) #define SECOND_PORT_INDEX 1 #endif +#ifndef USE_PARAMETER_GROUPS void resetSerialConfig(serialConfig_t *serialConfig) { memset(serialConfig, 0, sizeof(serialConfig_t)); @@ -647,6 +674,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) serialConfig->portConfigs[1].functionMask = FUNCTION_MSP; #endif } +#endif void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { @@ -656,6 +684,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) rcControlsConfig->alt_hold_fast_change = 1; } +#ifndef USE_PARAMETER_GROUPS void resetMixerConfig(mixerConfig_t *mixerConfig) { #ifdef TARGET_DEFAULT_MIXER @@ -665,6 +694,7 @@ void resetMixerConfig(mixerConfig_t *mixerConfig) #endif mixerConfig->yaw_motor_direction = 1; } +#endif #ifdef USE_MAX7456 void resetMax7456Config(vcdProfile_t *pVcdProfile) @@ -795,7 +825,13 @@ void createDefaultConfig(master_t *config) config->version = EEPROM_CONF_VERSION; // global settings +#ifndef USE_PARAMETER_GROUPS config->systemConfig.current_profile_index = 0; // default profile + config->systemConfig.debug_mode = DEBUG_MODE; + config->systemConfig.task_statistics = true; +#endif + + config->imuConfig.dcm_kp = 2500; // 1.0 * 10000 config->imuConfig.dcm_ki = 0; // 0.003 * 10000 #ifndef USE_PARAMETER_GROUPS @@ -820,11 +856,6 @@ void createDefaultConfig(master_t *config) config->gyroConfig.gyroMovementCalibrationThreshold = 48; #endif - config->systemConfig.debug_mode = DEBUG_MODE; - config->systemConfig.task_statistics = true; - - - #ifndef USE_PARAMETER_GROUPS resetCompassConfig(&config->compassConfig); #endif @@ -836,9 +867,11 @@ void createDefaultConfig(master_t *config) resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims); config->accelerometerConfig.acc_lpf_hz = 10.0f; #endif +#ifndef USE_PARAMETER_GROUPS config->boardAlignment.rollDegrees = 0; config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; +#endif config->rcControlsConfig.yaw_control_direction = 1; // xxx_hardware: 0:default/autodetect, 1: disable @@ -864,7 +897,7 @@ void createDefaultConfig(master_t *config) #endif #ifdef BEEPER - resetBeeperConfig(&config->beeperDevConfig); + resetBeeperDevConfig(&config->beeperDevConfig); #endif #ifdef SONAR @@ -916,20 +949,27 @@ void createDefaultConfig(master_t *config) config->pwmConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; #endif +#ifndef USE_PARAMETER_GROUPS config->armingConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support config->armingConfig.disarm_kill_switch = 1; config->armingConfig.auto_disarm_delay = 5; +#endif + config->imuConfig.small_angle = 25; config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo +#ifndef USE_PARAMETER_GROUPS resetMixerConfig(&config->mixerConfig); +#endif resetMotorConfig(&config->motorConfig); #ifdef USE_SERVOS resetServoConfig(&config->servoConfig); #endif +#ifndef USE_PARAMETER_GROUPS resetFlight3DConfig(&config->flight3DConfig); +#endif #ifdef LED_STRIP resetLedStripConfig(&config->ledStripConfig); @@ -945,7 +985,9 @@ void createDefaultConfig(master_t *config) resetSerialPinConfig(&config->serialPinConfig); +#ifndef USE_PARAMETER_GROUPS resetSerialConfig(&config->serialConfig); +#endif resetProfile(&config->profile[0]); @@ -973,6 +1015,7 @@ void createDefaultConfig(master_t *config) config->throttleCorrectionConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv config->throttleCorrectionConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv +#ifndef USE_PARAMETER_GROUPS // Failsafe Variables config->failsafeConfig.failsafe_delay = 10; // 1sec config->failsafeConfig.failsafe_off_delay = 10; // 1sec @@ -980,6 +1023,7 @@ void createDefaultConfig(master_t *config) config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition config->failsafeConfig.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;// default full failsafe procedure is 0: auto-landing +#endif #ifdef USE_SERVOS // servos @@ -1165,11 +1209,12 @@ void validateAndFixConfig(void) } #endif +#ifndef USE_PARAMETER_GROUPS serialConfig_t *serialConfig = &masterConfig.serialConfig; - if (!isSerialConfigValid(serialConfig)) { resetSerialConfig(serialConfig); } +#endif validateAndFixGyroConfig(); @@ -1267,7 +1312,7 @@ void readEEPROM(void) // setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); if (systemConfig()->current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check - systemConfig()->current_profile_index = 0; + systemConfigMutable()->current_profile_index = 0; } setProfile(systemConfig()->current_profile_index); @@ -1313,7 +1358,7 @@ void changeProfile(uint8_t profileIndex) if (profileIndex >= MAX_PROFILE_COUNT) { profileIndex = MAX_PROFILE_COUNT - 1; } - systemConfig()->current_profile_index = profileIndex; + systemConfigMutable()->current_profile_index = profileIndex; writeEEPROM(); readEEPROM(); beeperConfirmationBeeps(profileIndex + 1); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8cba180f3b..909bc335db 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -70,6 +70,14 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e +PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0); + +PG_RESET_TEMPLATE(armingConfig_t, armingConfig, + .gyro_cal_on_first_arm = 0, // TODO - Cleanup retarded arm support + .disarm_kill_switch = 1, + .auto_disarm_delay = 5 +); + bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index ba604c309c..44e8dcd6d2 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -53,6 +53,17 @@ static failsafeState_t failsafeState; +PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); + +PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, + .failsafe_delay = 10, // 1sec + .failsafe_off_delay = 10, // 1sec + .failsafe_throttle = 1000, // default throttle off. + .failsafe_kill_switch = 0, // default failsafe switch action is identical to rc link loss + .failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition + .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing +); + /* * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. */ diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5c2a44a1ad..ad75905ca9 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -50,6 +50,25 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); + +PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, + .deadband3d_low = 1406, + .deadband3d_high = 1514, + .neutral3d = 1460, + .deadband3d_throttle = 50 +); + +PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); + +#ifndef TARGET_DEFAULT_MIXER +#define TARGET_DEFAULT_MIXER MIXER_QUADX +#endif +PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, + .mixerMode = TARGET_DEFAULT_MIXER, + .yaw_motor_direction = 1, +); + #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 3890794c64..5097d12c8c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -99,6 +99,34 @@ const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 2500 #define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0])) +PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 0); + +void pgResetFn_serialConfig(serialConfig_t *serialConfig) +{ + memset(serialConfig, 0, sizeof(serialConfig_t)); + + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + serialConfig->portConfigs[i].identifier = serialPortIdentifiers[i]; + serialConfig->portConfigs[i].msp_baudrateIndex = BAUD_115200; + serialConfig->portConfigs[i].gps_baudrateIndex = BAUD_38400; + serialConfig->portConfigs[i].telemetry_baudrateIndex = BAUD_AUTO; + serialConfig->portConfigs[i].blackbox_baudrateIndex = BAUD_115200; + } + + serialConfig->portConfigs[0].functionMask = FUNCTION_MSP; + +#ifdef USE_VCP + if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) { + serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1); + if (uart1Config) { + uart1Config->functionMask = FUNCTION_MSP; + } + } +#endif + + serialConfig->reboot_character = 'R'; +} + baudRate_e lookupBaudRateIndex(uint32_t baudRate) { uint8_t index; diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 0b45b8ffe8..f62fa33f71 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -33,6 +33,9 @@ static bool standardBoardAlignment = true; // board orientation correction static float boardRotation[3][3]; // matrix +// no template required since defaults are zero +PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0); + static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment) { return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index f1cda235a9..5adb095838 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -54,6 +54,29 @@ #include "telemetry/ibus.h" +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + +#if defined(STM32F3) +#define TELEMETRY_DEFAULT_INVERSION 1 +#else +#define TELEMETRY_DEFAULT_INVERSION 0 +#endif + +PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, + .telemetry_inversion = TELEMETRY_DEFAULT_INVERSION, + .sportHalfDuplex = 1, + .telemetry_switch = 0, + .gpsNoFixLatitude = 0, + .gpsNoFixLongitude = 0, + .frsky_coordinate_format = FRSKY_FORMAT_DMS, + .frsky_unit = FRSKY_UNIT_METRICS, + .frsky_vfas_precision = 0, + .frsky_vfas_cell_voltage = 0, + .hottAlarmSoundInterval = 5, + .pidValuesAsTelemetry = 0, + .report_cell_voltage = false +); + void telemetryInit(void) { #ifdef TELEMETRY_FRSKY