diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1f6f04e657..810fd00c4b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -495,7 +495,7 @@ static void writeIntraframe(void) * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. * Throttle lies in range [minthrottle..maxthrottle]: */ - blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -539,7 +539,7 @@ static void writeIntraframe(void) blackboxWriteSigned16VBArray(blackboxCurrent->debug, 4); //Motors can be below minthrottle when disarmed, but that doesn't happen much - blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.motorConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle); //Motors tend to be similar to each other so use the first motor's value as a predictor of the others for (x = 1; x < motorCount; x++) { @@ -898,13 +898,13 @@ bool inMotorTestMode(void) { static uint32_t resetTime = 0; uint16_t inactiveMotorCommand; if (feature(FEATURE_3D)) { - inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d; + inactiveMotorCommand = flight3DConfig()->neutral3d; #ifdef USE_DSHOT } else if (isMotorProtocolDshot()) { inactiveMotorCommand = DSHOT_DISARM_COMMAND; #endif } else { - inactiveMotorCommand = masterConfig.motorConfig.mincommand; + inactiveMotorCommand = motorConfig()->mincommand; } int i; @@ -1173,8 +1173,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom); - BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle); - BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle); + BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G); @@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo() ); BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); - BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom); + BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom); BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); @@ -1262,25 +1262,25 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); - BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyroConfig.gyro_lpf); - BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type); - BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1, - masterConfig.gyroConfig.gyro_soft_notch_hz_2); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyroConfig.gyro_soft_notch_cutoff_2); - BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.accelerometerConfig.acc_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware); - BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware); - BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.sensorSelectionConfig.mag_hardware); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", gyroConfig()->gyro_lpf); + BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", gyroConfig()->gyro_soft_lpf_type); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", gyroConfig()->gyro_soft_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", gyroConfig()->gyro_soft_notch_hz_1, + gyroConfig()->gyro_soft_notch_hz_2); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, + gyroConfig()->gyro_soft_notch_cutoff_2); + BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", sensorSelectionConfig()->acc_hardware); + BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", sensorSelectionConfig()->baro_hardware); + BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", sensorSelectionConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.armingConfig.gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", masterConfig.rxConfig.rcInterpolation); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", masterConfig.rxConfig.rcInterpolationInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", masterConfig.rxConfig.serialrx_provider); - BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.motorConfig.useUnsyncedPwm); - BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", masterConfig.motorConfig.motorPwmProtocol); - BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", masterConfig.motorConfig.motorPwmRate); + BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->useUnsyncedPwm); + BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->motorPwmProtocol); + BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->motorPwmRate); BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 57ecc67ba9..1d6ae0ca0e 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, - { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyroConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, - { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, - { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, - { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, - { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig()->gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, + { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, + { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, + { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 684fbf8b62..4d03a7b303 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -83,7 +83,7 @@ static OSD_Entry menuMiscEntries[]= { { "-- MISC --", OME_Label, NULL, NULL, 0 }, - { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 }, + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 }, { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4336ee4767..bc4211ab81 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -61,6 +61,21 @@ #include "sensors/battery.h" #include "sensors/compass.h" +#define motorConfig(x) (&masterConfig.motorConfig) +#define flight3DConfig(x) (&masterConfig.flight3DConfig) +#define servoConfig(x) (&masterConfig.servoConfig) +#define servoMixerConfig(x) (&masterConfig.servoMixerConfig) +#define gimbalConfig(x) (&masterConfig.gimbalConfig) +#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig) +#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig) +#define sensorTrims(x) (&masterConfig.sensorTrims) +#define boardAlignment(x) (&masterConfig.boardAlignment) +#define imuConfig(x) (&masterConfig.imuConfig) +#define gyroConfig(x) (&masterConfig.gyroConfig) +#define compassConfig(x) (&masterConfig.compassConfig) +#define accelerometerConfig(x) (&masterConfig.accelerometerConfig) +#define barometerConfig(x) (&masterConfig.barometerConfig) + // System-wide typedef struct master_s { uint8_t version; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 9e93c15ec1..63153f938e 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -528,7 +528,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) uint16_t getCurrentMinthrottle(void) { - return masterConfig.motorConfig.minthrottle; + return motorConfig()->minthrottle; } @@ -842,8 +842,8 @@ void activateConfig(void) #endif useFailsafeConfig(&masterConfig.failsafeConfig); - setAccelerationTrims(&masterConfig.sensorTrims.accZero); - setAccelerationFilter(masterConfig.accelerometerConfig.acc_lpf_hz); + setAccelerationTrims(&sensorTrims()->accZero); + setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); mixerUseConfigs( &masterConfig.flight3DConfig, @@ -878,8 +878,8 @@ void activateConfig(void) void validateAndFixConfig(void) { - if((masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) && (masterConfig.motorConfig.mincommand < 1000)){ - masterConfig.motorConfig.mincommand = 1000; + if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ + motorConfig()->mincommand = 1000; } if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { @@ -991,16 +991,16 @@ void validateAndFixConfig(void) void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff - if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) { - masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0; + if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { + gyroConfig()->gyro_soft_notch_hz_1 = 0; } - if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) { - masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0; + if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { + gyroConfig()->gyro_soft_notch_hz_2 = 0; } - if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) { + if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - masterConfig.gyroConfig.gyro_sync_denom = 1; + gyroConfig()->gyro_sync_denom = 1; } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e6f999722c..dcd8ac0ffc 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -778,9 +778,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_MISC: sbufWriteU16(dst, masterConfig.rxConfig.midrc); - sbufWriteU16(dst, masterConfig.motorConfig.minthrottle); - sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle); - sbufWriteU16(dst, masterConfig.motorConfig.mincommand); + sbufWriteU16(dst, motorConfig()->minthrottle); + sbufWriteU16(dst, motorConfig()->maxthrottle); + sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle); @@ -797,7 +797,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel); sbufWriteU8(dst, 0); - sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10); + sbufWriteU16(dst, compassConfig()->mag_declination / 10); sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale); sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage); @@ -866,9 +866,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_BOARD_ALIGNMENT: - sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees); - sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees); - sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees); + sbufWriteU16(dst, boardAlignment()->rollDegrees); + sbufWriteU16(dst, boardAlignment()->pitchDegrees); + sbufWriteU16(dst, boardAlignment()->yawDegrees); break; case MSP_VOLTAGE_METER_CONFIG: @@ -938,9 +938,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider); - sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees); - sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees); - sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees); + sbufWriteU16(dst, boardAlignment()->rollDegrees); + sbufWriteU16(dst, boardAlignment()->pitchDegrees); + sbufWriteU16(dst, boardAlignment()->yawDegrees); sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale); sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset); @@ -1062,47 +1062,47 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_3D: - sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low); - sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high); - sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d); + sbufWriteU16(dst, flight3DConfig()->deadband3d_low); + sbufWriteU16(dst, flight3DConfig()->deadband3d_high); + sbufWriteU16(dst, flight3DConfig()->neutral3d); break; case MSP_RC_DEADBAND: sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband); sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband); sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband); - sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle); + sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: - sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.gyro_align); - sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.acc_align); - sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.mag_align); + sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align); + sbufWriteU8(dst, sensorAlignmentConfig()->acc_align); + sbufWriteU8(dst, sensorAlignmentConfig()->mag_align); break; case MSP_ADVANCED_CONFIG: - if (masterConfig.gyroConfig.gyro_lpf) { + if (gyroConfig()->gyro_lpf) { sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000 sbufWriteU8(dst, 1); } else { - sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom); + sbufWriteU8(dst, gyroConfig()->gyro_sync_denom); sbufWriteU8(dst, masterConfig.pid_process_denom); } - sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm); - sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol); - sbufWriteU16(dst, masterConfig.motorConfig.motorPwmRate); + sbufWriteU8(dst, motorConfig()->useUnsyncedPwm); + sbufWriteU8(dst, motorConfig()->motorPwmProtocol); + sbufWriteU16(dst, motorConfig()->motorPwmRate); break; case MSP_FILTER_CONFIG : - sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz); + sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz); sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz); - sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1); - sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1); + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz); sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff); - sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2); - sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2); + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); break; case MSP_PID_ADVANCED: @@ -1121,9 +1121,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SENSOR_CONFIG: - sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware); - sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware); - sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware); + sbufWriteU8(dst, sensorSelectionConfig()->acc_hardware); + sbufWriteU8(dst, sensorSelectionConfig()->baro_hardware); + sbufWriteU8(dst, sensorSelectionConfig()->mag_hardware); break; case MSP_REBOOT: @@ -1336,9 +1336,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (tmp < 1600 && tmp > 1400) masterConfig.rxConfig.midrc = tmp; - masterConfig.motorConfig.minthrottle = sbufReadU16(src); - masterConfig.motorConfig.maxthrottle = sbufReadU16(src); - masterConfig.motorConfig.mincommand = sbufReadU16(src); + motorConfig()->minthrottle = sbufReadU16(src); + motorConfig()->maxthrottle = sbufReadU16(src); + motorConfig()->mincommand = sbufReadU16(src); masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src); @@ -1355,7 +1355,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.rxConfig.rssi_channel = sbufReadU8(src); sbufReadU8(src); - masterConfig.compassConfig.mag_declination = sbufReadU16(src) * 10; + compassConfig()->mag_declination = sbufReadU16(src) * 10; masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI @@ -1410,16 +1410,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_3D: - masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src); - masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src); - masterConfig.flight3DConfig.neutral3d = sbufReadU16(src); + flight3DConfig()->deadband3d_low = sbufReadU16(src); + flight3DConfig()->deadband3d_high = sbufReadU16(src); + flight3DConfig()->neutral3d = sbufReadU16(src); break; case MSP_SET_RC_DEADBAND: masterConfig.rcControlsConfig.deadband = sbufReadU8(src); masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src); masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src); - masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src); + flight3DConfig()->deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RESET_CURR_PID: @@ -1427,36 +1427,36 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_ALIGNMENT: - masterConfig.sensorAlignmentConfig.gyro_align = sbufReadU8(src); - masterConfig.sensorAlignmentConfig.acc_align = sbufReadU8(src); - masterConfig.sensorAlignmentConfig.mag_align = sbufReadU8(src); + sensorAlignmentConfig()->gyro_align = sbufReadU8(src); + sensorAlignmentConfig()->acc_align = sbufReadU8(src); + sensorAlignmentConfig()->mag_align = sbufReadU8(src); break; case MSP_SET_ADVANCED_CONFIG: - masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src); + gyroConfig()->gyro_sync_denom = sbufReadU8(src); masterConfig.pid_process_denom = sbufReadU8(src); - masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src); + motorConfig()->useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT - masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); + motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); #else - masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); + motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif - masterConfig.motorConfig.motorPwmRate = sbufReadU16(src); + motorConfig()->motorPwmRate = sbufReadU16(src); break; case MSP_SET_FILTER_CONFIG: - masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src); + gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src); currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { - masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src); - masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src); + gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src); + gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src); } if (dataSize > 13) { - masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src); - masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src); + gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src); + gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); @@ -1481,9 +1481,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src); - masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src); - masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src); + sensorSelectionConfig()->acc_hardware = sbufReadU8(src); + sensorSelectionConfig()->baro_hardware = sbufReadU8(src); + sensorSelectionConfig()->mag_hardware = sbufReadU8(src); break; case MSP_RESET_CONF: @@ -1641,9 +1641,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_BOARD_ALIGNMENT: - masterConfig.boardAlignment.rollDegrees = sbufReadU16(src); - masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src); - masterConfig.boardAlignment.yawDegrees = sbufReadU16(src); + boardAlignment()->rollDegrees = sbufReadU16(src); + boardAlignment()->pitchDegrees = sbufReadU16(src); + boardAlignment()->yawDegrees = sbufReadU16(src); break; case MSP_SET_VOLTAGE_METER_CONFIG: @@ -1729,9 +1729,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); // serialrx_type - masterConfig.boardAlignment.rollDegrees = sbufReadU16(src); // board_align_roll - masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src); // board_align_pitch - masterConfig.boardAlignment.yawDegrees = sbufReadU16(src); // board_align_yaw + boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll + boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch + boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src); masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 860c0038f2..8b8d99eac1 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -124,7 +124,7 @@ static void taskUpdateBattery(uint32_t currentTime) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTime; - updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); } } } @@ -157,7 +157,7 @@ static void taskUpdateRxMain(uint32_t currentTime) static void taskUpdateCompass(uint32_t currentTime) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTime, &masterConfig.sensorTrims.magZero); + compassUpdate(currentTime, &sensorTrims()->magZero); } } #endif @@ -197,7 +197,7 @@ static void taskTelemetry(uint32_t currentTime) telemetryCheckState(); if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + telemetryProcess(currentTime, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); } } #endif diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 987e89aba7..cff40a047b 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -515,7 +515,7 @@ void processRx(uint32_t currentTime) failsafeUpdateState(); } - throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset @@ -733,7 +733,7 @@ void subTaskMainSubprocesses(void) if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS - && !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) + && !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo) #endif && masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING @@ -801,7 +801,7 @@ void subTaskMotorUpdate(void) uint8_t setPidUpdateCountDown(void) { - if (masterConfig.gyroConfig.gyro_soft_lpf_hz) { + if (gyroConfig()->gyro_soft_lpf_hz) { return masterConfig.pid_process_denom - 1; } else { return 1; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ed54a283df..79ab7a59cb 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -699,27 +699,27 @@ const clivalue_t valueTable[] = { { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } }, { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, - { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "min_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "max_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "min_command", VAR_UINT16 | MASTER_VALUE, &motorConfig()->mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, #ifdef USE_DSHOT - { "digital_idle_percent", VAR_FLOAT | MASTER_VALUE, &masterConfig.motorConfig.digitalIdleOffsetPercent, .config.minmax = { 0, 20} }, + { "digital_idle_percent", VAR_FLOAT | MASTER_VALUE, &motorConfig()->digitalIdleOffsetPercent, .config.minmax = { 0, 20} }, #endif - { "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } }, + { "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } }, - { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently - { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently, - { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently + { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently, + { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, - { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, - { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 200, 32000 } }, + { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, + { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, + { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.armingConfig.auto_disarm_delay, .config.minmax = { 0, 60 } }, - { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.small_angle, .config.minmax = { 0, 180 } }, + { "small_angle", VAR_UINT8 | MASTER_VALUE, &imuConfig()->small_angle, .config.minmax = { 0, 180 } }, { "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, .config.minmax = { -1, 1 } }, @@ -802,26 +802,26 @@ const clivalue_t valueTable[] = { { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.consumptionWarningPercentage, .config.minmax = { 0, 100 } }, - { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, - { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } }, - { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } }, + { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, + { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, + { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, - { "align_board_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.rollDegrees, .config.minmax = { -180, 360 } }, - { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.pitchDegrees, .config.minmax = { -180, 360 } }, - { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } }, + { "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDegrees, .config.minmax = { -180, 360 } }, + { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } }, + { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDegrees, .config.minmax = { -180, 360 } }, { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, - { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, - { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, - { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, - { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, - { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, - { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, - { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, - { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, - { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_kp, .config.minmax = { 0, 50000 } }, - { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_ki, .config.minmax = { 0, 50000 } }, + { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, + { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 8 } }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, + { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, + { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, + { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, + { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, + { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp, .config.minmax = { 0, 50000 } }, + { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki, .config.minmax = { 0, 50000 } }, { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.alt_hold_deadband, .config.minmax = { 1, 250 } }, { "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rcControlsConfig.alt_hold_fast_change, .config.lookup = { TABLE_OFF_ON } }, @@ -837,12 +837,12 @@ const clivalue_t valueTable[] = { { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, #ifdef USE_SERVOS - { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, - { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoMixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, - { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, - { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoPwmRate, .config.minmax = { 50, 498 } }, - { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } }, + { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, + { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} }, + { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, + { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } }, + { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } }, #endif { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, @@ -868,25 +868,25 @@ const clivalue_t valueTable[] = { { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, - { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, - { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.accelerometerConfig.acc_lpf_hz, .config.minmax = { 0, 400 } }, - { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.xy, .config.minmax = { 0, 100 } }, - { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.z, .config.minmax = { 0, 100 } }, - { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.imuConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, + { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, + { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &accelerometerConfig()->acc_lpf_hz, .config.minmax = { 0, 400 } }, + { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.xy, .config.minmax = { 0, 100 } }, + { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.z, .config.minmax = { 0, 100 } }, + { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &imuConfig()->acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, #ifdef BARO - { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, &masterConfig.barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, - { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } }, - { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } }, - { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } }, - { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, + { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, &barometerConfig()->baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, + { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_noise_lpf, .config.minmax = { 0 , 1 } }, + { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_vel, .config.minmax = { 0 , 1 } }, + { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_alt, .config.minmax = { 0 , 1 } }, + { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, #endif #ifdef MAG - { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, - { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.compassConfig.mag_declination, .config.minmax = { -18000, 18000 } }, + { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, + { "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } }, #endif { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, @@ -944,9 +944,9 @@ const clivalue_t valueTable[] = { #endif #ifdef MAG - { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[X], .config.minmax = { -32768, 32767 } }, - { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, - { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, + { "magzero_x", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[X], .config.minmax = { -32768, 32767 } }, + { "magzero_y", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Y], .config.minmax = { -32768, 32767 } }, + { "magzero_z", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Z], .config.minmax = { -32768, 32767 } }, #endif #ifdef LED_STRIP { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, @@ -3790,9 +3790,9 @@ const cliResourceValue_t resourceTable[] = { #ifdef BEEPER { OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 }, #endif - { OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS }, + { OWNER_MOTOR, &motorConfig()->ioTags[0], MAX_SUPPORTED_MOTORS }, #ifdef USE_SERVOS - { OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS }, + { OWNER_SERVO, &servoConfig()->ioTags[0], MAX_SUPPORTED_SERVOS }, #endif #if defined(USE_PWM) || defined(USE_PPM) { OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 }, diff --git a/src/main/main.c b/src/main/main.c index 48849f012d..9d0a40e2c2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -254,12 +254,12 @@ void init(void) servoMixerInit(masterConfig.customServoMixer); #endif - uint16_t idlePulse = masterConfig.motorConfig.mincommand; + uint16_t idlePulse = motorConfig()->mincommand; if (feature(FEATURE_3D)) { - idlePulse = masterConfig.flight3DConfig.neutral3d; + idlePulse = flight3DConfig()->neutral3d; } - if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { + if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); idlePulse = 0; // brushed motors } @@ -284,7 +284,7 @@ void init(void) #if defined(USE_PWM) || defined(USE_PPM) if (feature(FEATURE_RX_PPM)) { - ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol); + ppmRxInit(&masterConfig.ppmConfig, motorConfig()->motorPwmProtocol); } else if (feature(FEATURE_RX_PARALLEL_PWM)) { pwmRxInit(&masterConfig.pwmConfig); } @@ -424,7 +424,7 @@ void init(void) #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, &masterConfig.sensorSelectionConfig, - masterConfig.compassConfig.mag_declination, + compassConfig()->mag_declination, &masterConfig.gyroConfig, sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. @@ -465,7 +465,7 @@ void init(void) cliInit(&masterConfig.serialConfig); #endif - failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + failsafeInit(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 5774973293..6d87ea0103 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -750,9 +750,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) case BST_MISC: bstWrite16(masterConfig.rxConfig.midrc); - bstWrite16(masterConfig.motorConfig.minthrottle); - bstWrite16(masterConfig.motorConfig.maxthrottle); - bstWrite16(masterConfig.motorConfig.mincommand); + bstWrite16(motorConfig()->minthrottle); + bstWrite16(motorConfig()->maxthrottle); + bstWrite16(motorConfig()->mincommand); bstWrite16(masterConfig.failsafeConfig.failsafe_throttle); @@ -769,7 +769,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(masterConfig.rxConfig.rssi_channel); bstWrite8(0); - bstWrite16(masterConfig.compassConfig.mag_declination / 10); + bstWrite16(compassConfig()->mag_declination / 10); bstWrite8(masterConfig.batteryConfig.vbatscale); bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); @@ -848,9 +848,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_BOARD_ALIGNMENT: - bstWrite16(masterConfig.boardAlignment.rollDegrees); - bstWrite16(masterConfig.boardAlignment.pitchDegrees); - bstWrite16(masterConfig.boardAlignment.yawDegrees); + bstWrite16(boardAlignment()->rollDegrees); + bstWrite16(boardAlignment()->pitchDegrees); + bstWrite16(boardAlignment()->yawDegrees); break; case BST_VOLTAGE_METER_CONFIG: @@ -910,9 +910,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(masterConfig.rxConfig.serialrx_provider); - bstWrite16(masterConfig.boardAlignment.rollDegrees); - bstWrite16(masterConfig.boardAlignment.pitchDegrees); - bstWrite16(masterConfig.boardAlignment.yawDegrees); + bstWrite16(boardAlignment()->rollDegrees); + bstWrite16(boardAlignment()->pitchDegrees); + bstWrite16(boardAlignment()->yawDegrees); bstWrite16(masterConfig.batteryConfig.currentMeterScale); bstWrite16(masterConfig.batteryConfig.currentMeterOffset); @@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(masterConfig.rcControlsConfig.yaw_deadband); break; case BST_FC_FILTERS: - bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values + bstWrite16(constrain(gyroConfig()->gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values break; default: // we do not know how to handle the (valid) message, indicate error BST @@ -1113,9 +1113,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (tmp < 1600 && tmp > 1400) masterConfig.rxConfig.midrc = tmp; - masterConfig.motorConfig.minthrottle = bstRead16(); - masterConfig.motorConfig.maxthrottle = bstRead16(); - masterConfig.motorConfig.mincommand = bstRead16(); + motorConfig()->minthrottle = bstRead16(); + motorConfig()->maxthrottle = bstRead16(); + motorConfig()->mincommand = bstRead16(); masterConfig.failsafeConfig.failsafe_throttle = bstRead16(); @@ -1132,7 +1132,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.rxConfig.rssi_channel = bstRead8(); bstRead8(); - masterConfig.compassConfig.mag_declination = bstRead16() * 10; + compassConfig()->mag_declination = bstRead16() * 10; masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI @@ -1254,9 +1254,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) featureSet(bstRead32()); // features bitmap break; case BST_SET_BOARD_ALIGNMENT: - masterConfig.boardAlignment.rollDegrees = bstRead16(); - masterConfig.boardAlignment.pitchDegrees = bstRead16(); - masterConfig.boardAlignment.yawDegrees = bstRead16(); + boardAlignment()->rollDegrees = bstRead16(); + boardAlignment()->pitchDegrees = bstRead16(); + boardAlignment()->yawDegrees = bstRead16(); break; case BST_SET_VOLTAGE_METER_CONFIG: masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended @@ -1327,9 +1327,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.rxConfig.serialrx_provider = bstRead8(); // serialrx_type - masterConfig.boardAlignment.rollDegrees = bstRead16(); // board_align_roll - masterConfig.boardAlignment.pitchDegrees = bstRead16(); // board_align_pitch - masterConfig.boardAlignment.yawDegrees = bstRead16(); // board_align_yaw + boardAlignment()->rollDegrees = bstRead16(); // board_align_roll + boardAlignment()->pitchDegrees = bstRead16(); // board_align_pitch + boardAlignment()->yawDegrees = bstRead16(); // board_align_yaw masterConfig.batteryConfig.currentMeterScale = bstRead16(); masterConfig.batteryConfig.currentMeterOffset = bstRead16(); @@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.rcControlsConfig.yaw_deadband = bstRead8(); break; case BST_SET_FC_FILTERS: - masterConfig.gyroConfig.gyro_lpf = bstRead16(); + gyroConfig()->gyro_lpf = bstRead16(); break; default: