diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 15e28cf88d..e431547d79 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -956,20 +956,18 @@ static int gcd(int num, int denom) static void validateBlackboxConfig() { - int div; - 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); + const int 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 @@ -985,7 +983,7 @@ static void validateBlackboxConfig() break; default: - blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL; + blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL; } } diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 73bca8f614..44a74b772d 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -441,7 +441,7 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) UNUSED(self); masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz; - gyroConfig()->gyro_soft_lpf_hz = cmsx_gyroSoftLpf; + gyroConfigMutable()->gyro_soft_lpf_hz = cmsx_gyroSoftLpf; masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit; masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz; @@ -487,9 +487,9 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { UNUSED(self); - gyroConfig()->gyroSync = cmsx_gyroSync; - gyroConfig()->gyroSyncDenominator = cmsx_gyroSyncDenom; - gyroConfig()->gyro_lpf = cmsx_gyroLpf; + gyroConfigMutable()->gyroSync = cmsx_gyroSync; + gyroConfigMutable()->gyroSyncDenominator = cmsx_gyroSyncDenom; + gyroConfigMutable()->gyro_lpf = cmsx_gyroLpf; return 0; } diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index f1d9553c8a..060d8c7910 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -53,21 +53,37 @@ #include "telemetry/telemetry.h" #define flight3DConfig(x) (&masterConfig.flight3DConfig) +#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig) #define servoConfig(x) (&masterConfig.servoConfig) +#define servoConfigMutable(x) (&masterConfig.servoConfig) #define servoMixerConfig(x) (&masterConfig.servoMixerConfig) +#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig) #define imuConfig(x) (&masterConfig.imuConfig) +#define imuConfigMutable(x) (&masterConfig.imuConfig) #define batteryConfig(x) (&masterConfig.batteryConfig) +#define batteryConfigMutable(x) (&masterConfig.batteryConfig) #define rcControlsConfig(x) (&masterConfig.rcControlsConfig) +#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig) #define gpsConfig(x) (&masterConfig.gpsConfig) +#define gpsConfigMutable(x) (&masterConfig.gpsConfig) #define navConfig(x) (&masterConfig.navConfig) +#define navConfigMutable(x) (&masterConfig.navConfig) #define armingConfig(x) (&masterConfig.armingConfig) +#define armingConfigMutable(x) (&masterConfig.armingConfig) #define mixerConfig(x) (&masterConfig.mixerConfig) +#define mixerConfigMutable(x) (&masterConfig.mixerConfig) #define serialConfig(x) (&masterConfig.serialConfig) +#define serialConfigMutable(x) (&masterConfig.serialConfig) #define telemetryConfig(x) (&masterConfig.telemetryConfig) +#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig) #define osdProfile(x) (&masterConfig.osdProfile) +#define osdProfileMutable(x) (&masterConfig.osdProfile) #define ledStripConfig(x) (&masterConfig.ledStripConfig) +#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig) #define pwmRxConfig(x) (&masterConfig.pwmRxConfig) +#define pwmRxConfigMutable(x) (&masterConfig.pwmRxConfig) #define customServoMixer(i) (&masterConfig.customServoMixer[i]) +#define customServoMixerMutable(i) (&masterConfig.customServoMixer[i]) // System-wide diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index b2f74da411..abdeca250e 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -102,14 +102,16 @@ extern const uint8_t __pg_resetdata_end[]; // Declare system config #define PG_DECLARE(_type, _name) \ extern _type _name ## _System; \ - static inline _type* _name(void) { return &_name ## _System; } \ + static inline const _type* _name(void) { return &_name ## _System; }\ + static inline _type* _name ## Mutable(void) { return &_name ## _System; }\ struct _dummy \ /**/ // Declare system config array #define PG_DECLARE_ARR(_type, _size, _name) \ extern _type _name ## _SystemArray[_size]; \ - static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \ static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \ struct _dummy \ /**/ @@ -117,7 +119,8 @@ extern const uint8_t __pg_resetdata_end[]; // Declare profile config #define PG_DECLARE_PROFILE(_type, _name) \ extern _type *_name ## _ProfileCurrent; \ - static inline _type* _name(void) { return _name ## _ProfileCurrent; } \ + static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \ + static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \ struct _dummy \ /**/ diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2e12b3d26e..146497a74b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -671,12 +671,12 @@ void validateAndFixConfig(void) { #ifdef USE_GYRO_NOTCH_1 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; } #endif #ifdef USE_GYRO_NOTCH_2 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; } #endif #ifdef USE_DTERM_NOTCH @@ -726,8 +726,8 @@ void validateAndFixConfig(void) // There is a timer clash between PWM RX pins and motor output pins - this forces us to have same timer tick rate for these timers // which is only possible when using brushless motors w/o oneshot (timer tick rate is PWM_TIMER_MHZ) // On CC3D OneShot is incompatible with PWM RX - motorConfig()->motorPwmProtocol = PWM_TYPE_STANDARD; - motorConfig()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; + motorConfigMutable()->motorPwmProtocol = PWM_TYPE_STANDARD; + motorConfigMutable()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; #endif #endif @@ -760,7 +760,7 @@ void validateAndFixConfig(void) * When async processing mode is enabled, gyroSync has to be forced to "ON" */ if (getAsyncMode() != ASYNC_MODE_NONE) { - gyroConfig()->gyroSync = 1; + gyroConfigMutable()->gyroSync = 1; } #endif @@ -777,11 +777,11 @@ void validateAndFixConfig(void) } if (gyroConfig()->gyroSyncDenominator < denominatorLimit) { - gyroConfig()->gyroSyncDenominator = denominatorLimit; + gyroConfigMutable()->gyroSyncDenominator = denominatorLimit; } if (gyroConfig()->looptime < 2000) { - gyroConfig()->looptime = 2000; + gyroConfigMutable()->looptime = 2000; } } @@ -855,7 +855,7 @@ void validateAndFixConfig(void) * If provided predefined mixer setup is disabled, fallback to default one */ if (!isMixerEnabled(mixerConfig()->mixerMode)) { - mixerConfig()->mixerMode = DEFAULT_MIXER; + mixerConfigMutable()->mixerMode = DEFAULT_MIXER; } #if defined(NAV) @@ -865,26 +865,26 @@ void validateAndFixConfig(void) /* Limitations of different protocols */ #ifdef BRUSHED_MOTORS - motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000); + motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000); #else switch (motorConfig()->motorPwmProtocol) { case PWM_TYPE_STANDARD: // Limited to 490 Hz - motorConfig()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 490); + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 490); break; case PWM_TYPE_ONESHOT125: // Limited to 3900 Hz - motorConfig()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 3900); + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 3900); break; case PWM_TYPE_ONESHOT42: // 2-8 kHz - motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 8000); + motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 8000); break; case PWM_TYPE_MULTISHOT: // 2-16 kHz - motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 16000); + motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 16000); break; case PWM_TYPE_BRUSHED: // 500Hz - 32kHz - motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000); + motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000); break; } #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 3cc78c5a16..ffc05d7648 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -214,7 +214,7 @@ void init(void) // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() - spektrumBind(rxConfig()); + spektrumBind(rxConfigMutable()); break; } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0c2624f3e0..09e540c964 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1283,12 +1283,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *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: - gyroConfig()->looptime = sbufReadU16(src); + gyroConfigMutable()->looptime = sbufReadU16(src); break; case MSP_SET_PID_CONTROLLER: @@ -1379,37 +1379,37 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_MISC: tmp = sbufReadU16(src); if (tmp < 1600 && tmp > 1400) - rxConfig()->midrc = tmp; + rxConfigMutable()->midrc = tmp; - motorConfig()->minthrottle = sbufReadU16(src); - motorConfig()->maxthrottle = sbufReadU16(src); - motorConfig()->mincommand = 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); #ifdef MAG - compassConfig()->mag_declination = sbufReadU16(src) * 10; + compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; #else sbufReadU16(src); #endif - 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: @@ -1461,16 +1461,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_3D: - flight3DConfig()->deadband3d_low = sbufReadU16(src); - flight3DConfig()->deadband3d_high = sbufReadU16(src); - flight3DConfig()->neutral3d = sbufReadU16(src); - flight3DConfig()->deadband3d_throttle = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_low = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_high = sbufReadU16(src); + flight3DConfigMutable()->neutral3d = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RC_DEADBAND: - rcControlsConfig()->deadband = sbufReadU8(src); - rcControlsConfig()->yaw_deadband = sbufReadU8(src); - rcControlsConfig()->alt_hold_deadband = sbufReadU8(src); + rcControlsConfigMutable()->deadband = sbufReadU8(src); + rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); + rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); break; case MSP_SET_RESET_CURR_PID: @@ -1478,36 +1478,36 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_ALIGNMENT: - gyroConfig()->gyro_align = sbufReadU8(src); - accelerometerConfig()->acc_align = sbufReadU8(src); + gyroConfigMutable()->gyro_align = sbufReadU8(src); + accelerometerConfigMutable()->acc_align = sbufReadU8(src); #ifdef MAG - compassConfig()->mag_align = sbufReadU8(src); + compassConfigMutable()->mag_align = sbufReadU8(src); #else sbufReadU8(src); #endif break; case MSP_SET_ADVANCED_CONFIG: - gyroConfig()->gyroSyncDenominator = sbufReadU8(src); + gyroConfigMutable()->gyroSyncDenominator = sbufReadU8(src); sbufReadU8(src); // BF: masterConfig.pid_process_denom sbufReadU8(src); // BF: motorConfig()->useUnsyncedPwm - motorConfig()->motorPwmProtocol = sbufReadU8(src); - motorConfig()->motorPwmRate = sbufReadU16(src); + motorConfigMutable()->motorPwmProtocol = sbufReadU8(src); + motorConfigMutable()->motorPwmRate = sbufReadU16(src); #ifdef USE_SERVOS - servoConfig()->servoPwmRate = sbufReadU16(src); + servoConfigMutable()->servoPwmRate = sbufReadU16(src); #else sbufReadU16(src); #endif - gyroConfig()->gyroSync = sbufReadU8(src); + gyroConfigMutable()->gyroSync = sbufReadU8(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 = constrain(sbufReadU16(src), 0, 255); currentProfile->pidProfile.yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255); #ifdef USE_GYRO_NOTCH_1 - gyroConfig()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500); - gyroConfig()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500); + gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500); + gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500); #endif #ifdef USE_DTERM_NOTCH currentProfile->pidProfile.dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500); @@ -1515,8 +1515,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) pidInitFilters(¤tProfile->pidProfile); #endif #ifdef USE_GYRO_NOTCH_2 - gyroConfig()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500); - gyroConfig()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500); + gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500); + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500); #endif //BF: masterConfig.gyro_soft_notch_hz_1 = read16(); //BF: masterConfig.gyro_soft_notch_cutoff_1 = read16(); @@ -1558,14 +1558,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif #ifdef MAG - compassConfig()->mag_hold_rate_limit = sbufReadU8(src); + compassConfigMutable()->mag_hold_rate_limit = sbufReadU8(src); sbufReadU8(src); //MAG_HOLD_ERROR_LPF_FREQ #else sbufReadU8(src); sbufReadU8(src); #endif - mixerConfig()->yaw_jump_prevention_limit = sbufReadU16(src); - gyroConfig()->gyro_lpf = sbufReadU8(src); + mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src); + gyroConfigMutable()->gyro_lpf = sbufReadU8(src); currentProfile->pidProfile.acc_soft_lpf_hz = sbufReadU8(src); sbufReadU8(src); //reserved sbufReadU8(src); //reserved @@ -1574,19 +1574,19 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - accelerometerConfig()->acc_hardware = sbufReadU8(src); + accelerometerConfigMutable()->acc_hardware = sbufReadU8(src); #ifdef BARO - barometerConfig()->baro_hardware = sbufReadU8(src); + barometerConfigMutable()->baro_hardware = sbufReadU8(src); #else sbufReadU8(src); #endif #ifdef MAG - compassConfig()->mag_hardware = sbufReadU8(src); + compassConfigMutable()->mag_hardware = sbufReadU8(src); #else sbufReadU8(src); #endif #ifdef PITOT - pitotmeterConfig()->pitot_hardware = sbufReadU8(src); + pitotmeterConfigMutable()->pitot_hardware = sbufReadU8(src); #else sbufReadU8(src); #endif @@ -1596,14 +1596,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef NAV case MSP_SET_NAV_POSHOLD: - navConfig()->general.flags.user_control_mode = sbufReadU8(src); - navConfig()->general.max_speed = sbufReadU16(src); - navConfig()->general.max_climb_rate = sbufReadU16(src); - navConfig()->general.max_manual_speed = sbufReadU16(src); - navConfig()->general.max_manual_climb_rate = sbufReadU16(src); - navConfig()->mc.max_bank_angle = sbufReadU8(src); - navConfig()->general.flags.use_thr_mid_for_althold = sbufReadU8(src); - navConfig()->mc.hover_throttle = sbufReadU16(src); + navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); + navConfigMutable()->general.max_speed = sbufReadU16(src); + navConfigMutable()->general.max_climb_rate = sbufReadU16(src); + navConfigMutable()->general.max_manual_speed = sbufReadU16(src); + navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); + navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); + navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src); + navConfigMutable()->mc.hover_throttle = sbufReadU16(src); break; #endif @@ -1636,9 +1636,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 @@ -1743,40 +1743,40 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_BOARD_ALIGNMENT: - boardAlignment()->rollDeciDegrees = sbufReadU16(src); - boardAlignment()->pitchDeciDegrees = sbufReadU16(src); - boardAlignment()->yawDeciDegrees = sbufReadU16(src); + boardAlignmentMutable()->rollDeciDegrees = sbufReadU16(src); + boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); + boardAlignmentMutable()->yawDeciDegrees = 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 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) { // for compatibility with betaflight @@ -1785,45 +1785,45 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) sbufReadU16(src); } if (dataSize > 16) { - rxConfig()->rx_spi_protocol = sbufReadU8(src); + rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); } if (dataSize > 17) { - rxConfig()->rx_spi_id = sbufReadU32(src); + rxConfigMutable()->rx_spi_id = sbufReadU32(src); } if (dataSize > 21) { - rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src); + rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(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); if (dataSize > 8) { - failsafeConfig()->failsafe_recovery_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_recovery_delay = sbufReadU8(src); } break; case MSP_SET_RXFAIL_CONFIG: i = sbufReadU8(src); if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { - rxFailsafeChannelConfigs(i)->mode = sbufReadU8(src); - rxFailsafeChannelConfigs(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); + rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src); + rxFailsafeChannelConfigsMutable(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; @@ -1831,20 +1831,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()->rollDeciDegrees = sbufReadU16(src); // board_align_roll - boardAlignment()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch - boardAlignment()->yawDeciDegrees = sbufReadU16(src); // board_align_yaw + boardAlignmentMutable()->rollDeciDegrees = sbufReadU16(src); // board_align_roll + boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch + boardAlignmentMutable()->yawDeciDegrees = 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: diff --git a/src/main/fc/serial_cli.c b/src/main/fc/serial_cli.c index 08915c40ea..54cefa4f2b 100644 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -1048,7 +1048,7 @@ static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptr } #ifdef USE_PARAMETER_GROUPS -static void dumpPgValues(uint16_t valueSection, uint8_t dumpMask, pgn_t pgn, void *currentConfig, void *defaultConfig) +static void dumpPgValues(uint16_t valueSection, uint8_t dumpMask, pgn_t pgn, const void *currentConfig, const void *defaultConfig) { const char *format = "set %s = "; for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { @@ -1129,31 +1129,31 @@ static void backupConfigs(void) static void restoreConfigs(void) { - *gyroConfig() = gyroConfigCopy; - *accelerometerConfig() = accelerometerConfigCopy; + *gyroConfigMutable() = gyroConfigCopy; + *accelerometerConfigMutable() = accelerometerConfigCopy; #ifdef MAG - *compassConfig() = compassConfigCopy; + *compassConfigMutable() = compassConfigCopy; #endif #ifdef BARO - *barometerConfig() = barometerConfigCopy; + *barometerConfigMutable() = barometerConfigCopy; #endif #ifdef PITOT - *pitotmeterConfig() = pitotmeterConfigCopy; + *pitotmeterConfigMutable() = pitotmeterConfigCopy; #endif - *rxConfig() = rxConfigCopy; + *rxConfigMutable() = rxConfigCopy; for (int ii = 0; ii < MAX_SUPPORTED_RC_CHANNEL_COUNT; ++ii) { - *rxFailsafeChannelConfigs(ii) = rxFailsafeChannelConfigsCopy[ii]; + *rxFailsafeChannelConfigsMutable(ii) = rxFailsafeChannelConfigsCopy[ii]; } for (int ii = 0; ii < NON_AUX_CHANNEL_COUNT; ++ii) { - *rxChannelRangeConfigs(ii) = rxChannelRangeConfigsCopy[ii]; + *rxChannelRangeConfigsMutable(ii) = rxChannelRangeConfigsCopy[ii]; } - *motorConfig() = motorConfigCopy; - *boardAlignment() = boardAlignmentCopy; + *motorConfigMutable() = motorConfigCopy; + *boardAlignmentMutable() = boardAlignmentCopy; #ifdef USE_SERVOS - *gimbalConfig() = gimbalConfigCopy; + *gimbalConfigMutable() = gimbalConfigCopy; #endif for (int ii = 0; ii < MAX_SUPPORTED_MOTORS; ++ii) { - *customMotorMixer(ii) = customMotorMixerCopy[ii]; + *customMotorMixerMutable(ii) = customMotorMixerCopy[ii]; } } @@ -1381,7 +1381,8 @@ static void cliRxFail(char *cmdline) channel = atoi(ptr++); if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) { - rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel); + // const cast + rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigsMutable(channel); uint16_t value; rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX; @@ -1849,8 +1850,9 @@ static void cliMotorMix(char *cmdline) printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); } 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; + for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + customMotorMixerMutable(i)->throttle = 0.0f; + } } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = nextArg(cmdline); if (ptr) { @@ -1861,7 +1863,7 @@ static void cliMotorMix(char *cmdline) break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { - mixerLoadMix(i, customMotorMixer(0)); + mixerLoadMix(i, customMotorMixerMutable(0)); cliPrintf("Loaded %s\r\n", mixerNames[i]); cliMotorMix(""); break; @@ -1874,22 +1876,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) { @@ -1958,7 +1960,7 @@ static void cliRxRange(char *cmdline) } else if (rangeMin < PWM_PULSE_MIN || rangeMin > PWM_PULSE_MAX || rangeMax < PWM_PULSE_MIN || rangeMax > PWM_PULSE_MAX) { cliShowParseError(); } else { - rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigs(i); + rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigsMutable(i); channelRangeConfig->min = rangeMin; channelRangeConfig->max = rangeMax; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 243354492b..edc4a3315a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -340,7 +340,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) // we're 1-based index++; // clear existing - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) customMixers[i].throttle = 0.0f; // do we have anything here to begin with? diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 82229a0f8d..54356425c0 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -2473,32 +2473,32 @@ void updateWaypointsAndNavigationMode(void) /*----------------------------------------------------------- * NAV main control functions *-----------------------------------------------------------*/ -void navigationUseConfig(navConfig_t *navConfigToUse) +void navigationUseConfig(const navConfig_t *navConfigToUse) { posControl.navConfig = navConfigToUse; } -void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig) +void navigationUseRcControlsConfig(const rcControlsConfig_t *initialRcControlsConfig) { posControl.rcControlsConfig = initialRcControlsConfig; } -void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig) +void navigationUseFlight3DConfig(const flight3DConfig_t * initialFlight3DConfig) { posControl.flight3DConfig = initialFlight3DConfig; } -void navigationUseRxConfig(rxConfig_t * initialRxConfig) +void navigationUseRxConfig(const rxConfig_t * initialRxConfig) { posControl.rxConfig = initialRxConfig; } -void navigationUsemotorConfig(motorConfig_t * initialmotorConfig) +void navigationUsemotorConfig(const motorConfig_t * initialmotorConfig) { posControl.motorConfig = initialmotorConfig; } -void navigationUsePIDs(pidProfile_t *initialPidProfile) +void navigationUsePIDs(const pidProfile_t *initialPidProfile) { posControl.pidProfile = initialPidProfile; @@ -2539,12 +2539,12 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile) (float)posControl.pidProfile->D8[PIDALT] / 100.0f); } -void navigationInit(navConfig_t *initialnavConfig, - pidProfile_t *initialPidProfile, - rcControlsConfig_t *initialRcControlsConfig, - rxConfig_t * initialRxConfig, - flight3DConfig_t * initialFlight3DConfig, - motorConfig_t * initialmotorConfig) +void navigationInit(const navConfig_t *initialnavConfig, + const pidProfile_t *initialPidProfile, + const rcControlsConfig_t *initialRcControlsConfig, + const rxConfig_t * initialRxConfig, + const flight3DConfig_t * initialFlight3DConfig, + const motorConfig_t * initialmotorConfig) { /* Initial state */ posControl.navState = NAV_STATE_IDLE; diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index b55abcf52e..1f81778e13 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -233,18 +233,18 @@ typedef struct { navWaypointActions_e activeWpAction; } navSystemStatus_t; -void navigationUsePIDs(pidProfile_t *pidProfile); -void navigationUseConfig(navConfig_t *navConfigToUse); -void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig); -void navigationUseRxConfig(rxConfig_t * initialRxConfig); -void navigationUsemotorConfig(motorConfig_t * initialmotorConfig); -void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig); -void navigationInit(navConfig_t *initialnavConfig, - pidProfile_t *initialPidProfile, - rcControlsConfig_t *initialRcControlsConfig, - rxConfig_t * initialRxConfig, - flight3DConfig_t * initialFlight3DConfig, - motorConfig_t * initialmotorConfig); +void navigationUsePIDs(const pidProfile_t *pidProfile); +void navigationUseConfig(const navConfig_t *navConfigToUse); +void navigationUseRcControlsConfig(const rcControlsConfig_t *initialRcControlsConfig); +void navigationUseRxConfig(const rxConfig_t * initialRxConfig); +void navigationUsemotorConfig(const motorConfig_t * initialmotorConfig); +void navigationUseFlight3DConfig(const flight3DConfig_t * initialFlight3DConfig); +void navigationInit(const navConfig_t *initialnavConfig, + const pidProfile_t *initialPidProfile, + const rcControlsConfig_t *initialRcControlsConfig, + const rxConfig_t * initialRxConfig, + const flight3DConfig_t * initialFlight3DConfig, + const motorConfig_t * initialmotorConfig); /* Navigation system updates */ void updateWaypointsAndNavigationMode(void); diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 8a5071d90b..98421741c2 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -286,12 +286,12 @@ typedef struct { /* Internals */ int16_t rcAdjustment[4]; - navConfig_t * navConfig; - rcControlsConfig_t * rcControlsConfig; - pidProfile_t * pidProfile; - rxConfig_t * rxConfig; - flight3DConfig_t * flight3DConfig; - motorConfig_t * motorConfig; + const navConfig_t * navConfig; + const rcControlsConfig_t * rcControlsConfig; + const pidProfile_t * pidProfile; + const rxConfig_t * rxConfig; + const flight3DConfig_t * flight3DConfig; + const motorConfig_t * motorConfig; } navigationPosControl_t; extern navigationPosControl_t posControl; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f540721492..219669ba6a 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -122,8 +122,8 @@ void resetAllRxChannelRangeConfigurations(void) { // set default calibration to full range and 1:1 mapping for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - rxChannelRangeConfigs(i)->min = PWM_RANGE_MIN; - rxChannelRangeConfigs(i)->max = PWM_RANGE_MAX; + rxChannelRangeConfigsMutable(i)->min = PWM_RANGE_MIN; + rxChannelRangeConfigsMutable(i)->max = PWM_RANGE_MAX; } } @@ -523,7 +523,7 @@ static uint16_t getRxfailValue(uint8_t channel) } } -STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig_t *range) +STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range) { // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT if (sample == PPM_RCVR_TIMEOUT) { @@ -639,12 +639,10 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) void parseRcChannels(const char *input) { - const char *c, *s; - - for (c = input; *c; c++) { - s = strchr(rcChannelLetters, *c); + for (const char *c = input; *c; c++) { + const char *s = strchr(rcChannelLetters, *c); if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS)) - rxConfig()->rcmap[s - rcChannelLetters] = c - input; + rxConfigMutable()->rcmap[s - rcChannelLetters] = c - input; } } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index e61a95ae33..ce5ff4b586 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -358,7 +358,7 @@ void performAcclerationCalibration(void) sensorCalibrationSolveForOffset(&calState, accTmp); for (int axis = 0; axis < 3; axis++) { - accelerometerConfig()->accZero.raw[axis] = lrintf(accTmp[axis]); + accelerometerConfigMutable()->accZero.raw[axis] = lrintf(accTmp[axis]); } /* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */ @@ -375,7 +375,7 @@ void performAcclerationCalibration(void) sensorCalibrationSolveForScale(&calState, accTmp); for (int axis = 0; axis < 3; axis++) { - accelerometerConfig()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096); + accelerometerConfigMutable()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096); } saveConfigAndNotify(); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index a1c8a007ba..9511eadbe9 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -63,8 +63,8 @@ void updateBoardAlignment(int16_t roll, int16_t pitch) const float sinAlignYaw = sin_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees)); const float cosAlignYaw = cos_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees)); - boardAlignment()->rollDeciDegrees += -sinAlignYaw * pitch + cosAlignYaw * roll; - boardAlignment()->pitchDeciDegrees += cosAlignYaw * pitch + sinAlignYaw * roll; + boardAlignmentMutable()->rollDeciDegrees += -sinAlignYaw * pitch + cosAlignYaw * roll; + boardAlignmentMutable()->pitchDeciDegrees += cosAlignYaw * pitch + sinAlignYaw * roll; initBoardAlignment(); } diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 1b78ecfeb9..d6b48a01cb 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -294,7 +294,7 @@ void compassUpdate(timeUs_t currentTimeUs) calStartedAt = currentTimeUs; for (int axis = 0; axis < 3; axis++) { - compassConfig()->magZero.raw[axis] = 0; + compassConfigMutable()->magZero.raw[axis] = 0; magPrev[axis] = 0; } @@ -333,7 +333,7 @@ void compassUpdate(timeUs_t currentTimeUs) sensorCalibrationSolveForOffset(&calState, magZerof); for (int axis = 0; axis < 3; axis++) { - compassConfig()->magZero.raw[axis] = lrintf(magZerof[axis]); + compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]); } calStartedAt = 0; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bef7031271..b46fb63f5f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -78,25 +78,25 @@ bool sensorsAutodetect(void) #endif if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) { - accelerometerConfig()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC]; + accelerometerConfigMutable()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC]; eepromUpdatePending = true; } #ifdef BARO if (barometerConfig()->baro_hardware == BARO_AUTODETECT) { - barometerConfig()->baro_hardware = detectedSensors[SENSOR_INDEX_BARO]; + barometerConfigMutable()->baro_hardware = detectedSensors[SENSOR_INDEX_BARO]; eepromUpdatePending = true; } #endif if (compassConfig()->mag_hardware == MAG_AUTODETECT) { - compassConfig()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG]; + compassConfigMutable()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG]; eepromUpdatePending = true; } #ifdef PITOT if (pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) { - pitotmeterConfig()->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT]; + pitotmeterConfigMutable()->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT]; eepromUpdatePending = true; } #endif diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index d4369dc38c..1ca62b78c4 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -34,7 +34,7 @@ void targetConfiguration(master_t *config) { (void)config; if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { - gyroConfig()->gyro_align = CW180_DEG; - accelerometerConfig()->acc_align = CW180_DEG; + gyroConfigMutable()->gyro_align = CW180_DEG; + accelerometerConfigMutable()->acc_align = CW180_DEG; } } diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 52ef5fb7ae..4d90ffe4a8 100755 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -72,10 +72,10 @@ #include "config/config_master.h" -void targetConfiguration(struct master_s *config) +void targetConfiguration(master_t *config) { config->mixerConfig.mixerMode = MIXER_HEX6X; - rxConfig()->serialrx_provider = 2; + rxConfigMutable()->serialrx_provider = 2; featureSet(FEATURE_RX_SERIAL); config->serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index 1b59195dd9..3e8fae4e0f 100755 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -72,18 +72,18 @@ #include "config/config_master.h" -void targetConfiguration(struct master_s *config) +void targetConfiguration(master_t *config) { - gyroConfig()->looptime = 1000; + gyroConfigMutable()->looptime = 1000; - rxConfig()->rcmap[0] = 1; - rxConfig()->rcmap[1] = 2; - rxConfig()->rcmap[2] = 3; - rxConfig()->rcmap[3] = 0; - rxConfig()->rcmap[4] = 4; - rxConfig()->rcmap[5] = 5; - rxConfig()->rcmap[6] = 6; - rxConfig()->rcmap[7] = 7; + rxConfigMutable()->rcmap[0] = 1; + rxConfigMutable()->rcmap[1] = 2; + rxConfigMutable()->rcmap[2] = 3; + rxConfigMutable()->rcmap[3] = 0; + rxConfigMutable()->rcmap[4] = 4; + rxConfigMutable()->rcmap[5] = 5; + rxConfigMutable()->rcmap[6] = 6; + rxConfigMutable()->rcmap[7] = 7; featureSet(FEATURE_VBAT); featureSet(FEATURE_LED_STRIP);