diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1f6f04e657..c6d4c85282 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++) { @@ -904,7 +904,7 @@ bool inMotorTestMode(void) { 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); @@ -1278,9 +1278,9 @@ static bool blackboxWriteSysinfo() 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_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..31a11204e6 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -61,6 +61,8 @@ #include "sensors/battery.h" #include "sensors/compass.h" +#define motorConfig(x) (&masterConfig.motorConfig) + // 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..21cf4a5394 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; } @@ -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))) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e6f999722c..5ec6820307 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); @@ -1088,9 +1088,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, masterConfig.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 : @@ -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); @@ -1435,13 +1435,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_ADVANCED_CONFIG: masterConfig.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: diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ed54a283df..31be23a415 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -699,22 +699,22 @@ 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 } }, - { "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 } }, @@ -3790,7 +3790,7 @@ 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 }, #endif diff --git a/src/main/main.c b/src/main/main.c index 48849f012d..8c694b812c 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; } - 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); } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 5774973293..d22d52a0d0 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); @@ -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();