diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 810fd00c4b..7acdc0045b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -394,7 +394,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) return feature(FEATURE_VBAT); case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC: - return feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; + return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC; case FLIGHT_LOG_FIELD_CONDITION_SONAR: #ifdef SONAR @@ -1180,21 +1180,21 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { - blackboxPrintfHeaderLine("vbatscale:%u", masterConfig.batteryConfig.vbatscale); + blackboxPrintfHeaderLine("vbatscale:%u", batteryConfig()->vbatscale); } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } ); - BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage, - masterConfig.batteryConfig.vbatwarningcellvoltage, - masterConfig.batteryConfig.vbatmaxcellvoltage); + BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", batteryConfig()->vbatmincellvoltage, + batteryConfig()->vbatwarningcellvoltage, + batteryConfig()->vbatmaxcellvoltage); BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too: if (feature(FEATURE_CURRENT_METER)) { - blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); + blackboxPrintfHeaderLine("currentMeter:%d,%d", batteryConfig()->currentMeterOffset, batteryConfig()->currentMeterScale); } ); @@ -1260,8 +1260,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit); // End of Betaflight controller parameters - BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); - BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); + BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband); + BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", rcControlsConfig()->yaw_deadband); 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); diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 4d03a7b303..38d1e07160 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -84,8 +84,8 @@ static OSD_Entry menuMiscEntries[]= { "-- MISC --", OME_Label, NULL, NULL, 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 }, + { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 }, + { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 }, { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, { "BACK", OME_Back, NULL, NULL, 0}, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index bc4211ab81..1e5fe67f11 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -75,6 +75,11 @@ #define compassConfig(x) (&masterConfig.compassConfig) #define accelerometerConfig(x) (&masterConfig.accelerometerConfig) #define barometerConfig(x) (&masterConfig.barometerConfig) +#define throttleCorrectionConfig(x) (&masterConfig.throttleCorrectionConfig) +#define batteryConfig(x) (&masterConfig.batteryConfig) +#define rcControlsConfig(x) (&masterConfig.rcControlsConfig) +#define gpsProfile(x) (&masterConfig.gpsProfile) +#define gpsConfig(x) (&masterConfig.gpsConfig) // System-wide typedef struct master_s { diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 63153f938e..aa2553296d 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -861,7 +861,7 @@ void activateConfig(void) imuConfigure( &masterConfig.imuConfig, ¤tProfile->pidProfile, - masterConfig.throttleCorrectionConfig.throttle_correction_angle + throttleCorrectionConfig()->throttle_correction_angle ); configureAltitudeHold( @@ -908,7 +908,7 @@ void validateAndFixConfig(void) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); // current meter needs the same ports - if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } #endif @@ -930,7 +930,7 @@ void validateAndFixConfig(void) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); // current meter needs the same ports - if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } #endif @@ -938,13 +938,13 @@ void validateAndFixConfig(void) #endif #if defined(NAZE) && defined(SONAR) - if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } #endif #if defined(OLIMEXINO) && defined(SONAR) - if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } #endif diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5f9ca85178..2e8c012ce4 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -693,7 +693,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255)); sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery sbufWriteU16(dst, rssi); - if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { + if(batteryConfig()->multiwiiCurrentMeterOutput) { sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero } else sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A @@ -785,24 +785,24 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle); #ifdef GPS - sbufWriteU8(dst, masterConfig.gpsConfig.provider); // gps_type + sbufWriteU8(dst, gpsConfig()->provider); // gps_type sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - sbufWriteU8(dst, masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas + sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas #else sbufWriteU8(dst, 0); // gps_type sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t sbufWriteU8(dst, 0); // gps_ubx_sbas #endif - sbufWriteU8(dst, masterConfig.batteryConfig.multiwiiCurrentMeterOutput); + sbufWriteU8(dst, batteryConfig()->multiwiiCurrentMeterOutput); sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel); sbufWriteU8(dst, 0); sbufWriteU16(dst, compassConfig()->mag_declination / 10); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatscale); + sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); break; case MSP_MOTOR_PINS: @@ -872,17 +872,17 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_VOLTAGE_METER_CONFIG: - sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage); - sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatscale); + sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); break; case MSP_CURRENT_METER_CONFIG: - sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale); - sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset); - sbufWriteU8(dst, masterConfig.batteryConfig.currentMeterType); - sbufWriteU16(dst, masterConfig.batteryConfig.batteryCapacity); + sbufWriteU16(dst, batteryConfig()->currentMeterScale); + sbufWriteU16(dst, batteryConfig()->currentMeterOffset); + sbufWriteU8(dst, batteryConfig()->currentMeterType); + sbufWriteU16(dst, batteryConfig()->batteryCapacity); break; case MSP_MIXER: @@ -942,8 +942,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, boardAlignment()->pitchDegrees); sbufWriteU16(dst, boardAlignment()->yawDegrees); - sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale); - sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset); + sbufWriteU16(dst, batteryConfig()->currentMeterScale); + sbufWriteU16(dst, batteryConfig()->currentMeterOffset); break; case MSP_CF_SERIAL_CONFIG: @@ -1068,9 +1068,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_RC_DEADBAND: - sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband); - sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband); - sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband); + sbufWriteU8(dst, rcControlsConfig()->deadband); + sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); + sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); break; @@ -1344,24 +1344,24 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src); #ifdef GPS - masterConfig.gpsConfig.provider = sbufReadU8(src); // gps_type + gpsConfig()->provider = sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate - masterConfig.gpsConfig.sbasMode = sbufReadU8(src); // gps_ubx_sbas + gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas #else sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate sbufReadU8(src); // gps_ubx_sbas #endif - masterConfig.batteryConfig.multiwiiCurrentMeterOutput = sbufReadU8(src); + batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src); masterConfig.rxConfig.rssi_channel = sbufReadU8(src); sbufReadU8(src); 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 - masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + 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 break; case MSP_SET_MOTOR: @@ -1417,9 +1417,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *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); + rcControlsConfig()->deadband = sbufReadU8(src); + rcControlsConfig()->yaw_deadband = sbufReadU8(src); + rcControlsConfig()->alt_hold_deadband = sbufReadU8(src); flight3DConfig()->deadband3d_throttle = sbufReadU16(src); break; @@ -1649,17 +1649,17 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_VOLTAGE_METER_CONFIG: - masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + 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 break; case MSP_SET_CURRENT_METER_CONFIG: - masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src); - masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src); - masterConfig.batteryConfig.currentMeterType = sbufReadU8(src); - masterConfig.batteryConfig.batteryCapacity = sbufReadU16(src); + batteryConfig()->currentMeterScale = sbufReadU16(src); + batteryConfig()->currentMeterOffset = sbufReadU16(src); + batteryConfig()->currentMeterType = sbufReadU8(src); + batteryConfig()->batteryCapacity = sbufReadU16(src); break; #ifndef USE_QUAD_MIXER_ONLY @@ -1735,8 +1735,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw - masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src); - masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src); + batteryConfig()->currentMeterScale = sbufReadU16(src); + batteryConfig()->currentMeterOffset = sbufReadU16(src); break; case MSP_SET_CF_SERIAL_CONFIG: diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index cff40a047b..7f886228b6 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -284,19 +284,19 @@ void updateRcCommands(void) int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { - if (tmp > masterConfig.rcControlsConfig.deadband) { - tmp -= masterConfig.rcControlsConfig.deadband; + if (tmp > rcControlsConfig()->deadband) { + tmp -= rcControlsConfig()->deadband; } else { tmp = 0; } rcCommand[axis] = tmp; } else if (axis == YAW) { - if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { - tmp -= masterConfig.rcControlsConfig.yaw_deadband; + if (tmp > rcControlsConfig()->yaw_deadband) { + tmp -= rcControlsConfig()->yaw_deadband; } else { tmp = 0; } - rcCommand[axis] = tmp * -masterConfig.rcControlsConfig.yaw_control_direction; + rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; @@ -484,7 +484,7 @@ void updateMagHold(void) dif += 360; if (dif >= +180) dif -= 360; - dif *= -masterConfig.rcControlsConfig.yaw_control_direction; + dif *= -rcControlsConfig()->yaw_control_direction; if (STATE(SMALL_ANGLE)) rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg } else @@ -743,8 +743,8 @@ void subTaskMainSubprocesses(void) setpointRate[YAW] = 0; } - if (masterConfig.throttleCorrectionConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttleCorrectionConfig.throttle_correction_value); + if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } processRcCommand(); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 79ab7a59cb..f004ad0ed0 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -726,10 +726,10 @@ const clivalue_t valueTable[] = { { "reboot_character", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.reboot_character, .config.minmax = { 48, 126 } }, #ifdef GPS - { "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsConfig.provider, .config.lookup = { TABLE_GPS_PROVIDER } }, - { "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsConfig.sbasMode, .config.lookup = { TABLE_GPS_SBAS_MODE } }, - { "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsConfig.autoConfig, .config.lookup = { TABLE_OFF_ON } }, - { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsConfig.autoBaud, .config.lookup = { TABLE_OFF_ON } }, + { "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->provider, .config.lookup = { TABLE_GPS_PROVIDER } }, + { "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->sbasMode, .config.lookup = { TABLE_GPS_SBAS_MODE } }, + { "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->autoConfig, .config.lookup = { TABLE_OFF_ON } }, + { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->autoBaud, .config.lookup = { TABLE_OFF_ON } }, { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], .config.minmax = { 0, 200 } }, { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDPOS], .config.minmax = { 0, 200 } }, @@ -740,11 +740,11 @@ const clivalue_t valueTable[] = { { "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDNAVR], .config.minmax = { 0, 200 } }, - { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, &masterConfig.gpsProfile.gps_wp_radius, .config.minmax = { 0, 2000 } }, - { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsProfile.nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, - { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, &masterConfig.gpsProfile.nav_speed_min, .config.minmax = { 10, 2000 } }, - { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, &masterConfig.gpsProfile.nav_speed_max, .config.minmax = { 10, 2000 } }, - { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsProfile.nav_slew_rate, .config.minmax = { 0, 100 } }, + { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->gps_wp_radius, .config.minmax = { 0, 2000 } }, + { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsProfile()->nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, + { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->nav_speed_min, .config.minmax = { 10, 2000 } }, + { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->nav_speed_max, .config.minmax = { 10, 2000 } }, + { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &gpsProfile()->nav_slew_rate, .config.minmax = { 0, 100 } }, #endif #ifdef GTUNE @@ -788,20 +788,20 @@ const clivalue_t valueTable[] = { { "pid_values_as_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, #endif - { "battery_capacity", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.batteryCapacity, .config.minmax = { 0, 20000 } }, - { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } }, - { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, .config.minmax = { 10, 50 } }, - { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } }, - { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, - { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } }, - { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -16000, 16000 } }, - { "current_meter_offset", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { -16000, 16000 } }, - { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, - { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, - { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } }, - { "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 } }, + { "battery_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, + { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } }, + { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmaxcellvoltage, .config.minmax = { 10, 50 } }, + { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmincellvoltage, .config.minmax = { 10, 50 } }, + { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, + { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbathysteresis, .config.minmax = { 0, 250 } }, + { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } }, + { "current_meter_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, + { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, + { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, + { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, + { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "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 } }, @@ -823,15 +823,15 @@ const clivalue_t valueTable[] = { { "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 } }, - { "deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.deadband, .config.minmax = { 0, 32 } }, - { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.yaw_deadband, .config.minmax = { 0, 100 } }, + { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->alt_hold_deadband, .config.minmax = { 1, 250 } }, + { "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rcControlsConfig()->alt_hold_fast_change, .config.lookup = { TABLE_OFF_ON } }, + { "deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->deadband, .config.minmax = { 0, 32 } }, + { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->yaw_deadband, .config.minmax = { 0, 100 } }, - { "throttle_correction_value", VAR_UINT8 | MASTER_VALUE, &masterConfig.throttleCorrectionConfig.throttle_correction_value, .config.minmax = { 0, 150 } }, - { "throttle_correction_angle", VAR_UINT16 | MASTER_VALUE, &masterConfig.throttleCorrectionConfig.throttle_correction_angle, .config.minmax = { 1, 900 } }, + { "throttle_correction_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } }, + { "throttle_correction_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } }, - { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.yaw_control_direction, .config.minmax = { -1, 1 } }, + { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } }, { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index a2f3fd696e..96fade97fa 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -677,7 +677,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8((uint8_t)constrain(vbat, 0, 255)); bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery bstWrite16(rssi); - if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { + if(batteryConfig()->multiwiiCurrentMeterOutput) { bstWrite16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero } else bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A @@ -758,24 +758,24 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(masterConfig.failsafeConfig.failsafe_throttle); #ifdef GPS - bstWrite8(masterConfig.gpsConfig.provider); // gps_type + bstWrite8(gpsConfig()->provider); // gps_type bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - bstWrite8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas + bstWrite8(gpsConfig()->sbasMode); // gps_ubx_sbas #else bstWrite8(0); // gps_type bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t bstWrite8(0); // gps_ubx_sbas #endif - bstWrite8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput); + bstWrite8(batteryConfig()->multiwiiCurrentMeterOutput); bstWrite8(masterConfig.rxConfig.rssi_channel); bstWrite8(0); bstWrite16(compassConfig()->mag_declination / 10); - bstWrite8(masterConfig.batteryConfig.vbatscale); - bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatmaxcellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatwarningcellvoltage); + bstWrite8(batteryConfig()->vbatscale); + bstWrite8(batteryConfig()->vbatmincellvoltage); + bstWrite8(batteryConfig()->vbatmaxcellvoltage); + bstWrite8(batteryConfig()->vbatwarningcellvoltage); break; case BST_MOTOR_PINS: // FIXME This is hardcoded and should not be. @@ -855,17 +855,17 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_VOLTAGE_METER_CONFIG: - bstWrite8(masterConfig.batteryConfig.vbatscale); - bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatmaxcellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatwarningcellvoltage); + bstWrite8(batteryConfig()->vbatscale); + bstWrite8(batteryConfig()->vbatmincellvoltage); + bstWrite8(batteryConfig()->vbatmaxcellvoltage); + bstWrite8(batteryConfig()->vbatwarningcellvoltage); break; case BST_CURRENT_METER_CONFIG: - bstWrite16(masterConfig.batteryConfig.currentMeterScale); - bstWrite16(masterConfig.batteryConfig.currentMeterOffset); - bstWrite8(masterConfig.batteryConfig.currentMeterType); - bstWrite16(masterConfig.batteryConfig.batteryCapacity); + bstWrite16(batteryConfig()->currentMeterScale); + bstWrite16(batteryConfig()->currentMeterOffset); + bstWrite8(batteryConfig()->currentMeterType); + bstWrite16(batteryConfig()->batteryCapacity); break; case BST_MIXER: @@ -915,8 +915,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(boardAlignment()->pitchDegrees); bstWrite16(boardAlignment()->yawDegrees); - bstWrite16(masterConfig.batteryConfig.currentMeterScale); - bstWrite16(masterConfig.batteryConfig.currentMeterOffset); + bstWrite16(batteryConfig()->currentMeterScale); + bstWrite16(batteryConfig()->currentMeterOffset); break; case BST_CF_SERIAL_CONFIG: @@ -972,10 +972,10 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite32(0); // future exp break; case BST_DEADBAND: - bstWrite8(masterConfig.rcControlsConfig.alt_hold_deadband); - bstWrite8(masterConfig.rcControlsConfig.alt_hold_fast_change); - bstWrite8(masterConfig.rcControlsConfig.deadband); - bstWrite8(masterConfig.rcControlsConfig.yaw_deadband); + bstWrite8(rcControlsConfig()->alt_hold_deadband); + bstWrite8(rcControlsConfig()->alt_hold_fast_change); + bstWrite8(rcControlsConfig()->deadband); + bstWrite8(rcControlsConfig()->yaw_deadband); break; case BST_FC_FILTERS: bstWrite16(constrain(gyroConfig()->gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values @@ -1121,24 +1121,24 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.failsafeConfig.failsafe_throttle = bstRead16(); #ifdef GPS - masterConfig.gpsConfig.provider = bstRead8(); // gps_type + gpsConfig()->provider = bstRead8(); // gps_type bstRead8(); // gps_baudrate - masterConfig.gpsConfig.sbasMode = bstRead8(); // gps_ubx_sbas + gpsConfig()->sbasMode = bstRead8(); // gps_ubx_sbas #else bstRead8(); // gps_type bstRead8(); // gps_baudrate bstRead8(); // gps_ubx_sbas #endif - masterConfig.batteryConfig.multiwiiCurrentMeterOutput = bstRead8(); + batteryConfig()->multiwiiCurrentMeterOutput = bstRead8(); masterConfig.rxConfig.rssi_channel = bstRead8(); bstRead8(); compassConfig()->mag_declination = bstRead16() * 10; - masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert + batteryConfig()->vbatscale = bstRead8(); // actual vbatscale as intended + batteryConfig()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfig()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfig()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert break; case BST_SET_MOTOR: for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 @@ -1260,16 +1260,16 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) boardAlignment()->yawDegrees = bstRead16(); break; case BST_SET_VOLTAGE_METER_CONFIG: - masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert + batteryConfig()->vbatscale = bstRead8(); // actual vbatscale as intended + batteryConfig()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfig()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfig()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert break; case BST_SET_CURRENT_METER_CONFIG: - masterConfig.batteryConfig.currentMeterScale = bstRead16(); - masterConfig.batteryConfig.currentMeterOffset = bstRead16(); - masterConfig.batteryConfig.currentMeterType = bstRead8(); - masterConfig.batteryConfig.batteryCapacity = bstRead16(); + batteryConfig()->currentMeterScale = bstRead16(); + batteryConfig()->currentMeterOffset = bstRead16(); + batteryConfig()->currentMeterType = bstRead8(); + batteryConfig()->batteryCapacity = bstRead16(); break; #ifndef USE_QUAD_MIXER_ONLY @@ -1332,8 +1332,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) boardAlignment()->pitchDegrees = bstRead16(); // board_align_pitch boardAlignment()->yawDegrees = bstRead16(); // board_align_yaw - masterConfig.batteryConfig.currentMeterScale = bstRead16(); - masterConfig.batteryConfig.currentMeterOffset = bstRead16(); + batteryConfig()->currentMeterScale = bstRead16(); + batteryConfig()->currentMeterOffset = bstRead16(); break; case BST_SET_CF_SERIAL_CONFIG: { @@ -1399,10 +1399,10 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) DISABLE_ARMING_FLAG(PREVENT_ARMING); break; case BST_SET_DEADBAND: - masterConfig.rcControlsConfig.alt_hold_deadband = bstRead8(); - masterConfig.rcControlsConfig.alt_hold_fast_change = bstRead8(); - masterConfig.rcControlsConfig.deadband = bstRead8(); - masterConfig.rcControlsConfig.yaw_deadband = bstRead8(); + rcControlsConfig()->alt_hold_deadband = bstRead8(); + rcControlsConfig()->alt_hold_fast_change = bstRead8(); + rcControlsConfig()->deadband = bstRead8(); + rcControlsConfig()->yaw_deadband = bstRead8(); break; case BST_SET_FC_FILTERS: gyroConfig()->gyro_lpf = bstRead16(); diff --git a/src/main/telemetry/esc_telemetry.c b/src/main/telemetry/esc_telemetry.c index 9d5cde0f1e..7dd37ac08d 100644 --- a/src/main/telemetry/esc_telemetry.c +++ b/src/main/telemetry/esc_telemetry.c @@ -139,8 +139,8 @@ bool escTelemetryInit(void) if (escTelemetryPort) { escTelemetryEnabled = true; - masterConfig.batteryConfig.currentMeterType = CURRENT_SENSOR_ESC; - masterConfig.batteryConfig.batteryMeterType = BATTERY_SENSOR_ESC; + batteryConfig()->currentMeterType = CURRENT_SENSOR_ESC; + batteryConfig()->batteryMeterType = BATTERY_SENSOR_ESC; } return escTelemetryPort != NULL;